From c2d3bc2b39712a338cd989ef1a538cec376f2817 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 19 Jun 2023 21:53:52 +0900 Subject: [PATCH] feat(start_planner): start with acceleration (#4005) 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 | 65 ++++++++++++------- 4 files changed, 56 insertions(+), 40 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 cf5840d072e22..ae601844aecc4 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,12 +8,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 1c9d90b18b951..02a3b6169166a 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 @@ -35,11 +35,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 a1a130a557866..d192b4687e9fc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1067,12 +1067,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"); @@ -1096,10 +1097,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 75319b6ef3755..b11bb7731b826 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 @@ -141,13 +141,15 @@ 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 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); @@ -174,39 +176,42 @@ std::vector ShiftPullOut::calcPullOutPaths( }); 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()); @@ -216,6 +221,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 = @@ -238,6 +250,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; @@ -252,8 +267,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