From d6ba61a7b4f146ce3ffd818e516e4e0629be96d3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 12 Jul 2023 18:59:53 +0900 Subject: [PATCH] feat(avoidance): output feasible avoidance path (#4145) feat(avoidance): output feasible shift path Signed-off-by: satoshi-ota --- .../utils/path_shifter/path_shifter.hpp | 3 + .../avoidance/avoidance_module.cpp | 169 +++++++++++------- .../src/utils/path_shifter/path_shifter.cpp | 16 +- 3 files changed, 119 insertions(+), 69 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index 93c3a4a768c0b..4b1115a049ab0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -139,6 +139,9 @@ class PathShifter static double calcFeasibleVelocityFromJerk( const double lateral, const double jerk, const double distance); + static double calcLateralDistFromJerk( + const double longitudinal, const double jerk, const double velocity); + static double calcLongitudinalDistFromJerk( const double lateral, const double jerk, const double velocity); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index bd04f76d58bd3..00d2f4b1f9ee4 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -836,15 +836,96 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - const auto prepare_distance = helper_.getNominalPrepareDistance(); - // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = helper_.getEgoShift(); const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & base_link2rear = planner_data_->parameters.base_link2rear; + // Calculate feasible shift length + const auto get_shift_length = + [&](auto & object, const auto & desire_shift_length) -> boost::optional { + // use each object param + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto is_object_on_right = utils::avoidance::isOnRight(object); + + // use absolute dist for return-to-center, relative dist from current for avoiding. + const auto avoiding_shift = desire_shift_length - current_ego_shift; + const auto nominal_avoid_distance = helper_.getNominalAvoidanceDistance(avoiding_shift); + + // ego already has enough positive shift. + const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; + if (is_object_on_right && has_enough_positive_shift) { + return desire_shift_length; + } + + // ego already has enough negative shift. + const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3; + if (!is_object_on_right && has_enough_negative_shift) { + return desire_shift_length; + } + + // calculate remaining distance. + const auto prepare_distance = helper_.getNominalPrepareDistance(); + const auto constant = + object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance; + const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant; + + // the avoidance path is already approved + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + const auto is_approved = (helper_.getShift(object_pos) > 0.0 && is_object_on_right) || + (helper_.getShift(object_pos) < 0.0 && !is_object_on_right); + if (is_approved) { + return desire_shift_length; + } + + // prepare distance is not enough. unavoidable. + if (remaining_distance < 1e-3) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return boost::none; + } + + // nominal case. avoidable. + if (has_enough_distance) { + return desire_shift_length; + } + + // calculate lateral jerk. + const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( + avoiding_shift, remaining_distance, helper_.getSharpAvoidanceEgoSpeed()); + + // relax lateral jerk limit. avoidable. + if (required_jerk < parameters_->max_lateral_jerk) { + return desire_shift_length; + } + + // avoidance distance is not enough. unavoidable. + if (!parameters_->use_constraints_for_decel) { + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return boost::none; + } + + // output avoidance path under lateral jerk constraints. + const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk( + remaining_distance, parameters_->max_lateral_jerk, helper_.getSharpAvoidanceEgoSpeed()); + + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, + "original shift length is not feasible. generate avoidance path under the constraints. " + "[original: (%.2f) actual: (%.2f)]", + std::abs(avoiding_shift), feasible_shift_length); + + return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift + : -1.0 * feasible_shift_length + current_ego_shift; + }; + const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; + const auto is_valid_shift_line = [](const auto & s) { + return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; + }; + AvoidLineArray avoid_lines; for (auto & o : data.target_objects) { if (!o.avoid_margin) { @@ -857,8 +938,9 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } const auto is_object_on_right = utils::avoidance::isOnRight(o); - const auto shift_length = helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get()); - if (utils::avoidance::isSameDirectionShift(is_object_on_right, shift_length)) { + const auto desire_shift_length = + helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get()); + if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; if (o.avoid_required && is_forward_object(o)) { break; @@ -867,64 +949,24 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } } - const auto avoiding_shift = shift_length - current_ego_shift; - const auto return_shift = shift_length; - - // use absolute dist for return-to-center, relative dist from current for avoiding. - const auto nominal_avoid_distance = helper_.getNominalAvoidanceDistance(avoiding_shift); - const auto nominal_return_distance = helper_.getNominalAvoidanceDistance(return_shift); - // use each object param const auto object_type = utils::getHighestProbLabel(o.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto feasible_shift_length = get_shift_length(o, desire_shift_length); - /** - * Is there enough distance from ego to object for avoidance? - * - Yes -> use the nominal distance. - * - No -> check if it is possible to avoid within maximum jerk limit. - * - Yes -> use the stronger jerk. - * - No -> ignore this object. Expected behavior is that the vehicle will stop in front - * of the obstacle, then start avoidance. - */ - const auto constant = - object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance; - const auto has_enough_distance = o.longitudinal > constant + nominal_avoid_distance; - const auto remaining_distance = o.longitudinal - constant; - if (!has_enough_distance) { - if (remaining_distance <= 0.0) { - // TODO(Horibe) Even if there is no enough distance for avoidance shift, the - // return-to-center shift must be considered for each object if the current_shift - // is not zero. - if (!data.avoiding_now) { - o.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; - if (o.avoid_required && is_forward_object(o)) { - break; - } else { - continue; - } - } - } - - // This is the case of exceeding the jerk limit. Use the sharp avoidance ego speed. - const auto required_jerk = path_shifter_.calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_.getSharpAvoidanceEgoSpeed()); - if (required_jerk > parameters_->max_lateral_jerk) { - if (!data.avoiding_now) { - o.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - if (o.avoid_required && is_forward_object(o)) { - break; - } else { - continue; - } - } + if (!feasible_shift_length) { + if (o.avoid_required && is_forward_object(o)) { + break; + } else { + continue; } } - const auto avoiding_distance = - has_enough_distance ? nominal_avoid_distance : remaining_distance; - const auto is_valid_shift_line = [](const auto & s) { - return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; - }; + // use absolute dist for return-to-center, relative dist from current for avoiding. + const auto feasible_avoid_distance = + helper_.getNominalAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + const auto feasible_return_distance = + helper_.getNominalAvoidanceDistance(feasible_shift_length.get()); AvoidLine al_avoid; { @@ -932,13 +974,14 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto path_front_to_ego = avoidance_data_.arclength_from_ego.at(avoidance_data_.ego_closest_path_index); - al_avoid.start_longitudinal = o.longitudinal - offset - avoiding_distance; + al_avoid.start_longitudinal = + std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( avoidance_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); al_avoid.start = avoidance_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); - al_avoid.end_shift_length = shift_length; + al_avoid.end_shift_length = feasible_shift_length.get(); al_avoid.end_longitudinal = o.longitudinal - offset; al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; @@ -957,11 +1000,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto return_remaining_distance = std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); - al_return.start_shift_length = shift_length; + al_return.start_shift_length = feasible_shift_length.get(); al_return.end_shift_length = 0.0; al_return.start_longitudinal = o.longitudinal + offset; al_return.end_longitudinal = - o.longitudinal + offset + std::min(nominal_return_distance, return_remaining_distance); + o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; @@ -970,14 +1013,6 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } } - DEBUG_PRINT( - "object is set: avoid_shift = %f, return_shift = %f, dist = (avoidStart: %3.3f, avoidEnd: " - "%3.3f, returnEnd: %3.3f), avoiding_dist = (nom:%f, res:%f), avoid_margin = %f, return_dist " - "= %f", - avoiding_shift, return_shift, al_avoid.start_longitudinal, al_avoid.end_longitudinal, - al_return.end_longitudinal, nominal_avoid_distance, avoiding_distance, o.avoid_margin.get(), - nominal_return_distance); - o.is_avoidable = true; } diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index eb752220d3690..653efff7058d2 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -555,17 +555,29 @@ double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jer } double PathShifter::calcFeasibleVelocityFromJerk( - const double lateral, const double jerk, const double distance) + const double lateral, const double jerk, const double longitudinal) { const double j = std::abs(jerk); const double l = std::abs(lateral); - const double d = std::abs(distance); + const double d = std::abs(longitudinal); if (j < 1.0e-8) { return 1.0e10; // TODO(Horibe) maybe invalid arg? } return d / (4.0 * std::pow(0.5 * l / j, 1.0 / 3.0)); } +double PathShifter::calcLateralDistFromJerk( + const double longitudinal, const double jerk, const double velocity) +{ + const double j = std::abs(jerk); + const double d = std::abs(longitudinal); + const double v = std::abs(velocity); + if (j < 1.0e-8) { + return 1.0e10; // TODO(Horibe) maybe invalid arg? + } + return 2.0 * std::pow(d / (4.0 * v), 3.0) * j; +} + double PathShifter::calcLongitudinalDistFromJerk( const double lateral, const double jerk, const double velocity) {