fix(goal_planner): fix freespace planning chattering

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
autowarefoundation · Sep 27, 2024 · 297d780 · 297d780
1 parent 21f547d
commit 297d780
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
@@ -1329,9 +1329,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(
     return getPreviousModuleOutput();
   }
 
+  const bool is_freespace =
+    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE;
   if (
     path_decision_controller_.get_current_state().state ==
       PathDecisionState::DecisionKind::NOT_DECIDED &&
+    !is_freespace &&
     needPathUpdate(
       planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) {
     // if the final path is not decided and enough time has passed since last path update,
@@ -1370,9 +1373,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(
   setOutput(context_data, output);
 
   // return to lane parking if it is possible
-  if (
-    thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE &&
-    canReturnToLaneParking(context_data)) {
+  if (is_freespace && canReturnToLaneParking(context_data)) {
     thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path());
   }