fix(bpp)!: remove stop reason (#9449)

fix(bpp): remove stop reason

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_planner/autoware_behavior_path_planner/README.md

@@ -113,7 +113,6 @@ The Planner Manager's responsibilities include:
 | ~/output/turn_indicators_cmd  | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command.                                                                       | `volatile`        |
 | ~/output/hazard_lights_cmd    | `autoware_vehicle_msgs::msg::HazardLightsCommand`   | hazard lights command.                                                                         | `volatile`        |
 | ~/output/modified_goal        | `autoware_planning_msgs::msg::PoseWithUuidStamped`  | output modified goal commands.                                                                 | `transient_local` |
-| ~/output/stop_reasons         | `tier4_planning_msgs::msg::StopReasonArray`         | describe the reason for ego vehicle stop                                                       | `volatile`        |
 | ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability`     | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile`        |
 
 ### Debug

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp

@@ -40,7 +40,6 @@
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 #include <tier4_planning_msgs/msg/reroute_availability.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
-#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -71,7 +70,6 @@ using tier4_planning_msgs::msg::LateralOffset;
 using tier4_planning_msgs::msg::PathWithLaneId;
 using tier4_planning_msgs::msg::RerouteAvailability;
 using tier4_planning_msgs::msg::Scenario;
-using tier4_planning_msgs::msg::StopReasonArray;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
@@ -122,7 +120,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
   rclcpp::Publisher<PoseWithUuidStamped>::SharedPtr modified_goal_publisher_;
-  rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reason_publisher_;
   rclcpp::Publisher<RerouteAvailability>::SharedPtr reroute_availability_publisher_;
   rclcpp::Publisher<SteeringFactorArray>::SharedPtr pub_steering_factors_;
   rclcpp::TimerBase::SharedPtr timer_;

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp

@@ -26,7 +26,6 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
-#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 
 #include <lanelet2_core/primitives/Lanelet.h>
 
@@ -42,7 +41,6 @@ namespace autoware::behavior_path_planner
 
 using autoware::universe_utils::StopWatch;
 using tier4_planning_msgs::msg::PathWithLaneId;
-using tier4_planning_msgs::msg::StopReasonArray;
 using SceneModulePtr = std::shared_ptr<SceneModuleInterface>;
 using SceneModuleManagerPtr = std::shared_ptr<SceneModuleManagerInterface>;
 using DebugPublisher = autoware::universe_utils::DebugPublisher;
@@ -484,36 +482,6 @@ class PlannerManager
     return ret;
   }
 
