diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 625cd6ab74ff7..f16799d7ea206 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -5,6 +5,7 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. + center_line_path_interval: 1.0 # goal search goal_search: diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index a529eefac67af..6760ec787bb03 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -8,6 +8,7 @@ collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 th_distance_to_middle_of_the_road: 0.1 + center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 18b77f3faacf0..44abdb4267672 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -118,11 +118,12 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## **collision check** diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 0b0e01378b61a..8322952a0be76 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -70,6 +70,7 @@ PullOutPath --o PullOutPlannerBase | length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | | collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | | collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index ef1301beccf00..c2d36fdd6e0d7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -46,6 +46,9 @@ using geometry_msgs::msg::PoseArray; struct ParallelParkingParameters { + // common + double center_line_path_interval{0.0}; + // forward parking double after_forward_parking_straight_distance{0.0}; double forward_parking_velocity{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 22c65270c39ea..8ef426d4f2488 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -45,6 +45,7 @@ struct GoalPlannerParameters double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; double th_blinker_on_lateral_offset{0.0}; + double center_line_path_interval{0.0}; // goal search std::string search_priority; // "efficient_path" or "close_goal" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp index 445161f631bb7..dccaed3184e77 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp @@ -57,7 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_{}; - static constexpr double resample_interval_{1.0}; bool left_side_parking_{true}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 0842d0a17bd97..78a64feb2c1ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -41,9 +41,7 @@ class ShiftPullOut : public PullOutPlannerBase std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::StartPlannerParameters & parameter); + const Pose & start_pose, const Pose & goal_pose); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index e2d80759a18d0..6978a7d494f30 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -45,6 +45,7 @@ struct StartPlannerParameters double collision_check_margin{0.0}; double collision_check_distance_from_end{0.0}; double th_moving_object_velocity{0.0}; + double center_line_path_interval{0.0}; // shift pull out bool enable_shift_pull_out{false}; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index da1b8bf5792ee..6a8f7bc1ea8ca 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -38,6 +38,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); + p.center_line_path_interval = + node->declare_parameter(base_ns + "center_line_path_interval"); } // goal search @@ -121,6 +123,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(ns + "after_shift_straight_distance"); } + // parallel parking common + { + const std::string ns = base_ns + "pull_over.parallel_parking."; + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometirc parallel parking + } + // forward parallel parking forward { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index e00a1041e5316..a461ce1e2aea9 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -47,6 +47,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = @@ -71,6 +72,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "lane_departure_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometirc parallel parking // search start pose backward p.search_priority = node->declare_parameter( ns + "search_priority"); // "efficient_path" or "short_back_distance" 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 6b1a4a82b314e..3d315e7213f96 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 @@ -286,8 +286,9 @@ bool GeometricParallelParking::planPullOut( s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; - PathWithLaneId road_center_line_path = - planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + const PathWithLaneId road_center_line_path = utils::resamplePathWithSpline( + planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true), + parameters_.center_line_path_interval); if (road_center_line_path.points.empty()) { continue; @@ -362,8 +363,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( const Pose current_pose = planner_data_->self_odometry->pose.pose; const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose); - auto path = planner_data_->route_handler->getCenterLinePath( - road_lanes, current_arc_position.length, start_arc_position.length, true); + auto path = utils::resamplePathWithSpline( + planner_data_->route_handler->getCenterLinePath( + road_lanes, current_arc_position.length, start_arc_position.length, true), + parameters_.center_line_path_interval); path.header = planner_data_->route_handler->getRouteHeader(); if (!path.points.empty()) { path.points.back().point.longitudinal_velocity_mps = 0; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index d6a3746187b11..1a690362fbe25 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -112,7 +112,7 @@ boost::optional ShiftPullOver::generatePullOverPath( // generate road lane reference path to shift end const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( - generateReferencePath(road_lanes, shift_end_pose), resample_interval_); + generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); // calculate shift length const Pose & shift_end_pose_road_lane = diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index 36fb6381ac967..8e5d6b7545c1d 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -96,7 +96,9 @@ boost::optional FreespacePullOut::plan(const Pose & start_pose, con const bool path_terminal_is_goal = path_end_info.second; const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); - last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + last_path = utils::resamplePathWithSpline( + start_planner_utils::combineReferencePath(last_path, road_center_line_path), + parameters_.center_line_path_interval); const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; utils::correctDividedPathVelocity(partial_paths); diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 6225ce598afe3..a4e4a2378e8f1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -59,8 +59,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths( - *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_); + auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { return boost::none; } @@ -160,8 +159,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter) + const Pose & start_pose, const Pose & goal_pose) { std::vector candidate_paths{}; @@ -170,12 +168,16 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const double forward_path_length = common_parameter.forward_path_length; - const double backward_path_length = common_parameter.backward_path_length; - const double lateral_jerk = parameter.lateral_jerk; - const double minimum_lateral_acc = parameter.minimum_lateral_acc; - const double maximum_lateral_acc = parameter.maximum_lateral_acc; - const int lateral_acceleration_sampling_num = parameter.lateral_acceleration_sampling_num; + const auto & common_parameters = planner_data_->parameters; + const double forward_path_length = common_parameters.forward_path_length; + const double backward_path_length = common_parameters.backward_path_length; + const double lateral_jerk = parameters_.lateral_jerk; + const double minimum_lateral_acc = parameters_.minimum_lateral_acc; + const double maximum_lateral_acc = parameters_.maximum_lateral_acc; + const double maximum_curvature = parameters_.maximum_curvature; + const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance; + const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num; + // set minimum acc for breaking loop when sampling num is 1 const double acc_resolution = std::max( std::abs(maximum_lateral_acc - minimum_lateral_acc) / lateral_acceleration_sampling_num, @@ -189,9 +191,9 @@ std::vector ShiftPullOut::calcPullOutPaths( const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; - constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( - route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL); + route_handler.getCenterLinePath(road_lanes, s_start, s_end), + parameters_.center_line_path_interval); // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { @@ -230,8 +232,8 @@ std::vector ShiftPullOut::calcPullOutPaths( PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0); const auto pull_out_distance = calcPullOutLongitudinalDistance( - longitudinal_acc, shift_time, shift_length, parameter.maximum_curvature, - parameter.minimum_shift_pull_out_distance); + longitudinal_acc, shift_time, shift_length, maximum_curvature, + minimum_shift_pull_out_distance); const double terminal_velocity = longitudinal_acc * shift_time; // clip from ego pose @@ -253,7 +255,9 @@ std::vector ShiftPullOut::calcPullOutPaths( std::max(pull_out_distance, pull_out_distance_converted); // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted - if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) { + if ( + before_shifted_pull_out_distance < parameters_.center_line_path_interval && + !has_non_shifted_path) { candidate_paths.push_back(non_shifted_path); has_non_shifted_path = true; continue;