From 0ea526ae26c39f263a268a12b4fd09a72d0fb2f5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 18 Oct 2024 08:26:29 +0900 Subject: [PATCH 1/2] fix(avoidance): don't insert stop line if the ego can't avoid or return (#9089) * fix(avoidance): don't insert stop line if the ego can't avoid or return Signed-off-by: satoshi-ota * fix: build error Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp Co-authored-by: Go Sakayori --------- Signed-off-by: satoshi-ota Co-authored-by: Go Sakayori --- .../behavior_path_avoidance_module/helper.hpp | 13 +++++++++++++ .../src/scene.cpp | 19 +++++++++++++++++++ 2 files changed, 32 insertions(+) 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 4ff02df97bd89..60cf62a492afa 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 @@ -312,6 +312,19 @@ class AvoidanceHelper }); } + bool isFeasible(const AvoidLineArray & shift_lines) const + { + constexpr double JERK_BUFFER = 0.1; // [m/sss] + const auto & values = parameters_->velocity_map; + const auto idx = getConstraintsMapIndex(0.0, values); // use minimum avoidance speed + const auto jerk_limit = parameters_->lateral_max_jerk_map.at(idx); + return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { + return autoware::motion_utils::calc_jerk_from_lat_lon_distance( + line.getRelativeLength(), line.getRelativeLongitudinal(), values.at(idx)) < + jerk_limit + JERK_BUFFER; + }); + } + bool isReady(const ObjectDataArray & objects) const { if (objects.empty()) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b7a732359e601..4af3dfdb68009 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1505,6 +1505,11 @@ void AvoidanceModule::insertReturnDeadLine( { const auto & data = avoid_data_; + if (data.new_shift_line.empty()) { + RCLCPP_WARN(getLogger(), "module doesn't have return shift line."); + return; + } + if (data.to_return_point > planner_data_->parameters.forward_path_length) { RCLCPP_DEBUG(getLogger(), "return dead line is far enough."); return; @@ -1517,6 +1522,11 @@ void AvoidanceModule::insertReturnDeadLine( return; } + if (!helper_->isFeasible(data.new_shift_line)) { + RCLCPP_WARN(getLogger(), "return shift line is not feasible. do nothing.."); + return; + } + // Consider the difference in path length between the shifted path and original path (the path // that is shifted inward has a shorter distance to the end of the path than the other one.) const auto & to_reference_path_end = data.arclength_from_ego.back(); @@ -1527,6 +1537,10 @@ void AvoidanceModule::insertReturnDeadLine( const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; + if (to_stop_line < 0.0) { + RCLCPP_WARN(getLogger(), "ego overran return shift dead line. do nothing."); + return; + } // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately @@ -1594,6 +1608,11 @@ void AvoidanceModule::insertWaitPoint( return; } + if (data.to_stop_line < 0.0) { + RCLCPP_WARN(getLogger(), "ego overran avoidance dead line. do nothing."); + return; + } + // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { From 62a94ecd8d856877662d71a23eeea70e6a2a5461 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Fri, 18 Oct 2024 09:23:27 +0900 Subject: [PATCH 2/2] fix: build error Signed-off-by: satoshi-ota --- .../include/behavior_path_avoidance_module/helper.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 60cf62a492afa..f656176193f59 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 @@ -319,7 +319,7 @@ class AvoidanceHelper const auto idx = getConstraintsMapIndex(0.0, values); // use minimum avoidance speed const auto jerk_limit = parameters_->lateral_max_jerk_map.at(idx); return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { - return autoware::motion_utils::calc_jerk_from_lat_lon_distance( + return PathShifter::calcJerkFromLatLonDistance( line.getRelativeLength(), line.getRelativeLongitudinal(), values.at(idx)) < jerk_limit + JERK_BUFFER; });