feat(avoidance): reduce road shoulder margin if lateral distance is n…

…ot enough to avoid (autowarefoundation#4609)

* feat(avoidance): reduce road shoulder margin if lateral distance is not enough to avoid

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

* fix(avoidance): remove std::min

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

---------

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

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

@@ -154,7 +154,8 @@
           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]
+          soft_road_shoulder_margin: 0.8                # [m]
+          hard_road_shoulder_margin: 0.3                # [m]
           max_right_shift_length: 5.0
           max_left_shift_length: 5.0
         # avoidance distance parameters

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

@@ -230,7 +230,11 @@ struct AvoidanceParameters
 
   // The margin is configured so that the generated avoidance trajectory does not come near to the
   // road shoulder.
-  double road_shoulder_safety_margin{1.0};
+  double soft_road_shoulder_margin{1.0};
+
+  // The margin is configured so that the generated avoidance trajectory does not come near to the
+  // road shoulder.
+  double hard_road_shoulder_margin{1.0};
 
   // Even if the obstacle is very large, it will not avoid more than this length for right direction
   double max_right_shift_length;

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

@@ -158,7 +158,8 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   // avoidance maneuver (lateral)
   {
     std::string ns = "avoidance.avoidance.lateral.";
-    p.road_shoulder_safety_margin = get_parameter<double>(node, ns + "road_shoulder_safety_margin");
+    p.soft_road_shoulder_margin = get_parameter<double>(node, ns + "soft_road_shoulder_margin");
+    p.hard_road_shoulder_margin = get_parameter<double>(node, ns + "hard_road_shoulder_margin");
     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");
@@ -316,8 +317,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
       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);
+    updateParam<double>(parameters, ns + "soft_road_shoulder_margin", p->soft_road_shoulder_margin);
+    updateParam<double>(parameters, ns + "hard_road_shoulder_margin", p->hard_road_shoulder_margin);
   }
 
   {

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -915,18 +915,27 @@ void filterTargetObjects(
 
     // calculate avoid_margin dynamically
     // NOTE: This calculation must be after calculating to_road_shoulder_distance.
-    const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor +
-                                    object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
-    const double min_safety_lateral_distance =
-      object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
-    const auto max_allowable_lateral_distance =
-      o.to_road_shoulder_distance - parameters->road_shoulder_safety_margin - 0.5 * vehicle_width;
+    const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor +
+                                  object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
+    const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;
+    const auto soft_lateral_distance_limit =
+      o.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width;
+    const auto hard_lateral_distance_limit =
+      o.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width;
 
     const auto avoid_margin = [&]() -> boost::optional<double> {
-      if (max_allowable_lateral_distance < min_safety_lateral_distance) {
+      // Step1. check avoidable or not.
+      if (hard_lateral_distance_limit < min_avoid_margin) {
         return boost::none;
       }
-      return std::min(max_allowable_lateral_distance, max_avoid_margin);
+
+      // Step2. check if it should expand road shoulder margin.
+      if (soft_lateral_distance_limit < min_avoid_margin) {
+        return min_avoid_margin;
+      }
+
+      // Step3. nominal case. avoid margin is limited by soft constraint.
+      return std::min(soft_lateral_distance_limit, max_avoid_margin);
     }();
 
     if (!!avoid_margin) {