fix(obstacle_cruise_planner)!: remove stop reason (#9464)

fix(obstacle_cruise_planner): remove stop reason Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Nov 26, 2024 · 5372403 · 5372403
1 parent abf0f23
commit 5372403
Show file tree

Hide file tree

Showing 5 changed files with 5 additions and 66 deletions.
diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md
@@ -23,12 +23,11 @@ The `autoware_obstacle_cruise_planner` package has following modules.
 
 ### Output topics
 
-| Name                            | Type                                           | Description                           |
-| ------------------------------- | ---------------------------------------------- | ------------------------------------- |
-| `~/output/trajectory`           | autoware_planning_msgs::Trajectory             | output trajectory                     |
-| `~/output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
-| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
-| `~/output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
+| Name                            | Type                                           | Description                      |
+| ------------------------------- | ---------------------------------------------- | -------------------------------- |
+| `~/output/trajectory`           | autoware_planning_msgs::Trajectory             | output trajectory                |
+| `~/output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising      |
+| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit |
 
 ## Design
 

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
@@ -54,7 +54,6 @@ class PlannerInterface
     slow_down_param_(SlowDownParam(node)),
     stop_param_(StopParam(node, longitudinal_info))
   {
-    stop_reasons_pub_ = node.create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
     velocity_factors_pub_ =
       node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_cruise", 1);
     stop_speed_exceeded_pub_ =
@@ -146,7 +145,6 @@ class PlannerInterface
     stop_watch_;
 
   // Publishers
-  rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reasons_pub_;
   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
@@ -30,8 +30,6 @@
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "tier4_debug_msgs/msg/float32_stamped.hpp"
 #include "tier4_debug_msgs/msg/float64_stamped.hpp"
-#include "tier4_planning_msgs/msg/stop_factor.hpp"
-#include "tier4_planning_msgs/msg/stop_reason_array.hpp"
 #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"
 #include "tier4_planning_msgs/msg/velocity_limit.hpp"
 #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp"
@@ -58,9 +56,6 @@ using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
 using tier4_debug_msgs::msg::Float32Stamped;
 using tier4_debug_msgs::msg::Float64Stamped;
-using tier4_planning_msgs::msg::StopFactor;
-using tier4_planning_msgs::msg::StopReason;
-using tier4_planning_msgs::msg::StopReasonArray;
 using tier4_planning_msgs::msg::StopSpeedExceeded;
 using tier4_planning_msgs::msg::VelocityLimit;
 using tier4_planning_msgs::msg::VelocityLimitClearCommand;

diff --git a/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/autoware_obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml
@@ -31,6 +31,5 @@
     <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
     <remap from="~/output/velocity_limit" to="$(var output_velocity_limit)"/>
     <remap from="~/output/clear_velocity_limit" to="$(var output_clear_velocity_limit)"/>
-    <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
   </node>
 </launch>
diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
@@ -39,54 +39,6 @@ StopSpeedExceeded createStopSpeedExceededMsg(
   return msg;
 }
 
-tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray(
-  const PlannerData & planner_data, const geometry_msgs::msg::Pose & stop_pose,
-  const StopObstacle & stop_obstacle)
-{
-  // create header
-  std_msgs::msg::Header header;
-  header.frame_id = "map";
-  header.stamp = planner_data.current_time;
-
-  // create stop factor
-  StopFactor stop_factor;
-  stop_factor.stop_pose = stop_pose;
-  geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_point;
-  stop_factor_point.z = stop_pose.position.z;
-  stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength(
-    planner_data.traj_points, planner_data.ego_pose.position, stop_pose.position);
-  stop_factor.stop_factor_points.emplace_back(stop_factor_point);
-
-  // create stop reason stamped
-  StopReason stop_reason_msg;
-  stop_reason_msg.reason = StopReason::OBSTACLE_STOP;
-  stop_reason_msg.stop_factors.emplace_back(stop_factor);
-
-  // create stop reason array
-  StopReasonArray stop_reason_array;
-  stop_reason_array.header = header;
-  stop_reason_array.stop_reasons.emplace_back(stop_reason_msg);
-  return stop_reason_array;
-}
-
-StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time)
-{
-  // create header
-  std_msgs::msg::Header header;
-  header.frame_id = "map";
-  header.stamp = current_time;
-
-  // create stop reason stamped
-  StopReason stop_reason_msg;
-  stop_reason_msg.reason = StopReason::OBSTACLE_STOP;
-
-  // create stop reason array
-  StopReasonArray stop_reason_array;
-  stop_reason_array.header = header;
-  stop_reason_array.stop_reasons.emplace_back(stop_reason_msg);
-  return stop_reason_array;
-}
-
 double calcMinimumDistanceToStop(
   const double initial_vel, const double max_acc, const double min_acc)
 {
@@ -226,7 +178,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
                                   : std::abs(vehicle_info_.min_longitudinal_offset_m);
 
   if (stop_obstacles.empty()) {
-    stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time));
     velocity_factors_pub_->publish(
       obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time));
     // delete marker
@@ -381,9 +332,6 @@ std::vector<TrajectoryPoint> PlannerInterface::generateStopTrajectory(
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
-    const auto stop_reasons_msg =
-      makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle);
-    stop_reasons_pub_->publish(stop_reasons_msg);
     velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
       planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose));
     // Store stop reason debug data