fix(surround_obstacle_checker)!: remove stop reason (#9450)

fix(surround_obstacle_checker): remove stop reason

Signed-off-by: satoshi-ota <[email protected]>

planning/autoware_surround_obstacle_checker/README.md

@@ -34,8 +34,6 @@ else
 
 endif
 
-:Publish stop reason;
-
 stop
 @enduml
 ```
@@ -93,7 +91,6 @@ As mentioned in stop condition section, it prevents chattering by changing thres
 | `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command                                                          |
 | `~/output/max_velocity`                 | `tier4_planning_msgs::msg::VelocityLimit`             | Velocity limit command                                                                |
 | `~/output/no_start_reason`              | `diagnostic_msgs::msg::DiagnosticStatus`              | No start reason                                                                       |
-| `~/output/stop_reasons`                 | `tier4_planning_msgs::msg::StopReasonArray`           | Stop reasons                                                                          |
 | `~/debug/marker`                        | `visualization_msgs::msg::MarkerArray`                | Marker for visualization                                                              |
 | `~/debug/footprint`                     | `geometry_msgs::msg::PolygonStamped`                  | Ego vehicle base footprint for visualization                                          |
 | `~/debug/footprint_offset`              | `geometry_msgs::msg::PolygonStamped`                  | Ego vehicle footprint with `surround_check_distance` offset for visualization         |

planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml

@@ -10,7 +10,6 @@
   <node pkg="autoware_surround_obstacle_checker" exec="surround_obstacle_checker_node" name="surround_obstacle_checker" output="screen">
     <param from="$(var param_path)"/>
     <remap from="~/output/no_start_reason" to="/planning/scenario_planning/status/no_start_reason"/>
-    <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
     <remap from="~/output/max_velocity" to="$(var output_velocity_limit)"/>
     <remap from="~/output/velocity_limit_clear_command" to="$(var output_velocity_limit_clear_command)"/>
     <remap from="~/input/pointcloud" to="$(var input_pointcloud)"/>

planning/autoware_surround_obstacle_checker/src/debug_marker.cpp

@@ -75,7 +75,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode(
   clock_(clock)
 {
   debug_viz_pub_ = node.create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", 1);
-  stop_reason_pub_ = node.create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
   velocity_factor_pub_ =
     node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/surround_obstacle", 1);
   vehicle_footprint_pub_ = node.create_publisher<PolygonStamped>("~/debug/footprint", 1);
@@ -142,8 +141,6 @@ void SurroundObstacleCheckerDebugNode::publish()
   debug_viz_pub_->publish(visualization_msg);
 
   /* publish stop reason for autoware api */
-  const auto stop_reason_msg = makeStopReasonArray();
-  stop_reason_pub_->publish(stop_reason_msg);
   const auto velocity_factor_msg = makeVelocityFactorArray();
   velocity_factor_pub_->publish(velocity_factor_msg);
 
@@ -171,33 +168,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker()
   return msg;
 }
 
-StopReasonArray SurroundObstacleCheckerDebugNode::makeStopReasonArray()
-{
-  // create header
-  std_msgs::msg::Header header;
-  header.frame_id = "map";
-  header.stamp = this->clock_->now();
-
-  // create stop reason stamped
-  StopReason stop_reason_msg;
-  stop_reason_msg.reason = StopReason::SURROUND_OBSTACLE_CHECK;
-  StopFactor stop_factor;
-
-  if (stop_pose_ptr_ != nullptr) {
-    stop_factor.stop_pose = *stop_pose_ptr_;
-    if (stop_obstacle_point_ptr_ != nullptr) {
-      stop_factor.stop_factor_points.emplace_back(*stop_obstacle_point_ptr_);
-    }
-    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;
-}
-
 VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray()
 {
   VelocityFactorArray velocity_factor_array;

planning/autoware_surround_obstacle_checker/src/debug_marker.hpp

@@ -22,7 +22,6 @@
 #include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp>
 #include <geometry_msgs/msg/polygon_stamped.hpp>
 #include <geometry_msgs/msg/pose.hpp>
-#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
@@ -39,9 +38,6 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
 using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
 using geometry_msgs::msg::PolygonStamped;
-using tier4_planning_msgs::msg::StopFactor;
-using tier4_planning_msgs::msg::StopReason;
-using tier4_planning_msgs::msg::StopReasonArray;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
@@ -69,7 +65,6 @@ class SurroundObstacleCheckerDebugNode
 
 private:
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_viz_pub_;
-  rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_pub_;
   rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factor_pub_;
 
   rclcpp::Publisher<PolygonStamped>::SharedPtr vehicle_footprint_pub_;
@@ -85,7 +80,6 @@ class SurroundObstacleCheckerDebugNode
   geometry_msgs::msg::Pose self_pose_;
 
   MarkerArray makeVisualizationMarker();
-  StopReasonArray makeStopReasonArray();
   VelocityFactorArray makeVelocityFactorArray();
 
   PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z);

planning/autoware_surround_obstacle_checker/src/node.cpp

@@ -57,34 +57,6 @@ using autoware::universe_utils::createPoint;
 using autoware::universe_utils::pose2transform;
 using autoware_perception_msgs::msg::ObjectClassification;
 
-namespace
-{
-std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose)
-{
-  const std::string json_dumps_pose =
-    (boost::format(
-       R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") %
-     pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x %
-     pose.orientation.y % pose.orientation.z)
-      .str();
-  return json_dumps_pose;
-}
-
-diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag(
-  const std::string & no_start_reason, const geometry_msgs::msg::Pose & stop_pose)
-{
-  diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag;
-  diagnostic_msgs::msg::KeyValue no_start_reason_diag_kv;
-  no_start_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK;
-  no_start_reason_diag.name = "no_start_reason";
-  no_start_reason_diag.message = no_start_reason;
-  no_start_reason_diag_kv.key = "no_start_pose";
-  no_start_reason_diag_kv.value = jsonDumpsPose(stop_pose);
-  no_start_reason_diag.values.push_back(no_start_reason_diag_kv);
-  return no_start_reason_diag;
-}
-}  // namespace
-
 SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptions & node_options)
 : Node("surround_obstacle_checker_node", node_options)
 {
@@ -104,8 +76,6 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio
   vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
 
   // Publishers
-  pub_stop_reason_ =
-    this->create_publisher<diagnostic_msgs::msg::DiagnosticStatus>("~/output/no_start_reason", 1);
   pub_clear_velocity_limit_ = this->create_publisher<VelocityLimitClearCommand>(
     "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local());
   pub_velocity_limit_ = this->create_publisher<VelocityLimit>(
@@ -256,18 +226,15 @@ void SurroundObstacleCheckerNode::onTimer()
     debug_ptr_->pushObstaclePoint(nearest_obstacle.value().second, PointType::NoStart);
   }
 
-  diagnostic_msgs::msg::DiagnosticStatus no_start_reason_diag;
   if (state_ == State::STOP) {
     debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart);
-    no_start_reason_diag = makeStopReasonDiag("obstacle", odometry_ptr_->pose.pose);
   }
 
   tier4_debug_msgs::msg::Float64Stamped processing_time_msg;
   processing_time_msg.stamp = get_clock()->now();
   processing_time_msg.data = stop_watch.toc();
   pub_processing_time_->publish(processing_time_msg);
 
-  pub_stop_reason_->publish(no_start_reason_diag);
   debug_ptr_->publish();
 }
 

planning/autoware_surround_obstacle_checker/src/node.hpp

@@ -104,7 +104,6 @@ class SurroundObstacleCheckerNode : public rclcpp::Node
     sub_pointcloud_{this, "~/input/pointcloud", autoware::universe_utils::SingleDepthSensorQoS()};
   autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> sub_dynamic_objects_{
     this, "~/input/objects"};
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr pub_stop_reason_;
   rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr pub_clear_velocity_limit_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float64Stamped>::SharedPtr pub_processing_time_;

planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md

@@ -34,8 +34,6 @@ else
 
 endif
 
-:Publish stop reason;
-
 stop
 @enduml
 ```
@@ -93,7 +91,6 @@ Stop condition の項で述べたように、状態によって障害物判定
 | `~/output/velocity_limit_clear_command` | `tier4_planning_msgs::msg::VelocityLimitClearCommand` | Velocity limit clear command |
 | `~/output/max_velocity`                 | `tier4_planning_msgs::msg::VelocityLimit`             | Velocity limit command       |
 | `~/output/no_start_reason`              | `diagnostic_msgs::msg::DiagnosticStatus`              | No start reason              |
-| `~/output/stop_reasons`                 | `tier4_planning_msgs::msg::StopReasonArray`           | Stop reasons                 |
 | `~/debug/marker`                        | `visualization_msgs::msg::MarkerArray`                | Marker for visualization     |
 
 ## Parameters