From 7218731bd95fd912c759ad79cca5164600ae2ca8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Fri, 16 Jun 2023 22:51:01 +0900 Subject: [PATCH] feat(start_planner): start with acceleration Signed-off-by: kosuke55 --- .../start_planner/start_planner.param.yaml | 8 +- .../start_planner_parameters.hpp | 8 +- .../src/behavior_path_planner_node.cpp | 15 ++-- .../utils/start_planner/shift_pull_out.cpp | 79 ++++++++++++------- 4 files changed, 68 insertions(+), 42 deletions(-) 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 0d8782cb501b6..9a8b2ff9735d3 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 @@ -9,12 +9,12 @@ collision_check_distance_from_end: 1.0 # shift pull out enable_shift_pull_out: true - shift_pull_out_velocity: 2.0 - pull_out_sampling_num: 4 minimum_shift_pull_out_distance: 0.0 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.1 deceleration_interval: 15.0 + lateral_jerk: 0.5 + lateral_acceleration_sampling_num: 1 + minimum_lateral_acc: 0.15 + maximum_lateral_acc: 0.5 # geometric pull out enable_geometric_pull_out: true divide_pull_out_path: false 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 e76cfcfd88081..6d896852f4de2 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 @@ -33,11 +33,11 @@ struct StartPlannerParameters double collision_check_distance_from_end; // shift pull out bool enable_shift_pull_out; - double shift_pull_out_velocity; - int pull_out_sampling_num; double minimum_shift_pull_out_distance; - double maximum_lateral_jerk; - double minimum_lateral_jerk; + int lateral_acceleration_sampling_num; + double lateral_jerk; + double maximum_lateral_acc; + double minimum_lateral_acc; double deceleration_interval; // geometric pull out bool enable_geometric_pull_out; 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 5ec8c6d229e67..47f12b595428f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1079,12 +1079,13 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() declare_parameter(ns + "collision_check_distance_from_end"); // shift pull out p.enable_shift_pull_out = declare_parameter(ns + "enable_shift_pull_out"); - p.shift_pull_out_velocity = declare_parameter(ns + "shift_pull_out_velocity"); - p.pull_out_sampling_num = declare_parameter(ns + "pull_out_sampling_num"); p.minimum_shift_pull_out_distance = declare_parameter(ns + "minimum_shift_pull_out_distance"); - p.maximum_lateral_jerk = declare_parameter(ns + "maximum_lateral_jerk"); - p.minimum_lateral_jerk = declare_parameter(ns + "minimum_lateral_jerk"); + p.lateral_acceleration_sampling_num = + declare_parameter(ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = declare_parameter(ns + "lateral_jerk"); + p.maximum_lateral_acc = declare_parameter(ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = declare_parameter(ns + "minimum_lateral_acc"); p.deceleration_interval = declare_parameter(ns + "deceleration_interval"); // geometric pull out p.enable_geometric_pull_out = declare_parameter(ns + "enable_geometric_pull_out"); @@ -1108,10 +1109,10 @@ StartPlannerParameters BehaviorPathPlannerNode::getStartPlannerParam() p.ignore_distance_from_lane_end = declare_parameter(ns + "ignore_distance_from_lane_end"); // validation of parameters - if (p.pull_out_sampling_num < 1) { + if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( - get_logger(), "pull_out_sampling_num must be positive integer. Given parameter: " - << p.pull_out_sampling_num << std::endl + get_logger(), "lateral_acceleration_sampling_num must be positive integer. Given parameter: " + << p.lateral_acceleration_sampling_num << std::endl << "Terminating the program..."); exit(EXIT_FAILURE); } 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 42f419d6a641f..7d8448802b847 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 @@ -136,13 +136,15 @@ std::vector ShiftPullOut::calcPullOutPaths( // rename parameter const double backward_path_length = common_parameter.backward_path_length; - const double shift_pull_out_velocity = parameter.shift_pull_out_velocity; const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; - const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; - const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; - const int pull_out_sampling_num = parameter.pull_out_sampling_num; - const double jerk_resolution = - std::abs(maximum_lateral_jerk - minimum_lateral_jerk) / pull_out_sampling_num; + 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; + // 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, + std::numeric_limits::epsilon()); // generate road lane reference path const auto arc_position_start = getArcCoordinates(road_lanes, start_pose); @@ -155,43 +157,56 @@ std::vector ShiftPullOut::calcPullOutPaths( // if goal is behind start pose, const bool goal_is_behind = arc_position_goal.length < s_start; const double s_end = goal_is_behind ? road_lanes_length : arc_position_goal.length; - PathWithLaneId road_lane_reference_path = - utils::resamplePathWithSpline(route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); + constexpr double RESAMPLE_INTERVAL = 1.0; + PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( + route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL); + + // non_shifted_path for when shift length or pull out distance is too short + const PullOutPath non_shifted_path = std::invoke([&]() { + PullOutPath non_shifted_path{}; + non_shifted_path.partial_paths.push_back(road_lane_reference_path); + non_shifted_path.start_pose = start_pose; + non_shifted_path.end_pose = start_pose; + return non_shifted_path; + }); bool has_non_shifted_path = false; - for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; - lateral_jerk += jerk_resolution) { + for (double lateral_acc = minimum_lateral_acc; lateral_acc <= maximum_lateral_acc; + lateral_acc += acc_resolution) { PathShifter path_shifter{}; + path_shifter.setPath(road_lane_reference_path); - // calculate after/before shifted pull out distance - // lateral distance from road center to start pose - constexpr double minimum_shift_length = 0.01; - const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; // if shift length is too short, add non sifted path - if (std::abs(shift_length) < minimum_shift_length && !has_non_shifted_path) { - PullOutPath non_shifted_path{}; - non_shifted_path.partial_paths.push_back(road_lane_reference_path); - non_shifted_path.start_pose = start_pose; - non_shifted_path.end_pose = start_pose; + constexpr double MINIMUM_SHIFT_LENGTH = 0.01; + const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + if (std::abs(shift_length) < MINIMUM_SHIFT_LENGTH && !has_non_shifted_path) { candidate_paths.push_back(non_shifted_path); has_non_shifted_path = true; continue; } - const double pull_out_distance = std::max( - PathShifter::calcLongitudinalDistFromJerk( - abs(shift_length), lateral_jerk, shift_pull_out_velocity), - minimum_shift_pull_out_distance); + // calculate pull out distance, longitudinal acc, terminal velocity const size_t shift_start_idx = findNearestIndex(road_lane_reference_path.points, start_pose.position); + const double road_velocity = + road_lane_reference_path.points.at(shift_start_idx).point.longitudinal_velocity_mps; + const double shift_time = + PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); + const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, 1.0); + const double pull_out_distance = (longitudinal_acc * std::pow(shift_time, 2)) / 2.0; + const double terminal_velocity = longitudinal_acc * shift_time; + + // clip from ego pose PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path; road_lane_reference_path_from_ego.points.erase( road_lane_reference_path_from_ego.points.begin(), road_lane_reference_path_from_ego.points.begin() + shift_start_idx); // before means distance on road lane - const double before_shifted_pull_out_distance = calcBeforeShiftedArcLength( - road_lane_reference_path_from_ego, pull_out_distance, shift_length); + const double before_shifted_pull_out_distance = std::max( + minimum_shift_pull_out_distance, + calcBeforeShiftedArcLength( + road_lane_reference_path_from_ego, pull_out_distance, shift_length)); // check has enough distance const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); @@ -201,6 +216,13 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } + // 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) { + candidate_paths.push_back(non_shifted_path); + has_non_shifted_path = true; + continue; + } + // get shift end pose const auto shift_end_pose_ptr = std::invoke([&]() { const auto arc_position_shift_start = @@ -223,6 +245,9 @@ std::vector ShiftPullOut::calcPullOutPaths( shift_line.end = *shift_end_pose_ptr; shift_line.end_shift_length = shift_length; path_shifter.addShiftLine(shift_line); + path_shifter.setVelocity(0.0); // initial velocity is 0 + path_shifter.setLongitudinalAcceleration(longitudinal_acc); + path_shifter.setLateralAccelerationLimit(lateral_acc); // offset front side ShiftedPath shifted_path; @@ -237,8 +262,8 @@ std::vector ShiftPullOut::calcPullOutPaths( for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < pull_out_end_idx) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(terminal_velocity)); } } // if the end point is the goal, set the velocity to 0