feat(behavior_path_planner): add reroute availability publisher (#4543)

* feat(behavior_path_planner): add reroute availability publisher

Signed-off-by: yutaka <purewater0901@gmail.com>

* update

Signed-off-by: yutaka <purewater0901@gmail.com>

* add comment

Signed-off-by: yutaka <purewater0901@gmail.com>

---------

Signed-off-by: yutaka <purewater0901@gmail.com>

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -42,6 +42,7 @@
 #include <tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module.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 <visualization_msgs/msg/marker.hpp>
@@ -72,6 +73,7 @@ using steering_factor_interface::SteeringFactorInterface;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 using tier4_planning_msgs::msg::LateralOffset;
+using tier4_planning_msgs::msg::RerouteAvailability;
 using tier4_planning_msgs::msg::Scenario;
 using tier4_planning_msgs::msg::StopReasonArray;
 using visualization_msgs::msg::Marker;
@@ -102,6 +104,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   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::TimerBase::SharedPtr timer_;
 
   std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
@@ -171,6 +174,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<LaneChangeDebugMsgArray>::SharedPtr debug_lane_change_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;
 
+  /**
+   * @brief publish reroute availability
+   */
+  void publish_reroute_availability();
+
   /**
    * @brief publish steering factor from intersection
    */

planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp

@@ -214,6 +214,16 @@ class PlannerManager
     return stop_reason_array;
   }
 
+  /**
+   * @brief check if there are approved modules.
+   */
+  bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); }
+
+  /**
+   * @brief check if there are candidate modules.
+   */
+  bool hasCandidateModules() const { return !candidate_module_ptrs_.empty(); }
+
   /**
    * @brief reset root lanelet. if there are approved modules, don't reset root lanelet.
    * @param planner data.

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -68,6 +68,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
   modified_goal_publisher_ = create_publisher<PoseWithUuidStamped>("~/output/modified_goal", 1);
   stop_reason_publisher_ = create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
+  reroute_availability_publisher_ =
+    create_publisher<RerouteAvailability>("~/output/is_reroute_available", 1);
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
   debug_lane_change_msg_array_publisher_ =
@@ -539,6 +541,9 @@ void BehaviorPathPlannerNode::run()
   // compute turn signal
   computeTurnSignal(planner_data_, *path, output);
 
+  // publish reroute availability
+  publish_reroute_availability();
+
   // publish drivable bounds
   publish_bounds(*path);
 
@@ -654,6 +659,26 @@ void BehaviorPathPlannerNode::publish_steering_factor(
   steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now());
 }
 
+void BehaviorPathPlannerNode::publish_reroute_availability()
+{
+  const bool has_approved_modules = planner_manager_->hasApprovedModules();
+  const bool has_candidate_modules = planner_manager_->hasCandidateModules();
+
+  // In the current behavior path planner, we might get unexpected behavior when rerouting while
+  // modules other than lane follow are active. Therefore, rerouting will be allowed only when the
+  // lane follow module is running Note that if there is a approved module or candidate module, it
+  // means non-lane-following modules are runnning.
+  RerouteAvailability is_reroute_available;
+  is_reroute_available.stamp = this->now();
+  if (has_approved_modules || has_candidate_modules) {
+    is_reroute_available.availability = false;
+  } else {
+    is_reroute_available.availability = true;
+  }
+
+  reroute_availability_publisher_->publish(is_reroute_available);
+}
+
 void BehaviorPathPlannerNode::publish_turn_signal_debug_data(const TurnSignalDebugData & debug_data)
 {
   MarkerArray marker_array;