fix(goal_planner): disable freespace pull over after arriving modifie…

…d goal (#5290) fix(goal_planner): disable freespace pull over after arriving modified goal Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 12, 2023 · d8f285b · d8f285b
1 parent 744d48d
commit d8f285b
Showing 1 changed file with 8 additions and 0 deletions.
diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
@@ -218,6 +218,10 @@ void GoalPlannerModule::onFreespaceParkingTimer()
     return;
   }
 
+  if (isOnModifiedGoal()) {
+    return;
+  }
+
   const bool is_new_costmap =
     (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0;
   constexpr double path_update_duration = 1.0;
@@ -1210,6 +1214,10 @@ bool GoalPlannerModule::isStopped()
 
 bool GoalPlannerModule::isStuck()
 {
+  if (isOnModifiedGoal()) {
+    return false;
+  }
+
   constexpr double stuck_time = 5.0;
   if (!isStopped(odometry_buffer_stuck_, stuck_time)) {
     return false;