diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index b092eb7fc8802..bde6daa42d231 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -16,6 +16,7 @@ deceleration_interval: 15.0 # geometric pull out enable_geometric_pull_out: true + divide_pull_out_path: false geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index c206b39c5fdf6..fca7d5d2bfc14 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -38,6 +38,7 @@ struct PullOutParameters double deceleration_interval; // geometric pull out bool enable_geometric_pull_out; + bool divide_pull_out_path; double geometric_pull_out_velocity; double arc_path_interval; double lane_departure_margin; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bd6fe13254a5d..72a456a03f77f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -643,6 +643,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.deceleration_interval = dp("deceleration_interval", 10.0); // geometric pull out p.enable_geometric_pull_out = dp("enable_geometric_pull_out", true); + p.divide_pull_out_path = dp("divide_pull_out_path", false); p.geometric_pull_out_velocity = dp("geometric_pull_out_velocity", 1.0); p.arc_path_interval = dp("arc_path_interval", 1.0); p.lane_departure_margin = dp("lane_departure_margin", 0.2); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp index a080eefdf64f7..2316ac35c2ef5 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/geometric_pull_out.cpp @@ -59,7 +59,17 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p return {}; } - output.partial_paths = planner_.getPaths(); + if (parameters_.divide_pull_out_path) { + output.partial_paths = planner_.getPaths(); + } else { + const auto partial_paths = planner_.getPaths(); + auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); + // set same velocity to all points not to stop + for (auto & point : combined_path.points) { + point.point.longitudinal_velocity_mps = parameters_.geometric_pull_out_velocity; + } + output.partial_paths.push_back(combined_path); + } output.start_pose = planner_.getArcPaths().at(0).points.back().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose;