feat(behavior_path_planner): output multiple candidate paths

planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp

@@ -45,6 +45,7 @@
 #include <tier4_planning_msgs/msg/scenario.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
+#include <map>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -98,12 +99,13 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Subscription<PredictedObjects>::SharedPtr perception_subscriber_;
   rclcpp::Subscription<OccupancyGrid>::SharedPtr occupancy_grid_subscriber_;
   rclcpp::Publisher<PathWithLaneId>::SharedPtr path_publisher_;
-  rclcpp::Publisher<Path>::SharedPtr path_candidate_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
   rclcpp::TimerBase::SharedPtr timer_;
 
+  std::map<std::string, rclcpp::Publisher<Path>::SharedPtr> path_candidate_publishers_;
+
   std::shared_ptr<PlannerData> planner_data_;
   std::shared_ptr<BehaviorTreeManager> bt_manager_;
   std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
@@ -160,12 +162,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   PathWithLaneId::SharedPtr getPath(
     const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);
 
-  /**
-   * @brief extract path candidate from behavior tree output
-   */
-  PathWithLaneId::SharedPtr getPathCandidate(
-    const BehaviorModuleOutput & bt_out, const std::shared_ptr<PlannerData> planner_data);
-
   /**
    * @brief skip smooth goal connection
    */
@@ -184,11 +180,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<LaneChangeDebugMsgArray>::SharedPtr debug_lane_change_msg_array_publisher_;
 
-  /**
-   * @brief check path if it is unsafe or forced
-   */
-  bool isForcedCandidatePath() const;
-
   /**
    * @brief publish steering factor from intersection
    */
@@ -204,6 +195,18 @@ class BehaviorPathPlannerNode : public rclcpp::Node
    */
   void publishSceneModuleDebugMsg();
 
+  /**
+   * @brief publish path candidate
+   */
+  void publishPathCandidate(
+    const std::vector<std::shared_ptr<SceneModuleInterface>> & scene_modules);
+
+  /**
+   * @brief convert path with lane id to path for publish path candidate
+   */
+  Path convertToPath(
+    const std::shared_ptr<PathWithLaneId> & path_candidate_ptr, const bool is_ready);
+
   template <class T>
   size_t findEgoIndex(const std::vector<T> & points) const
   {

planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp

@@ -51,6 +51,10 @@ class BehaviorTreeManager
   BehaviorModuleOutput run(const std::shared_ptr<PlannerData> & data);
   std::vector<std::shared_ptr<SceneModuleStatus>> getModulesStatus();
   std::shared_ptr<SceneModuleVisitor> getAllSceneModuleDebugMsgData();
+  std::vector<std::shared_ptr<SceneModuleInterface>> getSceneModules() const
+  {
+    return scene_modules_;
+  }
 
   AvoidanceDebugMsgArray getAvoidanceDebugMsgArray();
 
@@ -68,6 +72,7 @@ class BehaviorTreeManager
   BT::Blackboard::Ptr blackboard_;
 
   BT::NodeStatus checkForceApproval(const std::string & name);
+  void resetNotRunningModulePathCandidate();
 
   // For Groot monitoring
   std::unique_ptr<BT::PublisherZMQ> groot_monitor_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -62,9 +62,6 @@ struct BehaviorModuleOutput
   // path planed by module
   PlanResult path{};
 
-  // path candidate planed by module
-  PlanResult path_candidate{};
-
   TurnSignalInfo turn_signal_info{};
 };
 
@@ -130,7 +127,7 @@ class SceneModuleInterface
     BehaviorModuleOutput out;
     out.path = util::generateCenterLinePath(planner_data_);
     const auto candidate = planCandidate();
-    out.path_candidate = std::make_shared<PathWithLaneId>(candidate.path_candidate);
+    path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
     return out;
   }
 
@@ -228,6 +225,10 @@ class SceneModuleInterface
 
   bool isWaitingApproval() const { return is_waiting_approval_; }
 
