From b2fb7831c565620c13529af2481795a687a7133c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 16 Jun 2023 15:57:44 +0900 Subject: [PATCH] fix(start_planner): fix geometric pull out stop point (#3994) Signed-off-by: kosuke55 --- .../geometric_parallel_parking.cpp | 1 - .../src/utils/start_planner/geometric_pull_out.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index fb99d04f89e91..528eae0d79a62 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -292,7 +292,6 @@ bool GeometricParallelParking::planPullOut( // set pull_out velocity to arc paths and 0 velocity to end point constexpr bool set_stop_end = false; setVelocityToArcPaths(arc_paths, parameters_.pull_out_velocity, set_stop_end); - arc_paths.back().points.front().point.longitudinal_velocity_mps = 0.0; // combine the road center line path with the second arc path auto paths = arc_paths; diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index f51f2ddd7c5ab..261a297f859bf 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -65,11 +65,10 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); + // insert stop velocity to first arc path end + output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; } else { auto partial_paths = planner_.getPaths(); - // remove stop velocity of first arc path - partial_paths.front().points.back().point.longitudinal_velocity_mps = - parallel_parking_parameters_.pull_out_velocity; const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); }