From be12c107ad3e9e47dcb7fe07bed720831b051104 Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Wed, 23 Oct 2024 11:59:09 +0900 Subject: [PATCH] refactor(lane_change): refactor longitudinal acceleration sampling (#9091) * fix calc_all_max_lc_lengths function Signed-off-by: mohammad alqudah * remove unused functions Signed-off-by: mohammad alqudah * remove limit on velocity in calc_all_max_lc_lengths function Signed-off-by: mohammad alqudah * sample longitudinal acceleration separately for each prepater duration Signed-off-by: mohammad alqudah * refactor prepare phase metrics calculation Signed-off-by: mohammad alqudah * check for zero value prepare duration Signed-off-by: mohammad alqudah * refactor calc_lon_acceleration_samples function Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../scene.hpp | 11 - .../utils/calculation.hpp | 19 +- .../utils/data_structs.hpp | 4 +- .../utils/utils.hpp | 11 - .../src/scene.cpp | 145 ++--------- .../src/utils/calculation.cpp | 225 ++++++++++++------ .../src/utils/utils.cpp | 48 ---- 7 files changed, 179 insertions(+), 284 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 4dec256a65703..5334af7ab6e8e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -120,12 +120,6 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_terminal_turn_signal_info() const final; - std::vector sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; - - std::vector calc_prepare_durations() const; - lane_change::TargetObjects get_target_objects( const FilteredByLanesExtendedObjects & filtered_objects, const lanelet::ConstLanelets & current_lanes) const; @@ -206,11 +200,6 @@ class NormalLaneChange : public LaneChangeBase bool check_prepare_phase() const; - double calcMaximumLaneChangeLength( - const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; - - std::pair calcCurrentMinMaxAcceleration() const; - void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); void updateStopTime(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 233fc886f836f..b6e6cb968ace8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -27,6 +27,8 @@ using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::MinMaxValue; using behavior_path_planner::lane_change::PhaseMetrics; +static constexpr double eps = 0.001; + double calc_dist_from_pose_to_terminal_end( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, const Pose & src_pose); @@ -103,14 +105,6 @@ double calc_ego_dist_to_lanes_start( const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes); -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc); - -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc); - double calc_lane_changing_acceleration( const double initial_lane_changing_velocity, const double max_path_velocity, const double lane_changing_time, const double prepare_longitudinal_acc); @@ -132,10 +126,13 @@ double calc_phase_length( const double velocity, const double maximum_velocity, const double acceleration, const double duration); +std::vector calc_lon_acceleration_samples( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration); + std::vector calc_prepare_phase_metrics( - const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, - const std::vector & lon_accel_values, const double current_velocity, - const double min_length_threshold = 0.0, + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double max_path_velocity, const double min_length_threshold = 0.0, const double max_length_threshold = std::numeric_limits::max()); std::vector calc_shift_phase_metrics( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index e4f70f54a8437..5178d76aca29e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -323,7 +323,6 @@ struct MinMaxValue struct TransientData { - MinMaxValue acc; // acceleration profile for accelerating lane change path MinMaxValue lane_changing_length; // lane changing length for a single lane change MinMaxValue current_dist_buffer; // distance buffer computed backward from current lanes' terminal end @@ -343,6 +342,9 @@ struct TransientData lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes + size_t current_path_seg_idx; // index of nearest segment to ego along current path + double current_path_velocity; // velocity of the current path at the ego position along the path + bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index b6c234fd7035d..18fa418a0970d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -64,20 +64,9 @@ bool is_mandatory_lane_change(const ModuleType lc_type); double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); -double calcMinimumAcceleration( - const double current_velocity, const double min_longitudinal_acc, - const LaneChangeParameters & lane_change_parameters); - -double calcMaximumAcceleration( - const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector getAccelerationValues( - const double min_acc, const double max_acc, const size_t sampling_num); - std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 6f71ffff734a0..36b90dbf10ffa 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -114,7 +114,15 @@ void NormalLaneChange::update_transient_data() } auto & transient_data = common_data_ptr_->transient_data; - std::tie(transient_data.acc.min, transient_data.acc.max) = calcCurrentMinMaxAcceleration(); + + const auto & p = *common_data_ptr_->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_output_.path.points, common_data_ptr_->get_ego_pose(), + p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); + transient_data.current_path_velocity = + prev_module_output_.path.points.at(nearest_seg_idx).point.longitudinal_velocity_mps; + transient_data.current_path_seg_idx = nearest_seg_idx; std::tie(transient_data.lane_changing_length, transient_data.current_dist_buffer) = calculation::calc_lc_length_and_dist_buffer(common_data_ptr_, get_current_lanes()); @@ -145,8 +153,6 @@ void NormalLaneChange::update_transient_data() updateStopTime(); transient_data.is_ego_stuck = is_ego_stuck(); - RCLCPP_DEBUG( - logger_, "acc - min: %.4f, max: %.4f", transient_data.acc.min, transient_data.acc.max); RCLCPP_DEBUG( logger_, "lane_changing_length - min: %.4f, max: %.4f", transient_data.lane_changing_length.min, transient_data.lane_changing_length.max); @@ -308,11 +314,9 @@ TurnSignalInfo NormalLaneChange::get_terminal_turn_signal_info() const const auto terminal_turn_signal_info = get_turn_signal(*start_pose, getLaneChangePath().info.lane_changing_end); - const double nearest_dist_threshold = common_param.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; - const size_t current_nearest_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const auto nearest_dist_threshold = common_param.ego_nearest_dist_threshold; + const auto nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; + const auto current_nearest_seg_idx = common_data_ptr_->transient_data.current_path_seg_idx; return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, @@ -861,6 +865,7 @@ bool NormalLaneChange::isAbortState() const lane_change_debug_.is_abort = true; return true; } + int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) const { const auto get_opposite_direction = @@ -868,114 +873,6 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } -std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const -{ - const auto & p = getCommonParam(); - - const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); - - const auto ego_seg_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_output_.path.points, getEgoPose(), p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold); - const auto max_path_velocity = - prev_module_output_.path.points.at(ego_seg_idx).point.longitudinal_velocity_mps; - - // calculate minimum and maximum acceleration - const auto min_acc = utils::lane_change::calcMinimumAcceleration( - getEgoVelocity(), vehicle_min_acc, *lane_change_parameters_); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); - - return {min_acc, max_acc}; -} - -std::vector NormalLaneChange::sampleLongitudinalAccValues( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const -{ - // TODO(Azu): sampler should work even when we're not approaching terminal - if (prev_module_output_.path.points.empty()) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - const auto current_pose = getEgoPose(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - - const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); - - // if max acc is not positive, then we do the normal sampling - if (max_acc <= 0.0) { - RCLCPP_DEBUG( - logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // calculate maximum lane change length - // TODO(Azu) Double check why it's failing with transient data - const auto current_max_dist_buffer = - calculation::calc_maximum_lane_change_length(common_data_ptr_, current_lanes.back(), max_acc); - - if (current_max_dist_buffer > common_data_ptr_->transient_data.dist_to_terminal_end) { - RCLCPP_DEBUG( - logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, - max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (common_data_ptr_->transient_data.is_ego_stuck) { - auto clock = rclcpp::Clock(RCL_ROS_TIME); - RCLCPP_INFO_THROTTLE( - logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, - max_acc); - return utils::lane_change::getAccelerationValues( - min_acc, max_acc, longitudinal_acc_sampling_num); - } - - // if maximum lane change length is less than length to goal or the end of target lanes, only - // sample max acc - if (route_handler.isInGoalRouteSection(target_lanes.back())) { - const auto goal_pose = route_handler.getGoalPose(); - if (current_max_dist_buffer < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { - RCLCPP_DEBUG( - logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); - return {max_acc}; - } - } else if (current_max_dist_buffer < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { - RCLCPP_DEBUG( - logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); - return {max_acc}; - } - - RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); - return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); -} - -std::vector NormalLaneChange::calc_prepare_durations() const -{ - const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto threshold = common_data_ptr_->bpp_param_ptr->base_link2front + - lc_param_ptr->min_length_for_turn_signal_activation; - const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; - - // TODO(Azu) this check seems to cause scenario failures. - if (common_data_ptr_->transient_data.dist_to_terminal_start >= threshold) { - return {max_prepare_duration}; - } - - std::vector prepare_durations; - constexpr double step = 0.5; - - for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { - prepare_durations.push_back(duration); - } - - return prepare_durations; -} - bool NormalLaneChange::get_prepare_segment( PathWithLaneId & prepare_segment, const double prepare_length) const { @@ -1296,22 +1193,14 @@ std::vector NormalLaneChange::get_prepare_metrics() cons const auto & current_lanes = common_data_ptr_->lanes_ptr->current; const auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto current_velocity = getEgoVelocity(); - - // get sampling acceleration values - const auto longitudinal_acc_sampling_values = - sampleLongitudinalAccValues(current_lanes, target_lanes); - - const auto prepare_durations = calc_prepare_durations(); - - RCLCPP_DEBUG( - logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", - prepare_durations.size(), longitudinal_acc_sampling_values.size()); + // set speed limit to be current path velocity; + const auto max_path_velocity = common_data_ptr_->transient_data.current_path_velocity; const auto dist_to_target_start = calculation::calc_ego_dist_to_lanes_start(common_data_ptr_, current_lanes, target_lanes); return calculation::calc_prepare_phase_metrics( - common_data_ptr_, prepare_durations, longitudinal_acc_sampling_values, current_velocity, - dist_to_target_start, common_data_ptr_->transient_data.dist_to_terminal_start); + common_data_ptr_, current_velocity, max_path_velocity, dist_to_target_start, + common_data_ptr_->transient_data.dist_to_terminal_start); } std::vector NormalLaneChange::get_lane_changing_metrics( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 762cada4e59d2..741a5d7b4761d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -141,59 +141,26 @@ double calc_ego_dist_to_lanes_start( return motion_utils::calcSignedArcLength(path.points, ego_position, target_front_pt); } -double calc_maximum_lane_change_length( - const double current_velocity, const LaneChangeParameters & lane_change_parameters, - const std::vector & shift_intervals, const double max_acc) +double calc_minimum_acceleration( + const LaneChangeParameters & lane_change_parameters, const double current_velocity, + const double min_acc_threshold, const double prepare_duration) { - if (shift_intervals.empty()) { - return 0.0; - } - - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto t_prepare = lane_change_parameters.lane_change_prepare_duration; - - auto vel = current_velocity; - - const auto calc_sum = [&](double sum, double shift_interval) { - // prepare section - const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; - vel = vel + max_acc * t_prepare; - - // lane changing section - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(vel); - const auto t_lane_changing = - autoware::motion_utils::calc_shift_time_from_jerk(shift_interval, lat_jerk, max_lat_acc); - const auto lane_changing_length = - vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; - - vel = vel + max_acc * t_lane_changing; - return sum + (prepare_length + lane_changing_length + finish_judge_buffer); - }; - - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); + if (prepare_duration < eps) return -std::abs(min_acc_threshold); + const double min_lc_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double acc = (min_lc_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, -std::abs(min_acc_threshold), -eps); } -double calc_maximum_lane_change_length( - const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet, - const double max_acc) +double calc_maximum_acceleration( + const double prepare_duration, const double current_velocity, const double current_max_velocity, + const double max_acc_threshold) { - const auto shift_intervals = - common_data_ptr->route_handler_ptr->getLateralIntervalsToPreferredLane( - current_terminal_lanelet); - const auto vel = std::max( - common_data_ptr->get_ego_speed(), - common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity); - return calc_maximum_lane_change_length( - vel, *common_data_ptr->lc_param_ptr, shift_intervals, max_acc); + if (prepare_duration < eps) return max_acc_threshold; + const double acc = (current_max_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, 0.0, max_acc_threshold); } -std::vector calc_all_min_lc_lengths( +std::vector calc_min_lane_change_lengths( const LCParamPtr & lc_param_ptr, const std::vector & shift_intervals) { if (shift_intervals.empty()) { @@ -221,7 +188,7 @@ std::vector calc_all_min_lc_lengths( return min_lc_lengths; } -std::vector calc_all_max_lc_lengths( +std::vector calc_max_lane_change_lengths( const CommonDataPtr & common_data_ptr, const std::vector & shift_intervals) { if (shift_intervals.empty()) { @@ -231,21 +198,28 @@ std::vector calc_all_max_lc_lengths( const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto lat_jerk = lc_param_ptr->lane_changing_lateral_jerk; const auto t_prepare = lc_param_ptr->lane_change_prepare_duration; - const auto max_acc = common_data_ptr->transient_data.acc.max; + const auto current_velocity = common_data_ptr->get_ego_speed(); + const auto path_velocity = common_data_ptr->transient_data.current_path_velocity; - const auto limit_vel = [&](const auto vel) { - const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; - return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); - }; + const auto max_acc = calc_maximum_acceleration( + t_prepare, current_velocity, path_velocity, lc_param_ptr->max_longitudinal_acc); + + // TODO(Quda, Azu): should probably limit upper bound of velocity as well, but + // disabled due failing evaluation tests. + // const auto limit_vel = [&](const auto vel) { + // const auto max_global_vel = common_data_ptr->bpp_param_ptr->max_vel; + // return std::clamp(vel, lc_param_ptr->minimum_lane_changing_velocity, max_global_vel); + // }; - auto vel = limit_vel(common_data_ptr->get_ego_speed()); + auto vel = + std::max(common_data_ptr->get_ego_speed(), lc_param_ptr->minimum_lane_changing_velocity); std::vector max_lc_lengths{}; const auto max_lc_length = [&](const auto shift_interval) { // prepare section - vel = limit_vel(vel + max_acc * t_prepare); const auto prepare_length = vel * t_prepare + 0.5 * max_acc * t_prepare * t_prepare; + vel = vel + max_acc * t_prepare; // lane changing section const auto [min_lat_acc, max_lat_acc] = lc_param_ptr->lane_change_lat_acc_map.find(vel); @@ -254,7 +228,7 @@ std::vector calc_all_max_lc_lengths( const auto lane_changing_length = vel * t_lane_changing + 0.5 * max_acc * t_lane_changing * t_lane_changing; - vel = limit_vel(vel + max_acc * t_lane_changing); + vel = vel + max_acc * t_lane_changing; return prepare_length + lane_changing_length; }; @@ -264,17 +238,16 @@ std::vector calc_all_max_lc_lengths( return max_lc_lengths; } -double calc_distance_buffer( - const LCParamPtr & lc_param_ptr, const std::vector & min_lc_lengths) +double calc_distance_buffer(const LCParamPtr & lc_param_ptr, const std::vector & lc_lengths) { - if (min_lc_lengths.empty()) { + if (lc_lengths.empty()) { return std::numeric_limits::max(); } const auto finish_judge_buffer = lc_param_ptr->lane_change_finish_judge_buffer; const auto backward_buffer = calc_stopping_distance(lc_param_ptr); - const auto lengths_sum = std::accumulate(min_lc_lengths.begin(), min_lc_lengths.end(), 0.0); - const auto num_of_lane_changes = static_cast(min_lc_lengths.size()); + const auto lengths_sum = std::accumulate(lc_lengths.begin(), lc_lengths.end(), 0.0); + const auto num_of_lane_changes = static_cast(lc_lengths.size()); return lengths_sum + (num_of_lane_changes * finish_judge_buffer) + ((num_of_lane_changes - 1.0) * backward_buffer); } @@ -299,19 +272,19 @@ std::pair calc_lc_length_and_dist_buffer( return {}; } const auto shift_intervals = calculation::calc_shift_intervals(common_data_ptr, lanes); - const auto all_min_lc_lengths = - calculation::calc_all_min_lc_lengths(common_data_ptr->lc_param_ptr, shift_intervals); + const auto min_lc_lengths = + calculation::calc_min_lane_change_lengths(common_data_ptr->lc_param_ptr, shift_intervals); const auto min_lc_length = - !all_min_lc_lengths.empty() ? all_min_lc_lengths.front() : std::numeric_limits::max(); + !min_lc_lengths.empty() ? min_lc_lengths.front() : std::numeric_limits::max(); const auto min_dist_buffer = - calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_min_lc_lengths); + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, min_lc_lengths); - const auto all_max_lc_lengths = - calculation::calc_all_max_lc_lengths(common_data_ptr, shift_intervals); + const auto max_lc_lengths = + calculation::calc_max_lane_change_lengths(common_data_ptr, shift_intervals); const auto max_lc_length = - !all_max_lc_lengths.empty() ? all_max_lc_lengths.front() : std::numeric_limits::max(); + !max_lc_lengths.empty() ? max_lc_lengths.front() : std::numeric_limits::max(); const auto max_dist_buffer = - calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, all_max_lc_lengths); + calculation::calc_distance_buffer(common_data_ptr->lc_param_ptr, max_lc_lengths); return {{min_lc_length, max_lc_length}, {min_dist_buffer, max_dist_buffer}}; } @@ -326,6 +299,85 @@ double calc_phase_length( return std::min(length_with_acceleration, length_with_max_velocity); } +std::pair calc_min_max_acceleration( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration) +{ + const auto & lc_params = *common_data_ptr->lc_param_ptr; + const auto & bpp_params = *common_data_ptr->bpp_param_ptr; + const auto current_ego_velocity = common_data_ptr->get_ego_speed(); + + const auto min_accel_threshold = std::max(bpp_params.min_acc, lc_params.min_longitudinal_acc); + const auto max_accel_threshold = std::min(bpp_params.max_acc, lc_params.max_longitudinal_acc); + + // calculate minimum and maximum acceleration + const auto min_acc = calc_minimum_acceleration( + lc_params, current_ego_velocity, min_accel_threshold, prepare_duration); + const auto max_acc = calc_maximum_acceleration( + prepare_duration, current_ego_velocity, max_path_velocity, max_accel_threshold); + + return {min_acc, max_acc}; +} + +std::vector calc_acceleration_values( + const double min_accel, const double max_accel, const double sampling_num) +{ + if (min_accel > max_accel) return {}; + + if (max_accel - min_accel < eps) { + return {min_accel}; + } + + const auto resolution = std::abs(max_accel - min_accel) / sampling_num; + + std::vector sampled_values{min_accel}; + for (double accel = min_accel + resolution; accel < max_accel + eps; accel += resolution) { + // check whether if we need to add 0.0 + if (sampled_values.back() < -eps && accel > eps) { + sampled_values.push_back(0.0); + } + + sampled_values.push_back(accel); + } + std::reverse(sampled_values.begin(), sampled_values.end()); + + return sampled_values; +} + +std::vector calc_lon_acceleration_samples( + const CommonDataPtr & common_data_ptr, const double max_path_velocity, + const double prepare_duration) +{ + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_pose = common_data_ptr->get_ego_pose(); + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto goal_pose = common_data_ptr->route_handler_ptr->getGoalPose(); + const auto sampling_num = common_data_ptr->lc_param_ptr->longitudinal_acc_sampling_num; + + const auto [min_accel, max_accel] = + calc_min_max_acceleration(common_data_ptr, max_path_velocity, prepare_duration); + + const auto is_sampling_required = std::invoke([&]() -> bool { + if (max_accel < 0.0 || transient_data.is_ego_stuck) return true; + + const auto max_dist_buffer = transient_data.current_dist_buffer.max; + if (max_dist_buffer > transient_data.dist_to_terminal_end) return true; + + const auto dist_to_target_lane_end = + common_data_ptr->lanes_ptr->target_lane_in_goal_section + ? utils::getSignedDistance(current_pose, goal_pose, target_lanes) + : utils::getDistanceToEndOfLane(current_pose, target_lanes); + + return max_dist_buffer >= dist_to_target_lane_end; + }); + + if (is_sampling_required) { + return calc_acceleration_values(min_accel, max_accel, sampling_num); + } + + return {max_accel}; +} + double calc_lane_changing_acceleration( const double initial_lane_changing_velocity, const double max_path_velocity, const double lane_changing_time, const double prepare_longitudinal_acc) @@ -339,10 +391,32 @@ double calc_lane_changing_acceleration( prepare_longitudinal_acc); } +std::vector calc_prepare_durations(const CommonDataPtr & common_data_ptr) +{ + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto threshold = common_data_ptr->bpp_param_ptr->base_link2front + + lc_param_ptr->min_length_for_turn_signal_activation; + const auto max_prepare_duration = lc_param_ptr->lane_change_prepare_duration; + + // TODO(Azu) this check seems to cause scenario failures. + if (common_data_ptr->transient_data.dist_to_terminal_start >= threshold) { + return {max_prepare_duration}; + } + + std::vector prepare_durations; + constexpr double step = 0.5; + + for (double duration = max_prepare_duration; duration >= 0.0; duration -= step) { + prepare_durations.push_back(duration); + } + + return prepare_durations; +} + std::vector calc_prepare_phase_metrics( - const CommonDataPtr & common_data_ptr, const std::vector & prepare_durations, - const std::vector & lon_accel_values, const double current_velocity, - const double min_length_threshold, const double max_length_threshold) + const CommonDataPtr & common_data_ptr, const double current_velocity, + const double max_path_velocity, const double min_length_threshold, + const double max_length_threshold) { const auto & min_lc_vel = common_data_ptr->lc_param_ptr->minimum_lane_changing_velocity; const auto & max_vel = common_data_ptr->bpp_param_ptr->max_vel; @@ -360,9 +434,12 @@ std::vector calc_prepare_phase_metrics( return false; }; - metrics.reserve(prepare_durations.size() * lon_accel_values.size()); + const auto prepare_durations = calc_prepare_durations(common_data_ptr); + for (const auto & prepare_duration : prepare_durations) { - for (const auto & lon_accel : lon_accel_values) { + const auto lon_accel_samples = + calc_lon_acceleration_samples(common_data_ptr, max_path_velocity, prepare_duration); + for (const auto & lon_accel : lon_accel_samples) { const auto prepare_velocity = std::clamp(current_velocity + lon_accel * prepare_duration, min_lc_vel, max_vel); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index b8d34afeaab54..c900b7fe54302 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -89,25 +89,6 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMinimumAcceleration( - const double current_velocity, const double min_longitudinal_acc, - const LaneChangeParameters & lane_change_parameters) -{ - const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; - const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); -} - -double calcMaximumAcceleration( - const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) -{ - const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; - const double acc = (current_max_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, 0.0, max_longitudinal_acc); -} - void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { @@ -125,35 +106,6 @@ void setPrepareVelocity( } } -std::vector getAccelerationValues( - const double min_acc, const double max_acc, const size_t sampling_num) -{ - if (min_acc > max_acc) { - return {}; - } - - if (max_acc - min_acc < std::numeric_limits::epsilon()) { - return {min_acc}; - } - - constexpr double epsilon = 0.001; - const auto resolution = std::abs(max_acc - min_acc) / static_cast(sampling_num); - - std::vector sampled_values{min_acc}; - for (double sampled_acc = min_acc + resolution; - sampled_acc < max_acc + std::numeric_limits::epsilon(); sampled_acc += resolution) { - // check whether if we need to add 0.0 - if (sampled_values.back() < -epsilon && sampled_acc > epsilon) { - sampled_values.push_back(0.0); - } - - sampled_values.push_back(sampled_acc); - } - std::reverse(sampled_values.begin(), sampled_values.end()); - - return sampled_values; -} - lanelet::ConstLanelets get_target_neighbor_lanes( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType & type)