+  PlanResult getPathCandidate() const { return path_candidate_; }
+
+  void resetPathCandidate() { path_candidate_.reset(); }
+
   virtual void lockRTCCommand()
   {
     if (!rtc_interface_ptr_) {
@@ -258,6 +259,7 @@ class SceneModuleInterface
   std::unique_ptr<SteeringFactorInterface> steering_factor_interface_ptr_;
   UUID uuid_;
   bool is_waiting_approval_;
+  PlanResult path_candidate_;
 
   void updateRTCStatus(const double start_distance, const double finish_distance)
   {

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -61,7 +61,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
   // publisher
   path_publisher_ = create_publisher<PathWithLaneId>("~/output/path", 1);
-  path_candidate_publisher_ = create_publisher<Path>("~/output/path_candidate", 1);
   turn_signal_publisher_ =
     create_publisher<TurnIndicatorsCommand>("~/output/turn_indicators_cmd", 1);
   hazard_signal_publisher_ = create_publisher<HazardLightsCommand>("~/output/hazard_lights_cmd", 1);
@@ -115,6 +114,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1));
   // behavior tree manager
   {
+    const std::string path_candidate_name_space = "/planning/path_candidate/";
     mutex_bt_.lock();
 
     bt_manager_ = std::make_shared<BehaviorTreeManager>(*this, getBehaviorTreeManagerParam());
@@ -125,6 +125,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
     auto avoidance_module =
       std::make_shared<AvoidanceModule>("Avoidance", *this, avoidance_param_ptr);
+    path_candidate_publishers_.emplace(
+      "Avoidance", create_publisher<Path>(path_candidate_name_space + "avoidance", 1));
     bt_manager_->registerSceneModule(avoidance_module);
 
     auto lane_following_module =
@@ -133,12 +135,18 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
 
     auto lane_change_module =
       std::make_shared<LaneChangeModule>("LaneChange", *this, lane_change_param_ptr);
+    path_candidate_publishers_.emplace(
+      "LaneChange", create_publisher<Path>(path_candidate_name_space + "lane_change", 1));
     bt_manager_->registerSceneModule(lane_change_module);
 
     auto pull_over_module = std::make_shared<PullOverModule>("PullOver", *this, getPullOverParam());
+    path_candidate_publishers_.emplace(
+      "PullOver", create_publisher<Path>(path_candidate_name_space + "pull_over", 1));
     bt_manager_->registerSceneModule(pull_over_module);
 
     auto pull_out_module = std::make_shared<PullOutModule>("PullOut", *this, getPullOutParam());
+    path_candidate_publishers_.emplace(
+      "PullOut", create_publisher<Path>(path_candidate_name_space + "pull_out", 1));
     bt_manager_->registerSceneModule(pull_out_module);
 
     bt_manager_->createBehaviorTree();
@@ -643,8 +651,7 @@ void BehaviorPathPlannerNode::run()
       get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");
   }
 
-  const auto path_candidate = getPathCandidate(output, planner_data);
-  path_candidate_publisher_->publish(util::toPath(*path_candidate));
+  publishPathCandidate(bt_manager_->getSceneModules());
 
   publishSceneModuleDebugMsg();
 
@@ -758,6 +765,39 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg()
   }
 }
 
