refactor(avoidance): parameterize magic number (#4116)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

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

@@ -138,6 +138,7 @@
         lateral:
           lateral_execution_threshold: 0.499            # [m]
           lateral_small_shift_threshold: 0.101          # [m]
+          lateral_avoid_check_threshold: 0.1            # [m]
           road_shoulder_safety_margin: 0.3              # [m]
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0

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

@@ -248,6 +248,9 @@ struct AvoidanceParameters
   // line.
   double lateral_small_shift_threshold;
 
+  // use for judge if the ego is shifting or not.
+  double lateral_avoid_check_threshold;
+
   // For shift line generation process. The continuous shift length is quantized by this value.
   double quantize_filter_threshold;
 
@@ -460,8 +463,6 @@ struct AvoidancePlanningData
 
   bool safe{false};
 
-  bool avoiding_now{false};
-
   bool avoid_required{false};
 
   bool yield_required{false};

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

@@ -224,6 +224,11 @@ class AvoidanceHelper
     return std::numeric_limits<double>::max();
   }
 
+  bool isShifted() const
+  {
+    return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold;
+  }
+
   bool isInitialized() const
   {
     if (prev_spline_shift_path_.path.points.empty()) {

planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp

@@ -200,8 +200,7 @@ MarkerArray createEgoStatusMarkerArray(
   {
     std::ostringstream string_stream;
     string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
-    string_stream << "avoid_now:" << data.avoiding_now << ","
-                  << "avoid_req:" << data.avoid_required << ","
+    string_stream << "avoid_req:" << data.avoid_required << ","
                   << "yield_req:" << data.yield_required << ","
                   << "safe:" << data.safe;
     marker.text = string_stream.str();

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

@@ -379,7 +379,7 @@ ObjectData AvoidanceModule::createObjectData(
 bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
 {
   // transit yield maneuver only when the avoidance maneuver is not initiated.
-  if (data.avoiding_now) {
+  if (helper_.isShifted()) {
     return false;
   }
 
@@ -413,9 +413,6 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
 
 void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const
 {
-  constexpr double AVOIDING_SHIFT_THR = 0.1;
-  data.avoiding_now = std::abs(helper_.getEgoShift()) > AVOIDING_SHIFT_THR;
-
   auto path_shifter = path_shifter_;
 
   /**
@@ -578,7 +575,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
     return;
   }
 
-  if (data.avoiding_now) {
+  if (helper_.isShifted()) {
     return;
   }
 
@@ -3189,7 +3186,7 @@ void AvoidanceModule::insertWaitPoint(
     return;
   }
 
-  if (data.avoiding_now) {
+  if (helper_.isShifted()) {
     return;
   }
 
@@ -3280,7 +3277,7 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const
     return;
   }
 
-  if (data.avoiding_now) {
+  if (helper_.isShifted()) {
     return;
   }
 
@@ -3299,7 +3296,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
   }
 
   // insert slow down speed only when the avoidance maneuver is not initiated.
-  if (data.avoiding_now) {
+  if (helper_.isShifted()) {
     return;
   }
 

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

@@ -156,6 +156,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.lateral_execution_threshold = get_parameter<double>(node, ns + "lateral_execution_threshold");
     p.lateral_small_shift_threshold =
       get_parameter<double>(node, ns + "lateral_small_shift_threshold");
+    p.lateral_avoid_check_threshold =
+      get_parameter<double>(node, ns + "lateral_avoid_check_threshold");
     p.max_right_shift_length = get_parameter<double>(node, ns + "max_right_shift_length");
     p.max_left_shift_length = get_parameter<double>(node, ns + "max_left_shift_length");
   }
@@ -302,6 +304,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
       parameters, ns + "lateral_execution_threshold", p->lateral_execution_threshold);
     updateParam<double>(
       parameters, ns + "lateral_small_shift_threshold", p->lateral_small_shift_threshold);
+    updateParam<double>(
+      parameters, ns + "lateral_avoid_check_threshold", p->lateral_avoid_check_threshold);
     updateParam<double>(
       parameters, ns + "road_shoulder_safety_margin", p->road_shoulder_safety_margin);
   }