From 361fbafb8bc07bdf02f8f9019760e40351bfc030 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 17 Jul 2023 21:09:40 +0900 Subject: [PATCH] fix(start_planner): set stop path drivable area (#4297) Signed-off-by: kosuke55 --- .../start_planner/start_planner_module.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index dba81909d21fa..56f048c769d22 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -298,11 +298,12 @@ PathWithLaneId StartPlannerModule::getFullPath() const BehaviorModuleOutput StartPlannerModule::planWaitingApproval() { updatePullOutStatus(); - waitApproval(); if (IsGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); + path_reference_ = getPreviousModuleOutput().reference_path; + clearWaitingApproval(); return generateStopOutput(); } @@ -310,15 +311,13 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() if (!status_.is_safe) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); - // the path of getCurrent() is generated by generateStopPath() - const PathWithLaneId stop_path = getCurrentPath(); - output.path = std::make_shared(stop_path); - output.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(stop_path); path_reference_ = getPreviousModuleOutput().reference_path; - return output; + clearWaitingApproval(); + return generateStopOutput(); } + waitApproval(); + const double backward_path_length = planner_data_->parameters.backward_path_length + parameters_->max_back_distance; const auto current_lanes =