From 68346caec4e6b1190a7e397ec214141f2aa20356 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 6 Sep 2023 19:04:08 +0900 Subject: [PATCH 1/2] fix(avoidance): don't output unfeasible path (#4899) Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.cpp | 34 ++++++++++++++----- 1 file changed, 26 insertions(+), 8 deletions(-) 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 36a33e5bee088..b15358e3598e1 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 @@ -880,17 +880,35 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( } // output avoidance path under lateral jerk constraints. - const auto feasible_shift_length = PathShifter::calcLateralDistFromJerk( + const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed()); - 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); + if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return boost::none; + } + + const auto feasible_shift_length = + desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift + : -1.0 * feasible_relative_shift_length + current_ego_shift; + + const auto feasible = + std::abs(feasible_shift_length - object.overhang_dist) < + 0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + if (feasible) { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); + object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + return boost::none; + } + + { + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]", + std::abs(avoiding_shift), feasible_relative_shift_length); + } - return desire_shift_length > 0.0 ? feasible_shift_length + current_ego_shift - : -1.0 * feasible_shift_length + current_ego_shift; + return feasible_shift_length; }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; From aaa7281f7cee700b994ae70c3f9e3b75130ee5fc Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 21 Sep 2023 09:15:31 +0900 Subject: [PATCH 2/2] fix(avoidance): exit if there is no avoidable object (#5051) Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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 b15358e3598e1..3fd8760946c7e 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 @@ -166,7 +166,10 @@ bool AvoidanceModule::canTransitSuccessState() } } - const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); const bool has_shift_point = !path_shifter_.getShiftLines().empty(); const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; @@ -3030,6 +3033,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto object = data.target_objects.front(); + const auto enough_space = + object.is_avoidable || object.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);