From f478a0e8e40672939e752ed5abf383e72f959381 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 7 Dec 2023 17:01:47 +0900 Subject: [PATCH] fixup! refactor(avoidance): refactor decel profile generation logic --- .../behavior_path_avoidance_module/scene.hpp | 8 +- .../src/scene.cpp | 78 ++----------------- 2 files changed, 13 insertions(+), 73 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 1d4cb43b15978..4f48e38365ef5 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -243,8 +243,14 @@ class AvoidanceModule : public SceneModuleInterface */ void insertYieldVelocity(ShiftedPath & shifted_path) const; + /** + * @brief insert deceleration profile from shift length. + * @param shift length. + * @param decel section length. + * @param lower speed limit. + */ void insertDecelProfile( - PathWithLaneId & path, const double shift_length, const double to_stop_line, + PathWithLaneId & path, const double shift_length, const double decel_section_length, const double lower_speed) const; /** diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index afb32133b67ed..79fbb4a65bade 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1513,37 +1513,8 @@ void AvoidanceModule::insertReturnDeadLine( utils::avoidance::insertDecelPoint( getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); - // // insert slow down speed. - // const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( - // shift_length, helper_->getLateralMinJerkLimit(), to_stop_line); - // if (current_target_velocity < getEgoSpeed()) { - // RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); - // return; - // } - insertDecelProfile( shifted_path.path, shift_length, to_stop_line, parameters_->min_slow_down_speed); - - // const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); - // for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { - // const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); - - // // slow down speed is inserted only in front of the object. - // const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; - // if (shift_longitudinal_distance < 0.0) { - // break; - // } - - // // target speed with nominal jerk limits. - // const double v_target = PathShifter::calcFeasibleVelocityFromJerk( - // shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); - // const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; - // const double v_insert = - // std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed); - - // shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, - // v_insert); - // } } void AvoidanceModule::insertWaitPoint( @@ -1696,10 +1667,6 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto distance_to_object = object.longitudinal; const auto remaining_distance = distance_to_object - min_avoid_distance; const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; - // const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed); - // if (remaining_distance < decel_distance) { - // return; - // } if (distance_to_object < min_avoid_distance) { RCLCPP_DEBUG(getLogger(), "less than minimum avoidance distance."); return; @@ -1713,52 +1680,18 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const return; } - // // insert slow down speed. - // constexpr double BUFFER = 1.0; // [m/s] - // const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( - // shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); - // if (current_target_velocity + BUFFER < getEgoSpeed()) { - // utils::avoidance::insertDecelPoint( - // getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, - // slow_pose_); - // return; - // } - insertDecelProfile(shifted_path.path, shift_length, distance_to_object, lower_speed); - - // const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); - // for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { - // const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); - - // // slow down speed is inserted only in front of the object. - // const auto shift_longitudinal_distance = distance_to_object - distance_from_ego; - // if (shift_longitudinal_distance < min_avoid_distance) { - // break; - // } - - // // target speed with nominal jerk limits. - // const double v_target = PathShifter::calcFeasibleVelocityFromJerk( - // shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); - // const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; - // const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); - - // shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, - // v_insert); - // } - - // slow_pose_ = motion_utils::calcLongitudinalOffsetPose( - // shifted_path.path.points, start_idx, distance_to_object); } void AvoidanceModule::insertDecelProfile( - PathWithLaneId & path, const double shift_length, const double to_stop_line, + PathWithLaneId & path, const double shift_length, const double decel_section_length, const double lower_speed) const { - const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed); + const auto decel_distance = helper_->getFeasibleDecelDistance(lower_speed, false); // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( - shift_length, helper_->getLateralMinJerkLimit(), to_stop_line); + shift_length, helper_->getLateralMinJerkLimit(), decel_section_length); if (current_target_velocity < getEgoSpeed() + parameters_->buf_slow_down_speed) { utils::avoidance::insertDecelPoint( getEgoPosition(), decel_distance, lower_speed, path, slow_pose_); @@ -1770,7 +1703,7 @@ void AvoidanceModule::insertDecelProfile( const auto distance_from_ego = calcSignedArcLength(path.points, start_idx, i); // slow down speed is inserted only in front of the object. - const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; + const auto shift_longitudinal_distance = decel_section_length - distance_from_ego; if (shift_longitudinal_distance < 0.0) { break; } @@ -1784,7 +1717,8 @@ void AvoidanceModule::insertDecelProfile( path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); } - slow_pose_ = motion_utils::calcLongitudinalOffsetPose(path.points, start_idx, to_stop_line); + slow_pose_ = + motion_utils::calcLongitudinalOffsetPose(path.points, start_idx, decel_section_length); } std::shared_ptr AvoidanceModule::get_debug_msg_array() const