feat(behavior_path_planner): use module status idle before approval a…

…nd keep candiate running module when approved module succeed (#3928) * feat(behavior_path_planner): use module status idle before approval and keep candiate running module when approved module succeed Signed-off-by: kosuke55 <[email protected]> * fix pre commmit readability/braces Signed-off-by: kosuke55 <[email protected]> --------- Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Jun 19, 2023 · ea7e4e6 · ea7e4e6
1 parent 3c320a0
commit ea7e4e6
Show file tree

Hide file tree

Showing 7 changed files with 74 additions and 10 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -342,6 +342,32 @@ class PlannerManager
     candidate_module_ptrs_.clear();
   }
 
+  /**
+   * @brief stop and remove not RUNNING modules in candidate_module_ptrs_.
+   */
+  void clearNotRunningCandidateModules()
+  {
+    const auto it = std::remove_if(
+      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) {
+        if (m->getCurrentStatus() != ModuleStatus::RUNNING) {
+          deleteExpiredModules(m);
+          return true;
+        }
+        return false;
+      });
+    candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end());
+  }
+
+  /**
+   * @brief check if there is any RUNNING module in candidate_module_ptrs_.
+   */
+  bool hasAnyRunningCandidateModule()
+  {
+    return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) {
+      return m->getCurrentStatus() == ModuleStatus::RUNNING;
+    });
+  }
+
   /**
    * @brief get current root lanelet. the lanelet is used for reference path generation.
    * @param planner data.

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp
@@ -185,7 +185,9 @@ class SceneModuleInterface
    */
   virtual BehaviorModuleOutput run()
   {
+#ifdef USE_OLD_ARCHITECTURE
     current_state_ = ModuleStatus::RUNNING;
+#endif
 
     updateData();
 

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -53,12 +53,16 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr<PlannerData> & da
       approved_module_ptrs_.begin(), approved_module_ptrs_.end(),
       [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING; });
 
-    const bool is_any_candidate_module_running = std::any_of(
-      candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(),
-      [](const auto & m) { return m->getCurrentStatus() == ModuleStatus::RUNNING; });
+    // IDLE is a state in which an execution has been requested but not yet approved.
+    // once approved, it basically turns to running.
+    const bool is_any_candidate_module_running_or_idle =
+      std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](const auto & m) {
+        return m->getCurrentStatus() == ModuleStatus::RUNNING ||
+               m->getCurrentStatus() == ModuleStatus::IDLE;
+      });
 
     const bool is_any_module_running =
-      is_any_approved_module_running || is_any_candidate_module_running;
+      is_any_approved_module_running || is_any_candidate_module_running_or_idle;
 
     const bool is_out_of_route = utils::isEgoOutOfRoute(
       data->self_odometry->pose.pose, data->prev_modified_goal, data->route_handler);
@@ -422,8 +426,9 @@ std::pair<SceneModulePtr, BehaviorModuleOutput> PlannerManager::runRequestModule
 
 BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<PlannerData> & data)
 {
-  std::unordered_map<std::string, BehaviorModuleOutput> results;
+  const bool has_any_running_candidate_module = hasAnyRunningCandidateModule();
 
+  std::unordered_map<std::string, BehaviorModuleOutput> results;
   BehaviorModuleOutput output = getReferencePath(data);
   results.emplace("root", output);
 
@@ -519,14 +524,14 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * ModuleStatus::RUNNING, the previous module keeps running even if it is in
    * ModuleStatus::SUCCESS.
    */
-  if (lane_change_itr == approved_module_ptrs_.end()) {
+  if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) {
     std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) {
       debug_info_.emplace_back(m, Action::DELETE, "From Approved");
       deleteExpiredModules(m);
     });
 
     approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end());
-    clearCandidateModules();
+    clearNotRunningCandidateModules();
 
     return approved_modules_output;
   }
@@ -537,14 +542,14 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr<Pl
    * root lanelet is updated at the moment of lane change module's unregistering, and that causes
    * change First In module's input.
    */
-  if (not_success_itr == approved_module_ptrs_.rend()) {
+  if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) {
     std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) {
       debug_info_.emplace_back(m, Action::DELETE, "From Approved");
       deleteExpiredModules(m);
     });
 
     approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end());
-    clearCandidateModules();
+    clearNotRunningCandidateModules();
 
     root_lanelet_ = updateRootLanelet(data);
 

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -208,7 +208,15 @@ ModuleStatus AvoidanceModule::updateState()
   }
 
   helper_.setPreviousDrivingLanes(data.current_lanelets);
+
+#ifdef USE_OLD_ARCHITECTURE
   return ModuleStatus::RUNNING;
+#else
+  if (is_plan_running || current_state_ == ModuleStatus::RUNNING) {
+    return ModuleStatus::RUNNING;
+  }
+  return ModuleStatus::IDLE;
+#endif
 }
 
 bool AvoidanceModule::isAvoidancePlanRunning() const

diff --git a/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
@@ -189,6 +189,12 @@ ModuleStatus DynamicAvoidanceModule::updateState()
     return ModuleStatus::SUCCESS;
   }
 
+#ifndef USE_OLD_ARCHITECTURE
+  if (!isActivated() || isWaitingApproval()) {
+    return ModuleStatus::IDLE;
+  }
+#endif
+
   return ModuleStatus::RUNNING;
 }
 

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -87,6 +87,10 @@ bool LaneChangeInterface::isExecutionReady() const
 
 ModuleStatus LaneChangeInterface::updateState()
 {
+  if (!isActivated() || isWaitingApproval()) {
+    return ModuleStatus::IDLE;
+  }
+
   if (!module_type_->isValidPath()) {
 #ifdef USE_OLD_ARCHITECTURE
     return ModuleStatus::FAILURE;

diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
@@ -86,7 +86,9 @@ StartPlannerModule::StartPlannerModule(
 
 BehaviorModuleOutput StartPlannerModule::run()
 {
+#ifdef USE_OLD_ARCHITECTURE
   current_state_ = ModuleStatus::RUNNING;
+#endif
 
 #ifndef USE_OLD_ARCHITECTURE
   if (!isActivated()) {
@@ -176,11 +178,22 @@ bool StartPlannerModule::isExecutionReady() const
   return true;
 }
 
-// this runs only when RUNNING
 ModuleStatus StartPlannerModule::updateState()
 {
   RCLCPP_DEBUG(getLogger(), "START_PLANNER updateState");
 
+#ifdef USE_OLD_ARCHITECTURE
+  if (isActivated() && !isWaitingApproval()) {
+    current_state_ = ModuleStatus::RUNNING;
+  }
+#else
+  if (isActivated() && !isWaitingApproval()) {
+    current_state_ = ModuleStatus::RUNNING;
+  } else {
+    current_state_ = ModuleStatus::IDLE;
+  }
+#endif
+
   if (hasFinishedPullOut()) {
     return ModuleStatus::SUCCESS;
   }