feat(goal_planner): do not decelerate before path candidates generation

Signed-off-by: kosuke55 <[email protected]> refactor(goal_planner): use updateData Signed-off-by: kosuke55 <[email protected]> upda use getPreviousModuleOutput also in waiting approval Signed-off-by: kosuke55 <[email protected]>
autowarefoundation · Oct 26, 2023 · 74e93fe · 74e93fe
1 parent b4d5d90
commit 74e93fe
Showing 1 changed file with 11 additions and 4 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
@@ -908,14 +908,16 @@ void GoalPlannerModule::decideVelocity()
 
 BehaviorModuleOutput GoalPlannerModule::planWithGoalModification()
 {
+  // if pull over path candidates generation is not finished, use previous module output
+  if (status_.get_pull_over_path_candidates().empty()) {
+    return getPreviousModuleOutput();
+  }
+
   constexpr double path_update_duration = 1.0;
 
   resetPathCandidate();
   resetPathReference();
 
-  // set current road lanes, pull over lanes, and drivable lane
-  setLanes();
-
   // Check if it needs to decide path
   status_.set_has_decided_path(hasDecidedPath());
 
@@ -990,6 +992,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
 
 BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification()
 {
+  // if pull over path candidates generation is not finished, use previous module output
+  if (status_.get_pull_over_path_candidates().empty()) {
+    return getPreviousModuleOutput();
+  }
+
   waitApproval();
 
   BehaviorModuleOutput out;
@@ -1032,7 +1039,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification(
 
 std::pair<double, double> GoalPlannerModule::calcDistanceToPathChange() const
 {
-  const auto & full_path = status_.get_pull_over_path()->getFullPath();
+  const auto full_path = status_.get_pull_over_path()->getFullPath();
 
   const auto ego_segment_idx = motion_utils::findNearestSegmentIndex(
     full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits<double>::max(),