feat(avoidance): improve stop behavior under decel/jerk constraints option (#3978)

* feat(avoidance): add option to decel within constraints

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

* chore(avoidance): add comments

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

* refactor(utils): merge similar functions

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

---------

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

Author: satoshi-ota

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -231,10 +231,9 @@ class AvoidanceModule : public SceneModuleInterface
 
   /**
    * @brief insert stop point in output path.
-   * @param flag. if it is true, the ego decelerates within accel/jerk constraints.
    * @param target path.
    */
-  void insertPrepareVelocity(const bool avoidable, ShiftedPath & shifted_path) const;
+  void insertPrepareVelocity(ShiftedPath & shifted_path) const;
 
   /**
    * @brief insert decel point in output path in order to yield. the ego decelerates within

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

@@ -349,15 +349,21 @@ struct ObjectData  // avoidance target
   // to intersection
   double to_stop_factor_distance{std::numeric_limits<double>::infinity()};
 
+  // to stop line distance
+  double to_stop_line{std::numeric_limits<double>::infinity()};
+
   // if lateral margin is NOT enough, the ego must avoid the object.
   bool avoid_required{false};
 
-  // unavoidable reason
-  std::string reason{""};
-
   // is avoidable by behavior module
   bool is_avoidable{false};
 
+  // is stoppable under the constraints
+  bool is_stoppable{false};
+
+  // unavoidable reason
+  std::string reason{""};
+
   // lateral avoid margin
   // NOTE: If margin is less than the minimum margin threshold, boost::none will be set.
   boost::optional<double> avoid_margin{boost::none};

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

@@ -21,6 +21,7 @@
 #include <motion_utils/distance/distance.hpp>
 
 #include <algorithm>
+#include <limits>
 #include <memory>
 
 namespace behavior_path_planner::helper::avoidance
@@ -39,6 +40,13 @@ class AvoidanceHelper
   {
   }
 
+  AvoidanceHelper(
+    const std::shared_ptr<const PlannerData> & data,
+    const std::shared_ptr<AvoidanceParameters> & parameters)
+  : data_{data}, parameters_{parameters}
+  {
+  }
+
   void validate() const
   {
     const auto reference_points_size = prev_reference_path_.points.size();
@@ -190,22 +198,21 @@ class AvoidanceHelper
     return *itr;
   }
 
-  boost::optional<double> getFeasibleDecelDistance(const double target_velocity) const
+  double getFeasibleDecelDistance(
+    const double target_velocity, const bool use_hard_constraints = true) const
   {
+    const auto & p = parameters_;
     const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
-    const auto & a_lim = parameters_->max_deceleration;
-    const auto & j_lim = parameters_->max_jerk;
-    return calcDecelDistWithJerkAndAccConstraints(
+    const auto & a_lim = use_hard_constraints ? p->max_deceleration : p->nominal_deceleration;
+    const auto & j_lim = use_hard_constraints ? p->max_jerk : p->nominal_jerk;
+    const auto ret = calcDecelDistWithJerkAndAccConstraints(
       getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
-  }
 
-  boost::optional<double> getMildDecelDistance(const double target_velocity) const
-  {
-    const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
-    const auto & a_lim = parameters_->nominal_deceleration;
-    const auto & j_lim = parameters_->nominal_jerk;
-    return calcDecelDistWithJerkAndAccConstraints(
-      getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim);
+    if (!!ret) {
+      return ret.get();
+    }
+
+    return std::numeric_limits<double>::max();
   }
 
   bool isInitialized() const

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

@@ -98,6 +98,10 @@ void fillAvoidanceNecessity(
   ObjectData & object_data, const ObjectDataArray & registered_objects, const double vehicle_width,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+void fillObjectStoppableJudge(
+  ObjectData & object_data, const ObjectDataArray & registered_objects,
+  const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters);
+
 void updateRegisteredObject(
   ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters);

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

@@ -318,7 +318,12 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb
 void AvoidanceModule::fillAvoidanceTargetObjects(
   AvoidancePlanningData & data, DebugData & debug) const
 {
-  const auto expanded_lanelets = utils::avoidance::getTargetLanelets(
+  using utils::avoidance::fillObjectStoppableJudge;
+  using utils::avoidance::filterTargetObjects;
+  using utils::avoidance::getTargetLanelets;
+
+  // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
+  const auto expanded_lanelets = getTargetLanelets(
     planner_data_, data.current_lanelets, parameters_->detection_area_left_expand_dist,
     parameters_->detection_area_right_expand_dist * (-1.0));
 
@@ -337,7 +342,18 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
     objects.push_back(createObjectData(data, object));
   }
 
-  utils::avoidance::filterTargetObjects(objects, data, debug, planner_data_, parameters_);
+  // Filter out the objects to determine the ones to be avoided.
+  filterTargetObjects(objects, data, debug, planner_data_, parameters_);
+
+  // Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
+  // TODO(Satoshi OTA) use helper_ after the manager transition
+  helper::avoidance::AvoidanceHelper helper(planner_data_, parameters_);
+
+  const auto feasible_stop_distance = helper.getFeasibleDecelDistance(0.0);
+  std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) {
+    o.to_stop_line = calcDistanceToStopLine(o);
+    fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
+  });
 
   // debug
   {
@@ -355,9 +371,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
       debug_info_array.push_back(debug_info);
     };
 
-    for (const auto & o : objects) {
-      append(o);
-    }
+    std::for_each(objects.begin(), objects.end(), [&](const auto & o) { append(o); });
 
     updateAvoidanceDebugData(debug_info_array);
   }
@@ -516,7 +530,7 @@ void AvoidanceModule::fillEgoStatus(
     if (o.avoid_required && enough_space) {
       data.avoid_required = true;
       data.stop_target_object = o;
-      data.to_stop_line = calcDistanceToStopLine(o);
+      data.to_stop_line = o.to_stop_line;
       break;
     }
   }
@@ -660,17 +674,16 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
       break;
     }
     case AvoidanceState::AVOID_PATH_NOT_READY: {
-      insertPrepareVelocity(false, path);
+      insertPrepareVelocity(path);
       insertWaitPoint(parameters_->use_constraints_for_decel, path);
       break;
     }
     case AvoidanceState::AVOID_PATH_READY: {
-      insertPrepareVelocity(true, path);
       insertWaitPoint(parameters_->use_constraints_for_decel, path);
       break;
     }
     case AvoidanceState::AVOID_EXECUTE: {
-      insertStopPoint(true, path);
+      insertStopPoint(parameters_->use_constraints_for_decel, path);
       break;
     }
     default:
@@ -3290,18 +3303,26 @@ void AvoidanceModule::insertWaitPoint(
     return;
   }
 
+  // If we don't need to consider deceleration constraints, insert a deceleration point
+  // and return immediately
   if (!use_constraints_for_decel) {
     utils::avoidance::insertDecelPoint(
       getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_);
     return;
   }
 
-  const auto stop_distance = helper_.getMildDecelDistance(0.0);
-  if (stop_distance) {
-    const auto insert_distance = std::max(data.to_stop_line, *stop_distance);
+  // If target object can be stopped for, insert a deceleration point and return
+  if (data.stop_target_object.get().is_stoppable) {
     utils::avoidance::insertDecelPoint(
-      getEgoPosition(), insert_distance, 0.0, shifted_path.path, stop_pose_);
+      getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_);
+    return;
   }
+
+  // If the object cannot be stopped for, calculate a "mild" deceleration distance
+  // and insert a deceleration point at that distance
+  const auto stop_distance = helper_.getFeasibleDecelDistance(0.0, false);
+  utils::avoidance::insertDecelPoint(
+    getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_);
 }
 
 void AvoidanceModule::insertStopPoint(
@@ -3333,19 +3354,16 @@ void AvoidanceModule::insertStopPoint(
   const auto stop_distance =
     calcSignedArcLength(shifted_path.path.points, getEgoPosition(), stop_idx);
 
-  // Insert deceleration point at stop distance without deceleration if use_constraints_for_decel is
-  // false
+  // If we don't need to consider deceleration constraints, insert a deceleration point
+  // and return immediately
   if (!use_constraints_for_decel) {
     utils::avoidance::insertDecelPoint(
       getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_);
     return;
   }
 
   // Otherwise, consider deceleration constraints before inserting deceleration point
-  const auto decel_distance = helper_.getMildDecelDistance(0.0);
-  if (!decel_distance) {
-    return;
-  }
+  const auto decel_distance = helper_.getFeasibleDecelDistance(0.0, false);
   if (stop_distance < decel_distance) {
     return;
   }
@@ -3368,14 +3386,12 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const
     return;
   }
 
-  const auto decel_distance = helper_.getMildDecelDistance(p->yield_velocity);
-  if (decel_distance) {
-    utils::avoidance::insertDecelPoint(
-      getEgoPosition(), *decel_distance, p->yield_velocity, shifted_path.path, slow_pose_);
-  }
+  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(const bool avoidable, ShiftedPath & shifted_path) const
+void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
 {
   const auto & data = avoidance_data_;
 
@@ -3393,15 +3409,9 @@ void AvoidanceModule::insertPrepareVelocity(const bool avoidable, ShiftedPath &
     return;
   }
 
-  if (avoidable) {
-    return;
-  }
-
   const auto decel_distance = helper_.getFeasibleDecelDistance(0.0);
-  if (decel_distance) {
-    utils::avoidance::insertDecelPoint(
-      getEgoPosition(), *decel_distance, 0.0, shifted_path.path, slow_pose_);
-  }
+  utils::avoidance::insertDecelPoint(
+    getEgoPosition(), decel_distance, 0.0, shifted_path.path, slow_pose_);
 }
 
 std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const

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

@@ -600,6 +600,33 @@ void fillAvoidanceNecessity(
   object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor);
 }
 
+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) {
+    object_data.is_stoppable = true;
+  }
+
+  const auto id = object_data.object.object_id;
+  const auto same_id_obj = std::find_if(
+    registered_objects.begin(), registered_objects.end(),
+    [&id](const auto & o) { return o.object.object_id == id; });
+
+  const auto is_stoppable = object_data.to_stop_line > feasible_stop_distance;
+  if (is_stoppable) {
+    object_data.is_stoppable = true;
+    return;
+  }
+
+  if (same_id_obj == registered_objects.end()) {
+    object_data.is_stoppable = false;
+    return;
+  }
+
+  object_data.is_stoppable = same_id_obj->is_stoppable;
+}
+
 void updateRegisteredObject(
   ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters)