From 5f22fa87d971a040c271e13bd9e82ebb530b656e Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Mar 2024 12:16:48 +0900 Subject: [PATCH] fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition (#6355) * fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition Signed-off-by: satoshi-ota * fix(avoidance): don't insert stop point when the path is invalid Signed-off-by: satoshi-ota * refactor(avoidance): update parameter namespace Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance.param.yaml | 5 +- .../data_structs.hpp | 3 - .../parameter_helper.hpp | 7 +-- .../behavior_path_avoidance_module/scene.hpp | 7 --- .../src/scene.cpp | 59 ++++++++++++------- 5 files changed, 42 insertions(+), 39 deletions(-) diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index e1018b02526e3..684d7738273a2 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -9,8 +9,6 @@ # avoidance module common setting enable_bound_clipping: false - enable_yield_maneuver: true - enable_yield_maneuver_during_shifting: false enable_cancel_maneuver: true disable_path_update: false @@ -249,7 +247,8 @@ # For yield maneuver yield: - yield_velocity: 2.78 # [m/s] + enable: true # [-] + enable_during_shifting: false # [-] # For stop maneuver stop: diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 312f693479000..572e2f3ee8231 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -216,9 +216,6 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; - // keep target velocity in yield maneuver - double yield_velocity{0.0}; - // maximum stop distance double stop_max_distance{0.0}; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 8dac56909a2e0..8ed9d2d0683c4 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -41,9 +41,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); @@ -293,7 +290,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // yield { const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.enable_yield_maneuver_during_shifting = + getOrDeclareParameter(*node, ns + "enable_during_shifting"); } // stop 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 0bb480bfa26b1..bb727a11d6934 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 @@ -235,13 +235,6 @@ class AvoidanceModule : public SceneModuleInterface */ void insertPrepareVelocity(ShiftedPath & shifted_path) const; - /** - * @brief insert decel point in output path in order to yield. the ego decelerates within - * accel/jerk constraints. - * @param target path. - */ - void insertYieldVelocity(ShiftedPath & shifted_path) const; - /** * @brief calculate stop distance based on object's overhang. * @param stop distance. diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 4bea2d19d5854..2d77adde08fe7 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -692,7 +692,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif break; } case AvoidanceState::YIELD: { - insertYieldVelocity(path); insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } @@ -1506,6 +1505,11 @@ void AvoidanceModule::insertWaitPoint( { const auto & data = avoid_data_; + // If avoidance path is NOT valid, don't insert any stop points. + if (!data.valid) { + return; + } + if (!data.stop_target_object) { return; } @@ -1592,28 +1596,20 @@ void AvoidanceModule::insertStopPoint( getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const +void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & p = parameters_; const auto & data = avoid_data_; - if (data.target_objects.empty()) { + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.safe && !data.stop_target_object) { return; } - if (helper_->isShifted()) { + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.valid) { return; } - const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false); - utils::avoidance::insertDecelPoint( - getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_); -} - -void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const -{ - const auto & data = avoid_data_; - // do nothing if there is no avoidance target. if (data.target_objects.empty()) { return; @@ -1629,26 +1625,45 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const return; } - const auto object = data.target_objects.front(); + const auto itr = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return o.avoid_required; }); + + const auto object = [&]() -> std::optional { + if (!data.yield_required) { + return data.target_objects.front(); + } + + if (itr == data.target_objects.end()) { + return std::nullopt; + } + + return *itr; + }(); + + // do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary. + if (!object.has_value()) { + return; + } const auto enough_space = - object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK; if (!enough_space) { return; } // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_type = utils::getHighestProbLabel(object.value().object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor + + const auto avoid_margin = object_parameter.lateral_hard_margin * object.value().distance_factor + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto shift_length = - helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); + const auto shift_length = helper_->getShiftLength( + object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin); // check slow down feasibility const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length); - const auto distance_to_object = object.longitudinal; + const auto distance_to_object = object.value().longitudinal; const auto remaining_distance = distance_to_object - min_avoid_distance; const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front()); if (remaining_distance < decel_distance) { @@ -1656,7 +1671,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const } // decide slow down lower limit. - const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; + const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(