From 74e93fe1730662137a5a2d49ceb3772f4f786063 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 25 Oct 2023 22:02:59 +0900 Subject: [PATCH] feat(goal_planner): do not decelerate before path candidates generation Signed-off-by: kosuke55 refactor(goal_planner): use updateData Signed-off-by: kosuke55 upda use getPreviousModuleOutput also in waiting approval Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 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 index 2db3560d6e4f3..60c6559b8de6f 100644 --- 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 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::max(),