fix(avoidance): prevent sudden steering at avoidance maneuver (autowa…

…refoundation#5572)

* fix(avoidance): prevent sudden steering at yield maneuver

Signed-off-by: satoshi-ota <[email protected]>

* feat(avoidance): output debug info

Signed-off-by: satoshi-ota <[email protected]>

* Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

Co-authored-by: Fumiya Watanabe <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: satoshi-ota <[email protected]>
Co-authored-by: Fumiya Watanabe <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -178,7 +178,8 @@
           max_departure_from_lane: 0.5                  # [m]
         # avoidance distance parameters
         longitudinal:
-          prepare_time: 2.0                             # [s]
+          min_prepare_time: 1.0                         # [s]
+          max_prepare_time: 2.0                         # [s]
           min_prepare_distance: 1.0                     # [m]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s]

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -213,7 +213,8 @@ struct AvoidanceParameters
   double stop_buffer{0.0};
 
   // start avoidance after this time to avoid sudden path change
-  double prepare_time{0.0};
+  double min_prepare_time{0.0};
+  double max_prepare_time{0.0};
 
   // Even if the vehicle speed is zero, avoidance will start after a distance of this much.
   double min_prepare_distance{0.0};

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp

@@ -91,10 +91,16 @@ class AvoidanceHelper
     return std::max(getEgoSpeed(), values.at(idx));
   }
 
+  double getMinimumPrepareDistance() const
+  {
+    const auto & p = parameters_;
+    return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance);
+  }
+
   double getNominalPrepareDistance() const
   {
     const auto & p = parameters_;
-    return std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance);
+    return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance);
   }
 
   double getNominalAvoidanceDistance(const double shift_length) const

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -444,6 +444,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
 {
   // transit yield maneuver only when the avoidance maneuver is not initiated.
   if (helper_.isShifted()) {
+    RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated.");
     return false;
   }
 
@@ -453,13 +454,17 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
     const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
     const auto to_shift_start_point = calcSignedArcLength(
       path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx);
-    constexpr double FIXED_PATH_TIME = 1.0;
-    if (to_shift_start_point < FIXED_PATH_TIME * getEgoSpeed()) {
+    if (to_shift_start_point < helper_.getMinimumPrepareDistance()) {
+      RCLCPP_DEBUG(
+        getLogger(),
+        "Distance to shift start point is less than minimum prepare distance. The distance is not "
+        "enough.");
       return false;
     }
   }
 
   if (!data.stop_target_object) {
+    RCLCPP_DEBUG(getLogger(), "can pass by the object safely without avoidance maneuver.");
     return true;
   }
 
@@ -1661,6 +1666,10 @@ void AvoidanceModule::applySmallShiftFilter(
       continue;
     }
 
+    if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) {
+      continue;
+    }
+
     shift_lines.push_back(s);
   }
 }
@@ -2405,9 +2414,8 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(
   for (size_t i = 0; i < shift_lines.size(); ++i) {
     const auto & candidate = shift_lines.at(i);
 
-    // new shift points must exist in front of Ego
-    // this value should be larger than -eps consider path shifter calculation error.
-    if (candidate.start_idx < avoid_data_.ego_closest_path_index) {
+    // prevent sudden steering.
+    if (candidate.start_longitudinal < helper_.getMinimumPrepareDistance()) {
       break;
     }
 

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -209,7 +209,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   // avoidance maneuver (longitudinal)
   {
     std::string ns = "avoidance.avoidance.longitudinal.";
-    p.prepare_time = getOrDeclareParameter<double>(*node, ns + "prepare_time");
+    p.min_prepare_time = getOrDeclareParameter<double>(*node, ns + "min_prepare_time");
+    p.max_prepare_time = getOrDeclareParameter<double>(*node, ns + "max_prepare_time");
     p.min_prepare_distance = getOrDeclareParameter<double>(*node, ns + "min_prepare_distance");
     p.min_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "min_slow_down_speed");
     p.buf_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "buf_slow_down_speed");
@@ -375,7 +376,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
 
   {
     const std::string ns = "avoidance.avoidance.longitudinal.";
-    updateParam<double>(parameters, ns + "prepare_time", p->prepare_time);
+    updateParam<double>(parameters, ns + "min_prepare_time", p->min_prepare_time);
+    updateParam<double>(parameters, ns + "max_prepare_time", p->max_prepare_time);
     updateParam<double>(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed);
     updateParam<double>(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed);
   }