feat(behavior_path_planner): move reset path candidate function to be…

…havior tree manager Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
autowarefoundation · Dec 19, 2022 · dd7de61 · dd7de61
1 parent 3098ace
commit dd7de61
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 11 deletions.
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp
@@ -72,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_;

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -768,19 +768,11 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg()
 void BehaviorPathPlannerNode::publishPathCandidate(
   const std::vector<std::shared_ptr<SceneModuleInterface>> & scene_modules)
 {
-  const bool is_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 (path_candidate_publishers_.count(module->name()) == 0) {
-      continue;
-    }
-    if (is_running && (module->current_state_ != BT::NodeStatus::RUNNING)) {
-      module->resetPathCandidate();
+    if (path_candidate_publishers_.count(module->name()) != 0) {
+      path_candidate_publishers_.at(module->name())
+        ->publish(convertToPath(module->getPathCandidate(), module->isExecutionReady()));
     }
-    path_candidate_publishers_.at(module->name())
-      ->publish(convertToPath(module->getPathCandidate(), module->isExecutionReady()));
   }
 }
 

diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/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(