fix(obstacle_cruise_planner): remove velocity factor (#1747)

Signed-off-by: satoshi-ota <[email protected]>
tier4 · Dec 27, 2024 · 6648656 · 6648656
1 parent d138fba
commit 6648656
Show file tree

Hide file tree

Showing 7 changed files with 0 additions and 43 deletions.
diff --git a/...re_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/...re_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp
@@ -57,8 +57,6 @@ class PlannerInterface
     slow_down_param_(SlowDownParam(node)),
     stop_param_(StopParam(node, longitudinal_info))
   {
-    velocity_factors_pub_ =
-      node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_cruise", 1);
     stop_speed_exceeded_pub_ =
       node.create_publisher<StopSpeedExceeded>("~/output/stop_speed_exceeded", 1);
     metrics_pub_ = node.create_publisher<MetricArray>("~/metrics", 10);
@@ -151,7 +149,6 @@ class PlannerInterface
     stop_watch_;
 
   // Publishers
-  rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factors_pub_;
   rclcpp::Publisher<StopSpeedExceeded>::SharedPtr stop_speed_exceeded_pub_;
   rclcpp::Publisher<MetricArray>::SharedPtr metrics_pub_;
 

diff --git a/.../autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/.../autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp
@@ -17,8 +17,6 @@
 
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
-#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp"
-#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp"
 #include "autoware_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_perception_msgs/msg/predicted_objects.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
@@ -39,9 +37,6 @@
 #include <pcl/point_types.h>
 
 using autoware::vehicle_info_utils::VehicleInfo;
-using autoware_adapi_v1_msgs::msg::PlanningBehavior;
-using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;

diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp
@@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset(
   return 0;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE,
-  const std::optional<geometry_msgs::msg::Pose> pose = std::nullopt);
 }  // namespace obstacle_cruise_utils
 
 #endif  // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_
diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml
@@ -18,7 +18,6 @@
   <buildtool_depend>ament_cmake_auto</buildtool_depend>
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
-  <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_interpolation</depend>
   <depend>autoware_lanelet2_extension</depend>
   <depend>autoware_motion_utils</depend>

diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
@@ -339,9 +339,6 @@ std::vector<TrajectoryPoint> PIDBasedPlanner::planCruise(
         stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose,
         tier4_planning_msgs::msg::PlanningFactor::NONE,
         tier4_planning_msgs::msg::SafetyFactorArray{});
-      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-        planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE,
-        stop_traj_points.at(wall_idx).pose));
     }
 
     // do cruise planning

diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
@@ -182,8 +182,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
                                   : std::abs(vehicle_info_.min_longitudinal_offset_m);
 
   if (stop_obstacles.empty()) {
-    velocity_factors_pub_->publish(
-      obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time));
     // delete marker
     const auto markers =
       autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
@@ -336,8 +334,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
-    velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-      planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose));
     planning_factor_interface_->add(
       output_traj_points, planner_data.ego_pose, stop_pose,
       tier4_planning_msgs::msg::PlanningFactor::STOP,
@@ -594,9 +590,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
       const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
         planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward);
-      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
-        planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE,
-        slow_down_traj_points.at(slow_down_wall_idx).pose));
       autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
       planning_factor_interface_->add(
         slow_down_traj_points, planner_data.ego_pose,

diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp
@@ -118,25 +118,4 @@ std::vector<StopObstacle> getClosestStopObstacles(const std::vector<StopObstacle
   return candidates;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::string & behavior,
-  const std::optional<geometry_msgs::msg::Pose> pose)
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = time;
-
-  if (pose) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = behavior;
-    velocity_factor.pose = pose.value();
-    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
-    velocity_factor.status = VelocityFactor::UNKNOWN;
-    velocity_factor.detail = std::string();
-    velocity_factor_array.factors.push_back(velocity_factor);
-  }
-  return velocity_factor_array;
-}
-
 }  // namespace obstacle_cruise_utils