feat(behavior_path_planner): remove SUCCESS module candidate path (au…

…towarefoundation#3929) Signed-off-by: kosuke55 <[email protected]>
tier4 · Jun 19, 2023 · fa87795 · fa87795
1 parent f653f72
commit fa87795
Showing 1 changed file with 10 additions and 3 deletions.
diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -1405,9 +1405,16 @@ void BehaviorPathPlannerNode::publishPathCandidate(
     }
 
     for (auto & module : manager->getSceneModules()) {
-      path_candidate_publishers_.at(module->name())
-        ->publish(
-          convertToPath(module->getPathCandidate(), module->isExecutionReady(), planner_data));
+      const auto & status = module->getCurrentStatus();
+      const auto candidate_path = std::invoke([&]() {
+        if (status == ModuleStatus::SUCCESS || status == ModuleStatus::FAILURE) {
+          // clear candidate path if the module is finished
+          return convertToPath(nullptr, false, planner_data);
+        }
+        return convertToPath(module->getPathCandidate(), module->isExecutionReady(), planner_data);
+      });
+
+      path_candidate_publishers_.at(module->name())->publish(candidate_path);
     }
   }
 }