+void BehaviorPathPlannerNode::publishPathCandidate(
+  const std::vector<std::shared_ptr<SceneModuleInterface>> & scene_modules)
+{
+  for (auto & module : scene_modules) {
+    if (path_candidate_publishers_.count(module->name()) != 0) {
+      path_candidate_publishers_.at(module->name())
+        ->publish(convertToPath(module->getPathCandidate(), module->isExecutionReady()));
+    }
+  }
+}
+
+Path BehaviorPathPlannerNode::convertToPath(
+  const std::shared_ptr<PathWithLaneId> & path_candidate_ptr, const bool is_ready)
+{
+  Path output;
+  output.header = planner_data_->route_handler->getRouteHeader();
+  output.header.stamp = this->now();
+
+  if (!path_candidate_ptr) {
+    return output;
+  }
+
+  output = util::toPath(*path_candidate_ptr);
+
+  if (!is_ready) {
+    for (auto & point : output.points) {
+      point.longitudinal_velocity_mps = 0.0;
+    }
+  }
+
+  return output;
+}
+
 PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
   const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
 {
@@ -782,46 +822,6 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath(
   return std::make_shared<PathWithLaneId>(resampled_path);
 }
 
-PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate(
-  const BehaviorModuleOutput & bt_output, const std::shared_ptr<PlannerData> planner_data)
-{
-  auto path_candidate =
-    bt_output.path_candidate ? bt_output.path_candidate : std::make_shared<PathWithLaneId>();
-
-  if (isForcedCandidatePath()) {
-    for (auto & path_point : path_candidate->points) {
-      path_point.point.longitudinal_velocity_mps = 0.0;
-    }
-  }
-
-  path_candidate->header = planner_data->route_handler->getRouteHeader();
-  path_candidate->header.stamp = this->now();
-  RCLCPP_DEBUG(
-    get_logger(), "BehaviorTreeManager: path candidate is %s.",
-    bt_output.path_candidate ? "FOUND" : "NOT FOUND");
-  return path_candidate;
-}
-
-bool BehaviorPathPlannerNode::isForcedCandidatePath() const
-{
-  const auto & module_status_ptr_vec = bt_manager_->getModulesStatus();
-  for (const auto & module_status_ptr : module_status_ptr_vec) {
-    if (!module_status_ptr) {
-      continue;
-    }
-    if (module_status_ptr->module_name != "LaneChange") {
-      continue;
-    }
-    const auto & is_waiting_approval = module_status_ptr->is_waiting_approval;
-    const auto & is_execution_ready = module_status_ptr->is_execution_ready;
-    if (is_waiting_approval && !is_execution_ready) {
-      return true;
-    }
-    break;
-  }
-  return false;
-}
-
 bool BehaviorPathPlannerNode::skipSmoothGoalConnection(
   const std::vector<std::shared_ptr<SceneModuleStatus>> & statuses) const
 {

planning/behavior_path_planner/src/behavior_tree_manager.cpp

@@ -98,6 +98,8 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr<PlannerData>
 
   RCLCPP_DEBUG(logger_, "BehaviorPathPlanner::run end status = %s", BT::toStr(res).c_str());
 
+  resetNotRunningModulePathCandidate();
+
   std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) {
     m->publishDebugMarker();
     if (!m->isExecutionRequested()) {
@@ -127,6 +129,19 @@ BT::NodeStatus BehaviorTreeManager::checkForceApproval(const std::string & name)
   return approval.module_name == name ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
 }
 
+void BehaviorTreeManager::resetNotRunningModulePathCandidate()
+{
+  const bool is_any_module_running = std::any_of(
+    scene_modules_.begin(), scene_modules_.end(),
+    [](const auto & module) { return module->current_state_ == BT::NodeStatus::RUNNING; });
+
+  for (auto & module : scene_modules_) {
+    if (is_any_module_running && (module->current_state_ != BT::NodeStatus::RUNNING)) {
+      module->resetPathCandidate();
+    }
+  }
+}
+
 void BehaviorTreeManager::resetBehaviorTree() { bt_tree_.haltTree(); }
 
 void BehaviorTreeManager::addGrootMonitoring(

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2219,6 +2219,8 @@ BehaviorModuleOutput AvoidanceModule::plan()
 {
   DEBUG_PRINT("AVOIDANCE plan");
 
+  resetPathCandidate();
+
   const auto shift_lines = calcShiftLines(current_raw_shift_lines_, debug_data_);
 
   const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter_);
@@ -2366,7 +2368,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval()
     clearWaitingApproval();
     removeCandidateRTCStatus();
   }
-  out.path_candidate = std::make_shared<PathWithLaneId>(candidate.path_candidate);
+  path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
   return out;
 }
 
@@ -2773,6 +2775,7 @@ void AvoidanceModule::initVariables()
 
   debug_data_ = DebugData();
   debug_marker_.markers.clear();
+  resetPathCandidate();
   registered_raw_shift_lines_ = {};
   current_raw_shift_lines_ = {};
   original_unique_id = 0;

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -143,6 +143,8 @@ BT::NodeStatus LaneChangeModule::updateState()
 
 BehaviorModuleOutput LaneChangeModule::plan()
 {
+  resetPathCandidate();
+
   auto path = status_.lane_change_path.path;
 
   if (!isValidPath(path)) {
@@ -201,7 +203,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval()
   BehaviorModuleOutput out;
   out.path = std::make_shared<PathWithLaneId>(getReferencePath());
   const auto candidate = planCandidate();
-  out.path_candidate = std::make_shared<PathWithLaneId>(candidate.path_candidate);
+  path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
   updateRTCStatus(candidate);
   waitApproval();
   return out;
@@ -640,6 +642,7 @@ void LaneChangeModule::resetParameters()
   steering_factor_interface_ptr_->clearSteeringFactors();
   object_debug_.clear();
   debug_marker_.markers.clear();
+  resetPathCandidate();
 }
 
 void LaneChangeModule::acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -100,6 +100,7 @@ void PullOutModule::onExit()
   clearWaitingApproval();
   removeRTCStatus();
   steering_factor_interface_ptr_->clearSteeringFactors();
+  resetPathCandidate();
   current_state_ = BT::NodeStatus::SUCCESS;
   RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit");
 }
@@ -165,6 +166,7 @@ BehaviorModuleOutput PullOutModule::plan()
 {
   if (isWaitingApproval()) {
     clearWaitingApproval();
+    resetPathCandidate();
     // save current_pose when approved for start_point of turn_signal for backward driving
     last_approved_pose_ = std::make_unique<Pose>(planner_data_->self_pose->pose);
   }
@@ -295,7 +297,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval()
 
   output.path = std::make_shared<PathWithLaneId>(stop_path);
   output.turn_signal_info = calcTurnSignalInfo();
-  output.path_candidate = std::make_shared<PathWithLaneId>(candidate_path);
+  path_candidate_ = std::make_shared<PathWithLaneId>(candidate_path);
 
   const uint16_t steering_factor_direction = std::invoke([&output]() {
     if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {