feat(avoidance): make it selectable avoidance policy

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

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

@@ -175,16 +175,29 @@
         max_distance: 20.0                              # [m]
         stop_buffer: 1.0                                # [m]
 
-      constraints:
-        # vehicle slows down under longitudinal constraints
-        use_constraints_for_decel: false                # [-]
+      policy:
+        # policy for vehicle slow down behavior. select "best_effort" or "reliable".
+        # "best_effort": slow down deceleration & jerk are limited by constraints.
+        #                but there is a possibility that the vehicle can't stop in front of the vehicle.
+        # "reliable": insert stop or slow down point with ignoring decel/jerk constraints.
+        #             make it possible to increase chance to avoid but uncomfortable deceleration maybe happen.
+        deceleration: "best_effort"                     # [-]
+        # policy for avoidance lateral margin. select "best_effort" or "reliable".
+        # "best_effort": output avoidance path with shorten lateral margin when there is no enough longitudinal
+        #                margin to avoid.
+        # "reliable": module output avoidance path with safe (rtc cooperate) state only when the vehicle can avoid
+        #             with expected lateral margin.
+        lateral_margin: "best_effort"                   # [-]
+        # if true, module doesn't wait deceleration and outputs avoidance path by best effort margin.
+        use_shorten_margin_immediately: true            # [-]
 
+      constraints:
         # lateral constraints
         lateral:
           velocity: [1.0, 1.38, 11.1]                   # [m/s]
           max_accel_values: [0.5, 0.5, 0.5]             # [m/ss]
           min_jerk_values: [0.2, 0.2, 0.2]              # [m/sss]
-          max_jerk_values: [1.0, 1.0, 1.0]             # [m/sss]
+          max_jerk_values: [1.0, 1.0, 1.0]              # [m/sss]
 
         # longitudinal constraints
         longitudinal:

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

@@ -113,8 +113,11 @@ struct AvoidanceParameters
   // use intersection area for avoidance
   bool use_intersection_areas{false};
 
-  // constrains
-  bool use_constraints_for_decel{false};
+  // // constrains
+  // bool use_constraints_for_decel{false};
+
+  // // policy
+  // bool use_relaxed_margin_immediately{false};
 
   // max deceleration for
   double max_deceleration;
@@ -274,6 +277,15 @@ struct AvoidanceParameters
   // For shift line generation process. Remove sharp(=jerky) shift line.
   double sharp_shift_filter_threshold;
 
+  // policy
+  bool use_shorten_margin_immediately{false};
+
+  // policy
+  std::string policy_deceleration{"best_effort"};
+
+  // policy
+  std::string policy_lateral_margin{"best_effort"};
+
   // target velocity matrix
   std::vector<double> velocity_map;
 

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

@@ -95,6 +95,11 @@ bool isDrivingSameLane(
 
   return !same_ids.empty();
 }
+
+bool isBestEffort(const std::string & policy)
+{
+  return policy == "best_effort";
+}
 }  // namespace
 
 AvoidanceModule::AvoidanceModule(
@@ -617,28 +622,28 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
     return;
   }
 
-  insertPrepareVelocity(path);
+  // insertPrepareVelocity(path);
 
   switch (data.state) {
     case AvoidanceState::NOT_AVOID: {
       break;
     }
     case AvoidanceState::YIELD: {
       insertYieldVelocity(path);
-      insertWaitPoint(parameters_->use_constraints_for_decel, path);
+      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       removeRegisteredShiftLines();
       break;
     }
     case AvoidanceState::AVOID_PATH_NOT_READY: {
-      insertWaitPoint(parameters_->use_constraints_for_decel, path);
+      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     case AvoidanceState::AVOID_PATH_READY: {
-      insertWaitPoint(parameters_->use_constraints_for_decel, path);
+      insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     case AvoidanceState::AVOID_EXECUTE: {
-      insertStopPoint(parameters_->use_constraints_for_decel, path);
+      insertStopPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
     default:
@@ -836,6 +841,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     const auto avoiding_shift = desire_shift_length - current_ego_shift;
     const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift);
 
+    if (!isBestEffort(parameters_->policy_lateral_margin)) {
+      return desire_shift_length;
+    }
+
     // ego already has enough positive shift.
     const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3;
     if (is_object_on_right && has_enough_positive_shift) {
@@ -848,6 +857,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
       return desire_shift_length;
     }
 
+    // don't relax shift length since it can stop in front of the object.
+    if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) {
+      return desire_shift_length;
+    }
+
     // calculate remaining distance.
     const auto prepare_distance = helper_.getNominalPrepareDistance();
     const auto constant =
@@ -884,7 +898,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     }
 
     // avoidance distance is not enough. unavoidable.
-    if (!parameters_->use_constraints_for_decel) {
+    if (!isBestEffort(parameters_->policy_deceleration)) {
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
       return boost::none;
     }

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

@@ -191,10 +191,21 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.stop_buffer = get_parameter<double>(node, ns + "stop_buffer");
   }
 
-  // constraints
+  // policy
   {
-    std::string ns = "avoidance.constraints.";
-    p.use_constraints_for_decel = get_parameter<bool>(node, ns + "use_constraints_for_decel");
+    std::string ns = "avoidance.policy.";
+    p.policy_deceleration = get_parameter<std::string>(node, ns + "deceleration");
+    p.policy_lateral_margin = get_parameter<std::string>(node, ns + "lateral_margin");
+    p.use_shorten_margin_immediately =
+      get_parameter<bool>(node, ns + "use_shorten_margin_immediately");
+
+    if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") {
+      throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'.");
+    }
+
+    if (p.policy_lateral_margin != "best_effort" && p.policy_lateral_margin != "reliable") {
+      throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'.");
+    }
   }
 
   // constraints (longitudinal)

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

@@ -725,7 +725,7 @@ void fillObjectStoppableJudge(
   ObjectData & object_data, const ObjectDataArray & registered_objects,
   const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  if (!parameters->use_constraints_for_decel) {
+  if (parameters->policy_deceleration == "reliable") {
     object_data.is_stoppable = true;
     return;
   }