fix(avoidance): don't insert stop line if the ego can't avoid or retu…

…rn (autowarefoundation#9089) (#1592) * fix(avoidance): don't insert stop line if the ego can't avoid or return (autowarefoundation#9089) * fix(avoidance): don't insert stop line if the ego can't avoid or return Signed-off-by: satoshi-ota <[email protected]> * fix: build error Signed-off-by: satoshi-ota <[email protected]> * 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 <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]> Co-authored-by: Go Sakayori <[email protected]> * fix: build error Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]> Co-authored-by: Go Sakayori <[email protected]>
tier4 · Oct 21, 2024 · 1a9d2d8 · 1a9d2d8
1 parent a7e68a1
commit 1a9d2d8
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 0 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
@@ -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 PathShifter::calcJerkFromLatLonDistance(
+               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
@@ -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) {