diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 11388dd6bb74a..b4c6230750946 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -185,7 +185,8 @@ class AvoidanceHelper max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); return std::clamp( - 1.5 * dynamic_distance, parameters_->object_check_min_forward_distance, + 1.5 * dynamic_distance + getNominalPrepareDistance(), + parameters_->object_check_min_forward_distance, parameters_->object_check_max_forward_distance); } 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 54771cc4e66c1..80baa5230f4f5 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 @@ -920,8 +920,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( 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 { + const auto get_shift_profile = + [&]( + auto & object, const auto & desire_shift_length) -> std::optional> { // use each object param const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -931,55 +932,57 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( const auto avoiding_shift = desire_shift_length - current_ego_shift; const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift); + // calculate remaining distance. + const auto prepare_distance = helper_.getNominalPrepareDistance(); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto constant = object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal + prepare_distance; + const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant; + const auto avoidance_distance = + has_enough_distance ? nominal_avoid_distance : remaining_distance; + + // nominal case. avoidable. + if (has_enough_distance) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + if (!isBestEffort(parameters_->policy_lateral_margin)) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // 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; + return std::make_pair(desire_shift_length, avoidance_distance); } // 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; + return std::make_pair(desire_shift_length, avoidance_distance); } // don't relax shift length since it can stop in front of the object. if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } - // calculate remaining distance. - const auto prepare_distance = helper_.getNominalPrepareDistance(); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto constant = object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + 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; + return std::make_pair(desire_shift_length, avoidance_distance); } // 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; + return std::nullopt; } // calculate lateral jerk. @@ -988,13 +991,13 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // relax lateral jerk limit. avoidable. if (required_jerk < helper_.getLateralMaxJerkLimit()) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return boost::none; + return std::nullopt; } // output avoidance path under lateral jerk constraints. @@ -1003,7 +1006,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { object.reason = "LessThanExecutionThreshold"; - return boost::none; + return std::nullopt; } const auto feasible_shift_length = @@ -1017,7 +1020,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return boost::none; + return std::nullopt; } { @@ -1026,7 +1029,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( std::abs(avoiding_shift), feasible_relative_shift_length); } - return feasible_shift_length; + return std::make_pair(feasible_shift_length, avoidance_distance); }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; @@ -1061,9 +1064,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // 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); + const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (!feasible_shift_length) { + if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; } else { @@ -1072,10 +1075,8 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( } // use absolute dist for return-to-center, relative dist from current for avoiding. - const auto feasible_avoid_distance = - helper_.getMaxAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); const auto feasible_return_distance = - helper_.getMaxAvoidanceDistance(feasible_shift_length.get()); + helper_.getMaxAvoidanceDistance(feasible_shift_profile.value().first); AvoidLine al_avoid; { @@ -1091,14 +1092,15 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // start point (use previous linear shift length as start shift length.) al_avoid.start_longitudinal = [&]() { - const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3); + const auto nearest_avoid_distance = + std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3); if (data.to_start_point > to_shift_end) { return nearest_avoid_distance; } const auto minimum_avoid_distance = - helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + helper_.getMinAvoidanceDistance(feasible_shift_profile.value().first - current_ego_shift); const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); @@ -1110,7 +1112,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); // end point - al_avoid.end_shift_length = feasible_shift_length.get(); + al_avoid.end_shift_length = feasible_shift_profile.value().first; al_avoid.end_longitudinal = to_shift_end; // misc @@ -1125,7 +1127,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( const auto to_shift_start = o.longitudinal + offset; // start point - al_return.start_shift_length = feasible_shift_length.get(); + al_return.start_shift_length = feasible_shift_profile.value().first; al_return.start_longitudinal = to_shift_start; // end point