refactor(avoidance): parameterize

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

Author: satoshi-ota

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -133,10 +133,13 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.threshold_time_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "time_threshold");
+    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
+    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.time_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.time_threshold");
+    p.distance_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.distance_threshold");
     p.object_ignore_section_traffic_light_in_front_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.traffic_light.front_distance");
     p.object_ignore_section_crosswalk_in_front_distance =

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -155,8 +155,10 @@
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
           enable: false                                  # [-]
-          time_threshold: 1.0                            # [s]
-          distance_threshold: 1.0                        # [m]
+          closest_distance_to_wait_and_see: 10.0         # [m]
+          condition:
+            time_threshold: 1.0                          # [s]
+            distance_threshold: 1.0                      # [m]
           ignore_area:
             traffic_light:
               front_distance: 100.0                      # [m]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -100,7 +100,7 @@ struct AvoidanceParameters
   bool enable_cancel_maneuver{false};
 
   // enable avoidance for all parking vehicle
-  bool enable_force_avoidance_for_stopped_vehicle{false};
+  bool enable_avoidance_for_ambiguous_vehicle{false};
 
   // enable yield maneuver.
   bool enable_yield_maneuver{false};
@@ -184,8 +184,9 @@ struct AvoidanceParameters
   double object_check_min_road_shoulder_width{0.0};
 
   // force avoidance
-  double threshold_time_force_avoidance_for_stopped_vehicle{0.0};
-  double force_avoidance_distance_threshold{0.0};
+  double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0};
+  double time_threshold_for_ambiguous_vehicle{0.0};
+  double distance_threshold_for_ambiguous_vehicle{0.0};
 
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -309,12 +309,13 @@ class AvoidanceHelper
     const auto desire_shift_length =
       getShiftLength(object, is_object_on_right, object.avoid_margin.value());
 
-    constexpr double BUFFER = 10.0;
     const auto prepare_distance = getMinimumPrepareDistance();
     const auto constant_distance = getFrontConstantDistance(object);
     const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
 
-    return object.longitudinal < prepare_distance + constant_distance + avoidance_distance + BUFFER;
+    return object.longitudinal <
+           prepare_distance + constant_distance + avoidance_distance +
+             parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle;
   }
 
   bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -141,12 +141,13 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.threshold_time_force_avoidance_for_stopped_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "time_threshold");
-    p.force_avoidance_distance_threshold =
-      getOrDeclareParameter<double>(*node, ns + "distance_threshold");
+    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
+    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.time_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.time_threshold");
+    p.distance_threshold_for_ambiguous_vehicle =
+      getOrDeclareParameter<double>(*node, ns + "condition.distance_threshold");
     p.object_ignore_section_traffic_light_in_front_distance =
       getOrDeclareParameter<double>(*node, ns + "ignore_area.traffic_light.front_distance");
     p.object_ignore_section_crosswalk_in_front_distance =

planning/behavior_path_avoidance_module/src/utils.cpp

@@ -680,7 +680,7 @@ bool isNeverAvoidanceTarget(
   const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   const auto is_moving_distance_longer_than_threshold =
     tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) >
-    parameters->force_avoidance_distance_threshold;
+    parameters->distance_threshold_for_ambiguous_vehicle;
   if (is_moving_distance_longer_than_threshold) {
     object.reason = AvoidanceDebugFactor::MOVING_OBJECT;
     return true;
@@ -860,13 +860,13 @@ bool isSatisfiedWithVehicleCondition(
 
   // from here, filtering for ambiguous vehicle.
 
-  if (!parameters->enable_force_avoidance_for_stopped_vehicle) {
+  if (!parameters->enable_avoidance_for_ambiguous_vehicle) {
     object.reason = "AmbiguousStoppedVehicle";
     return false;
   }
 
   const auto stop_time_longer_than_threshold =
-    object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;
+    object.stop_time > parameters->time_threshold_for_ambiguous_vehicle;
   if (!stop_time_longer_than_threshold) {
     object.reason = "AmbiguousStoppedVehicle(wait-and-see)";
     return false;