fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition (#6355)

* fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition

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

* fix(avoidance): don't insert stop point when the path is invalid

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

* refactor(avoidance): update parameter namespace

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

---------

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

Author: satoshi-ota

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -6,8 +6,6 @@
       resample_interval_for_output: 4.0                 # [m]
       # avoidance module common setting
       enable_bound_clipping: false
-      enable_yield_maneuver: true
-      enable_yield_maneuver_during_shifting: false
       enable_cancel_maneuver: true
       disable_path_update: false
 
@@ -247,7 +245,8 @@
 
       # For yield maneuver
       yield:
-        yield_velocity: 2.78                            # [m/s]
+        enable: true                                    # [-]
+        enable_during_shifting: false                   # [-]
 
       # For stop maneuver
       stop:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -216,9 +216,6 @@ struct AvoidanceParameters
   size_t hysteresis_factor_safe_count;
   double hysteresis_factor_expand_rate{0.0};
 
-  // keep target velocity in yield maneuver
-  double yield_velocity{0.0};
-
   // maximum stop distance
   double stop_max_distance{0.0};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -41,9 +41,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "resample_interval_for_output");
     p.enable_bound_clipping = getOrDeclareParameter<bool>(*node, ns + "enable_bound_clipping");
     p.enable_cancel_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable_cancel_maneuver");
-    p.enable_yield_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable_yield_maneuver");
-    p.enable_yield_maneuver_during_shifting =
-      getOrDeclareParameter<bool>(*node, ns + "enable_yield_maneuver_during_shifting");
     p.disable_path_update = getOrDeclareParameter<bool>(*node, ns + "disable_path_update");
     p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "publish_debug_marker");
     p.print_debug_info = getOrDeclareParameter<bool>(*node, ns + "print_debug_info");
@@ -295,7 +292,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   // yield
   {
     const std::string ns = "avoidance.yield.";
-    p.yield_velocity = getOrDeclareParameter<double>(*node, ns + "yield_velocity");
+    p.enable_yield_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable");
+    p.enable_yield_maneuver_during_shifting =
+      getOrDeclareParameter<bool>(*node, ns + "enable_during_shifting");
   }
 
   // stop

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -233,13 +233,6 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void insertPrepareVelocity(ShiftedPath & shifted_path) const;
 
-  /**
-   * @brief insert decel point in output path in order to yield. the ego decelerates within
-   * accel/jerk constraints.
-   * @param target path.
-   */
-  void insertYieldVelocity(ShiftedPath & shifted_path) const;
-
   /**
    * @brief calculate stop distance based on object's overhang.
    * @param stop distance.

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -692,7 +692,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
       break;
     }
     case AvoidanceState::YIELD: {
-      insertYieldVelocity(path);
       insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
       break;
     }
@@ -1525,6 +1524,11 @@ void AvoidanceModule::insertWaitPoint(
 {
   const auto & data = avoid_data_;
 
+  // If avoidance path is NOT valid, don't insert any stop points.
+  if (!data.valid) {
+    return;
+  }
+
   if (!data.stop_target_object) {
     return;
   }
@@ -1611,28 +1615,20 @@ void AvoidanceModule::insertStopPoint(
     getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_);
 }
 
-void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const
+void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
 {
-  const auto & p = parameters_;
   const auto & data = avoid_data_;
 
-  if (data.target_objects.empty()) {
+  // If avoidance path is NOT safe, don't insert any slow down sections.
+  if (!data.safe && !data.stop_target_object) {
     return;
   }
 
-  if (helper_->isShifted()) {
+  // If avoidance path is NOT safe, don't insert any slow down sections.
+  if (!data.valid) {
     return;
   }
 
-  const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false);
-  utils::avoidance::insertDecelPoint(
-    getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_);
-}
-
-void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
-{
-  const auto & data = avoid_data_;
-
   // do nothing if there is no avoidance target.
   if (data.target_objects.empty()) {
     return;
@@ -1648,34 +1644,53 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
     return;
   }
 
-  const auto object = data.target_objects.front();
+  const auto itr = std::find_if(
+    data.target_objects.begin(), data.target_objects.end(),
+    [](const auto & o) { return o.avoid_required; });
+
+  const auto object = [&]() -> std::optional<ObjectData> {
+    if (!data.yield_required) {
+      return data.target_objects.front();
+    }
+
+    if (itr == data.target_objects.end()) {
+      return std::nullopt;
+    }
+
+    return *itr;
+  }();
+
+  // do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary.
+  if (!object.has_value()) {
+    return;
+  }
 
   const auto enough_space =
-    object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
+    object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
   if (!enough_space) {
     return;
   }
 
   // calculate shift length for front object.
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
-  const auto object_type = utils::getHighestProbLabel(object.object.classification);
+  const auto object_type = utils::getHighestProbLabel(object.value().object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
-  const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
+  const auto avoid_margin = object_parameter.lateral_hard_margin * object.value().distance_factor +
                             object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
-  const auto shift_length =
-    helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);
+  const auto shift_length = helper_->getShiftLength(
+    object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin);
 
   // check slow down feasibility
   const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length);
-  const auto distance_to_object = object.longitudinal;
+  const auto distance_to_object = object.value().longitudinal;
   const auto remaining_distance = distance_to_object - min_avoid_distance;
   const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front());
   if (remaining_distance < decel_distance) {
     return;
   }
 
   // decide slow down lower limit.
-  const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed;
+  const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed;
 
   // insert slow down speed.
   const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(