From 8f67b64c2addaf1464875933bc07e133220df80d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 7 Feb 2024 03:44:51 +0900 Subject: [PATCH 1/2] fix(avoidance): unexpected stop decision in avoidance module (#6320) Signed-off-by: satoshi-ota --- .../behavior_path_avoidance_module/helper.hpp | 3 +- .../src/shift_line_generator.cpp | 30 ++++++++++++++----- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index f5b797978b744..0f72c1aefc33b 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -261,10 +261,11 @@ class AvoidanceHelper bool isComfortable(const AvoidLineArray & shift_lines) const { + const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { return PathShifter::calcJerkFromLatLonDistance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < - getLateralMaxJerkLimit(); + getLateralMaxJerkLimit() + JERK_BUFFER; }); } diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 6196b3e209d11..96e22152a2f93 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -185,29 +185,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // prepare distance is not enough. unavoidable. - if (remaining_distance < 1e-3) { + if (avoidance_distance < 1e-3) { object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; return std::nullopt; } // calculate lateral jerk. const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed()); + avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. if (required_jerk < helper_->getLateralMaxJerkLimit()) { return std::make_pair(desire_shift_length, avoidance_distance); } + constexpr double LON_DIST_BUFFER = 1e-3; + // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return std::nullopt; + if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } else { + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; + } } // output avoidance path under lateral jerk constraints. const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( - remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); + avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { object.reason = "LessThanExecutionThreshold"; @@ -218,8 +225,17 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift : -1.0 * feasible_relative_shift_length + current_ego_shift; + if ( + avoidance_distance < + helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } + + const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + const auto infeasible = - std::abs(feasible_shift_length - object.overhang_dist) < + std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER < 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); @@ -227,7 +243,7 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( return std::nullopt; } - return std::make_pair(feasible_shift_length, avoidance_distance); + return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance); }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; From 2c1ad39f2a67f3f7f8c01661707e098173ad54be Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 7 Feb 2024 09:09:41 +0900 Subject: [PATCH 2/2] fix(avoidance): ghost debug markers due to duplicated marker id (#6330) fix(avoidance): fix duplicated marker id Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/debug.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 7c55c62bd8cc1..fcca9b1a18f49 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -47,7 +47,7 @@ int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid) int32_t ret = 0; for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); + ret <<= sizeof(int8_t) * 8; ret |= uuid.uuid.at(i); }