fix(start_planner): fix geometric pull out stop point (autowarefounda…

…tion#3994)

Signed-off-by: kosuke55 <[email protected]>

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;

planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp

@@ -65,11 +65,10 @@ boost::optional<PullOutPath> 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);
   }