fix(start/goal_planner): do not call freespace planner callback when …

…not necessary (autowarefoundation#6125) Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
kyoichi-sugahara · Jan 23, 2024 · 39dcaa5 · 39dcaa5
1 parent a7c166a
commit 39dcaa5
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 1 deletion.
diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -275,6 +275,10 @@ void GoalPlannerModule::onTimer()
 
 void GoalPlannerModule::onFreespaceParkingTimer()
 {
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
+    return;
+  }
+
   if (!planner_data_) {
     return;
   }

diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp
@@ -87,6 +87,10 @@ StartPlannerModule::StartPlannerModule(
 
 void StartPlannerModule::onFreespacePlannerTimer()
 {
+  if (getCurrentStatus() == ModuleStatus::IDLE) {
+    return;
+  }
+
   if (!planner_data_) {
     return;
   }
@@ -97,7 +101,11 @@ void StartPlannerModule::onFreespacePlannerTimer()
 
   const bool is_new_costmap =
     (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0;
-  if (isStuck() && is_new_costmap) {
+  if (!is_new_costmap) {
+    return;
+  }
+
+  if (isStuck()) {
     planFreespacePath();
   }
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -275,6 +275,10 @@ void GoalPlannerModule::onTimer() @@
     void GoalPlannerModule::onFreespaceParkingTimer()
     {
+      if (getCurrentStatus() == ModuleStatus::IDLE) {
+        return;
+      }
       if (!planner_data_) {
         return;
       }
@@ Expand Down @@