From a0b620d1bdccadec35c47d1443be4bf763841cde Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 8 Feb 2024 13:13:24 +0900 Subject: [PATCH] fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition Signed-off-by: satoshi-ota --- .../data_structs.hpp | 3 - .../parameter_helper.hpp | 6 -- .../behavior_path_avoidance_module/scene.hpp | 7 --- .../src/scene.cpp | 57 ++++++++++--------- 4 files changed, 29 insertions(+), 44 deletions(-) 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 a536fa1b568cb..71683659fa4fd 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 @@ -213,9 +213,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 13a792600da7f..c61c166058ce6 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 @@ -282,12 +282,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "traffic_light.buffer"); } - // yield - { - const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); - } - // stop { const std::string ns = "avoidance.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 a74c3a69d7bce..a0fba9bcba971 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 @@ -233,13 +233,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 685739fee432e..04b3365276c38 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -660,7 +660,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif break; } case AvoidanceState::YIELD: { - insertYieldVelocity(path); insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } @@ -1549,24 +1548,6 @@ void AvoidanceModule::insertStopPoint( getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const -{ - const auto & p = parameters_; - const auto & data = avoid_data_; - - if (data.target_objects.empty()) { - return; - } - - if (helper_->isShifted()) { - 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_; @@ -1586,26 +1567,46 @@ 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.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto shift_length = - helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); + const auto avoid_margin = + object_parameter.safety_buffer_lateral * object.value().distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + 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) { @@ -1613,7 +1614,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(