-  /**
-   * @brief aggregate launched module's stop reasons.
-   * @return stop reason array
-   */
-  StopReasonArray getStopReasons() const
-  {
-    StopReasonArray stop_reason_array;
-    stop_reason_array.header.frame_id = "map";
-    stop_reason_array.header.stamp = clock_.now();
-
-    const auto approved_module_ptrs = approved_modules();
-    const auto candidate_module_ptrs = candidate_modules();
-
-    std::for_each(approved_module_ptrs.begin(), approved_module_ptrs.end(), [&](const auto & m) {
-      const auto reason = m->getStopReason();
-      if (reason.reason != "") {
-        stop_reason_array.stop_reasons.push_back(m->getStopReason());
-      }
-    });
-
-    std::for_each(candidate_module_ptrs.begin(), candidate_module_ptrs.end(), [&](const auto & m) {
-      const auto reason = m->getStopReason();
-      if (reason.reason != "") {
-        stop_reason_array.stop_reasons.push_back(m->getStopReason());
-      }
-    });
-
-    return stop_reason_array;
-  }
-
   /**
    * @brief check if re-routable approved module is running(namely except for fixed_goal_planner
    * and dynamic_avoidance)

planning/behavior_path_planner/autoware_behavior_path_planner/launch/behavior_path_planner.launch.xml

@@ -37,7 +37,6 @@
     <remap from="~/output/turn_indicators_cmd" to="/planning/turn_indicators_cmd"/>
     <remap from="~/output/hazard_lights_cmd" to="/planning/hazard_lights_cmd"/>
     <remap from="~/output/modified_goal" to="/planning/scenario_planning/modified_goal"/>
-    <remap from="~/output/stop_reasons" to="/planning/scenario_planning/status/stop_reasons"/>
     <!-- params -->
     <param name="launch_modules" value="$(var behavior_path_planner_launch_modules)"/>
     <!-- load config -->

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -51,7 +51,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   const auto durable_qos = rclcpp::QoS(1).transient_local();
   modified_goal_publisher_ =
     create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
-  stop_reason_publisher_ = create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
   pub_steering_factors_ =
     create_publisher<SteeringFactorArray>("/planning/steering_factor/intersection", 1);
   reroute_availability_publisher_ =
@@ -408,7 +407,6 @@ void BehaviorPathPlannerNode::run()
   publishSceneModuleDebugMsg(planner_manager_->getDebugMsg());
   publishPathCandidate(planner_manager_->getSceneModuleManagers(), planner_data_);
   publishPathReference(planner_manager_->getSceneModuleManagers(), planner_data_);
-  stop_reason_publisher_->publish(planner_manager_->getStopReasons());
 
   // publish modified goal only when it is updated
   if (

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp

@@ -39,9 +39,6 @@
 #include <autoware_adapi_v1_msgs/msg/steering_factor.hpp>
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
-#include <tier4_planning_msgs/msg/stop_factor.hpp>
-#include <tier4_planning_msgs/msg/stop_reason.hpp>
-#include <tier4_planning_msgs/msg/stop_reason_array.hpp>
 #include <tier4_rtc_msgs/msg/state.hpp>
 #include <unique_identifier_msgs/msg/uuid.hpp>
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
@@ -69,9 +66,6 @@ using autoware_adapi_v1_msgs::msg::SteeringFactor;
 using autoware_adapi_v1_msgs::msg::VelocityFactor;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::PathWithLaneId;
-using tier4_planning_msgs::msg::StopFactor;
-using tier4_planning_msgs::msg::StopReason;
-using tier4_planning_msgs::msg::StopReasonArray;
 using tier4_rtc_msgs::msg::State;
 using unique_identifier_msgs::msg::UUID;
 using visualization_msgs::msg::MarkerArray;
@@ -192,8 +186,6 @@ class SceneModuleInterface
 
     reset_factor();
 
-    stop_reason_ = StopReason();
-
     processOnExit();
   }
 
@@ -266,8 +258,6 @@ class SceneModuleInterface
 
   ModuleStatus getCurrentStatus() const { return current_state_; }
 
-  StopReason getStopReason() const { return stop_reason_; }
-
   void reset_factor()
   {
     steering_factor_interface_.reset();
@@ -446,8 +436,6 @@ class SceneModuleInterface
 
   BehaviorModuleOutput previous_module_output_;
 
-  StopReason stop_reason_;
-
   bool is_locked_new_module_launch_{false};
 
   bool is_locked_output_path_{false};
@@ -580,23 +568,6 @@ class SceneModuleInterface
       path.points, getEgoPose(), slow_pose_.value(), VelocityFactor::APPROACHING, "slow down");
   }
 
-  void setStopReason(const std::string & stop_reason, const PathWithLaneId & path)
-  {
-    stop_reason_.reason = stop_reason;
-    stop_reason_.stop_factors.clear();
-
-    if (!stop_pose_) {
-      stop_reason_.reason = "";
-      return;
-    }
-
-    StopFactor stop_factor;
-    stop_factor.stop_pose = stop_pose_.value();
-    stop_factor.dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength(
-      path.points, getEgoPosition(), stop_pose_.value().position);
-    stop_reason_.stop_factors.push_back(stop_factor);
-  }
-
   void setDrivableLanes(const std::vector<DrivableLanes> & drivable_lanes);
 
   BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; }

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_interface.cpp

@@ -26,8 +26,6 @@ void SceneModuleInterface::onEntry()
 {
   RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__);
 
-  stop_reason_ = StopReason();
-
   processOnEntry();
 }
 }  // namespace autoware::behavior_path_planner