feat(avoidance): improve avoidance stop & decel behavior (autowarefou…

…ndation#4141)

* feat(path_shifter): add utils

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

* feat(avoidance): shorten avoidance stop distance

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

* feat(avoidance): insert slow down speed

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

* fix(avoidance): don't set stoppable=true for objects that ego doesn't have to avoid

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

* refactor(avoidance): rename unreadable variable name

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

* docs(avoidance): add new parameter description

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

@@ -145,6 +145,8 @@
           min_avoidance_distance: 10.0                  # [m]
           min_nominal_avoidance_speed: 7.0              # [m/s]
           min_sharp_avoidance_speed: 1.0                # [m/s]
+          min_slow_down_speed: 1.38                     # [m/s]
+          buf_slow_down_speed: 0.56                     # [m/s]
 
       # For yield maneuver
       yield:
@@ -154,6 +156,7 @@
       stop:
         min_distance: 10.0                              # [m]
         max_distance: 20.0                              # [m]
+        stop_buffer: 1.0                                # [m]
 
       constraints:
         # vehicle slows down under longitudinal constraints

planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md

@@ -658,14 +658,16 @@ namespace: `avoidance.avoidance.lateral.`
 
 namespace: `avoidance.avoidance.longitudinal.`
 
-| Name                                 | Unit  | Type   | Description                                                                                            | Default value |
-| :----------------------------------- | :---- | :----- | :----------------------------------------------------------------------------------------------------- | :------------ |
-| prepare_time                         | [s]   | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.          | 2.0           |
-| longitudinal_collision_safety_buffer | [s]   | double | Longitudinal collision buffer between target object and shift line.                                    | 0.0           |
-| min_prepare_distance                 | [m]   | double | Minimum distance for "prepare_time" x "ego_speed".                                                     | 1.0           |
-| min_avoidance_distance               | [m]   | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low) | 10.0          |
-| min_nominal_avoidance_speed          | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1).                                       | 7.0           |
-| min_sharp_avoidance_speed            | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                         | 1.0           |
+| Name                                 | Unit  | Type   | Description                                                                                                                                                                                                                                                                                                                                                       | Default value  |
+| :----------------------------------- | :---- | :----- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- |
+| prepare_time                         | [s]   | double | Avoidance shift starts from point ahead of this time x ego_speed to avoid sudden path change.                                                                                                                                                                                                                                                                     | 2.0            |
+| longitudinal_collision_safety_buffer | [s]   | double | Longitudinal collision buffer between target object and shift line.                                                                                                                                                                                                                                                                                               | 0.0            |
+| min_prepare_distance                 | [m]   | double | Minimum distance for "prepare_time" x "ego_speed".                                                                                                                                                                                                                                                                                                                | 1.0            |
+| min_avoidance_distance               | [m]   | double | Minimum distance of avoidance path (i.e. this distance is needed even if its lateral jerk is very low)                                                                                                                                                                                                                                                            | 10.0           |
+| min_nominal_avoidance_speed          | [m/s] | double | Minimum speed for jerk calculation in a nominal situation (\*1).                                                                                                                                                                                                                                                                                                  | 7.0            |
+| min_sharp_avoidance_speed            | [m/s] | double | Minimum speed for jerk calculation in a sharp situation (\*1).                                                                                                                                                                                                                                                                                                    | 1.0            |
+| min_slow_down_speed                  | [m/s] | double | Minimum slow speed for avoidance prepare section.                                                                                                                                                                                                                                                                                                                 | 1.38 (5km/h)   |
+| buf_slow_down_speed                  | [m/s] | double | Buffer for controller tracking error. Basically, vehicle always cannot follow velocity profile precisely. Therefore, the module inserts lower speed than target speed that satisfies conditions to avoid object within accel/jerk constraints so that the avoidance path always can be output even if the current speed is a little bit higher than target speed. | 0.57 (2.0km/h) |
 
 ### Yield maneuver parameters
 
@@ -683,6 +685,7 @@ namespace: `avoidance.stop.`
 | :----------- | :--- | :----- | :---------------------------------------------------------------------------------------------------- | :------------ |
 | min_distance | [m]  | double | Minimum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 10.0          |
 | max_distance | [m]  | double | Maximum stop distance in the situation where avoidance maneuver is not approved or in yield maneuver. | 20.0          |
+| stop_buffer  | [m]  | double | Buffer distance in the situation where avoidance maneuver is not approved or in yield maneuver.       | 1.0           |
 
 ### Constraints parameters
 

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

@@ -199,6 +199,9 @@ struct AvoidanceParameters
   // maximum stop distance
   double stop_max_distance;
 
+  // stop buffer
+  double stop_buffer;
+
   // start avoidance after this time to avoid sudden path change
   double prepare_time;
 
@@ -219,6 +222,12 @@ struct AvoidanceParameters
   // distance for avoidance. Need a sharp avoidance path to avoid the object.
   double min_sharp_avoidance_speed;
 
+  // minimum slow down speed
+  double min_slow_down_speed;
+
+  // slow down speed buffer
+  double buf_slow_down_speed;
+
   // 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};

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

@@ -74,13 +74,6 @@ class AvoidanceHelper
     return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed);
   }
 
-  float getMaximumAvoidanceEgoSpeed() const
-  {
-    return parameters_->target_velocity_matrix.at(parameters_->col_size - 1);
-  }
-
-  float getMinimumAvoidanceEgoSpeed() const { return parameters_->target_velocity_matrix.front(); }
-
   double getNominalPrepareDistance() const
   {
     const auto & p = parameters_;
@@ -103,7 +96,7 @@ class AvoidanceHelper
   {
     const auto & p = parameters_;
     const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk(
-      shift_length, p->nominal_lateral_jerk, getMinimumAvoidanceEgoSpeed());
+      shift_length, p->max_lateral_jerk, p->min_sharp_avoidance_speed);
 
     return std::max(p->min_avoidance_distance, distance_by_jerk);
   }

planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp

@@ -136,6 +136,9 @@ class PathShifter
   // Utility Functions
   ////////////////////////////////////////
 
+  static double calcFeasibleVelocityFromJerk(
+    const double lateral, const double jerk, const double distance);
+
   static double calcLongitudinalDistFromJerk(
     const double lateral, const double jerk, const double velocity);
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -643,6 +643,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     p.min_avoidance_distance = declare_parameter<double>(ns + "min_avoidance_distance");
     p.min_nominal_avoidance_speed = declare_parameter<double>(ns + "min_nominal_avoidance_speed");
     p.min_sharp_avoidance_speed = declare_parameter<double>(ns + "min_sharp_avoidance_speed");
+    p.min_slow_down_speed = declare_parameter<double>(ns + "min_slow_down_speed");
+    p.buf_slow_down_speed = declare_parameter<double>(ns + "buf_slow_down_speed");
   }
 
   // yield
@@ -656,6 +658,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     std::string ns = "avoidance.stop.";
     p.stop_min_distance = declare_parameter<double>(ns + "min_distance");
     p.stop_max_distance = declare_parameter<double>(ns + "max_distance");
+    p.stop_buffer = declare_parameter<double>(ns + "stop_buffer");
   }
 
   // constraints

planning/behavior_path_planner/src/marker_util/avoidance/debug.cpp

@@ -104,9 +104,11 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
       marker.id = uuidToInt32(object.object.object_id);
       marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
       std::ostringstream string_stream;
-      string_stream << std::fixed << std::setprecision(2);
+      string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
       string_stream << "ratio:" << object.shiftable_ratio << " [-]\n"
-                    << "lateral: " << object.lateral << " [-]\n"
+                    << "lateral:" << object.lateral << " [m]\n"
+                    << "necessity:" << object.avoid_required << " [-]\n"
+                    << "stoppable:" << object.is_stoppable << " [-]\n"
                     << "stop_factor:" << object.to_stop_factor_distance << " [m]\n"
                     << "move_time:" << object.move_time << " [s]\n"
                     << "stop_time:" << object.stop_time << " [s]\n";

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

@@ -397,8 +397,8 @@ ObjectData AvoidanceModule::createObjectData(
   const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
   const auto object_closest_index = findNearestIndex(path_points, object_pose.position);
   const auto object_closest_pose = path_points.at(object_closest_index).point.pose;
-  const auto t = utils::getHighestProbLabel(object.classification);
-  const auto object_parameter = parameters_->object_parameters.at(t);
+  const auto object_type = utils::getHighestProbLabel(object.classification);
+  const auto object_parameter = parameters_->object_parameters.at(object_type);
 
   ObjectData object_data{};
 
@@ -640,8 +640,8 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
   }
 
   const auto o_front = data.stop_target_object.get();
-  const auto t = utils::getHighestProbLabel(o_front.object.classification);
-  const auto object_parameter = parameters_->object_parameters.at(t);
+  const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
+  const auto object_parameter = parameters_->object_parameters.at(object_type);
   const auto & base_link2front = planner_data_->parameters.base_link2front;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
@@ -689,6 +689,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
     return;
   }
 
+  insertPrepareVelocity(path);
+
   switch (data.state) {
     case AvoidanceState::NOT_AVOID: {
       break;
@@ -700,7 +702,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
       break;
     }
     case AvoidanceState::AVOID_PATH_NOT_READY: {
-      insertPrepareVelocity(path);
       insertWaitPoint(parameters_->use_constraints_for_decel, path);
       break;
     }
@@ -931,8 +932,8 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(
     const auto nominal_return_distance = helper_.getNominalAvoidanceDistance(return_shift);
 
     // use each object param
-    const auto t = utils::getHighestProbLabel(o.object.classification);
-    const auto object_parameter = parameters_->object_parameters.at(t);
+    const auto object_type = utils::getHighestProbLabel(o.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
 
     /**
      * Is there enough distance from ego to object for avoidance?
@@ -3317,15 +3318,15 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
   // D4: o_front.longitudinal
   // D5: base_link2front
 
-  const auto t = utils::getHighestProbLabel(object.object.classification);
-  const auto object_parameter = parameters_->object_parameters.at(t);
+  const auto object_type = utils::getHighestProbLabel(object.object.classification);
+  const auto object_parameter = parameters_->object_parameters.at(object_type);
 
   const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
                             object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
   const auto variable = helper_.getMinimumAvoidanceDistance(
     helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
-  const auto constant =
-    p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + base_link2front;
+  const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal +
+                        base_link2front + p->stop_buffer;
 
   return object.longitudinal -
          std::clamp(variable + constant, p->stop_min_distance, p->stop_max_distance);
@@ -3436,23 +3437,64 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
 {
   const auto & data = avoidance_data_;
 
+  // do nothing if there is no avoidance target.
   if (data.target_objects.empty()) {
     return;
   }
 
-  if (!!data.stop_target_object) {
-    if (data.stop_target_object.get().reason != AvoidanceDebugFactor::TOO_LARGE_JERK) {
-      return;
-    }
+  // insert slow down speed only when the avoidance maneuver is not initiated.
+  if (data.avoiding_now) {
+    return;
   }
 
-  if (data.avoiding_now) {
+  // insert slow down speed only when no shift line is approved.
+  if (!path_shifter_.getShiftLines().empty()) {
     return;
   }
 
-  const auto decel_distance = helper_.getFeasibleDecelDistance(0.0);
-  utils::avoidance::insertDecelPoint(
-    getEgoPosition(), decel_distance, 0.0, shifted_path.path, slow_pose_);
+  const auto object = data.target_objects.front();
+
+  // 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_parameter = parameters_->object_parameters.at(object_type);
+  const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
+                            object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
+  const auto shift_length =
+    helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);
+
+  // check slow down feasibility
+  const auto min_avoid_distance = helper_.getMinimumAvoidanceDistance(shift_length);
+  const auto distance_to_object = object.longitudinal;
+  const auto remaining_distance = distance_to_object - min_avoid_distance;
+  const auto decel_distance =
+    helper_.getFeasibleDecelDistance(parameters_->min_sharp_avoidance_speed);
+  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;
+
+  // insert slow down speed.
+  const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
+  for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
+    const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
+
+    // slow down speed is inserted only in front of the object.
+    const auto shift_longitudinal_distance = distance_to_object - distance_from_ego;
+    if (shift_longitudinal_distance < min_avoid_distance) {
+      break;
+    }
+
+    // target speed with nominal jerk limits.
+    const double v_target = PathShifter::calcFeasibleVelocityFromJerk(
+      shift_length, parameters_->nominal_lateral_jerk, shift_longitudinal_distance);
+    const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
+    const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed);
+
+    shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert);
+  }
 }
 
 std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const

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

@@ -90,12 +90,15 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
   {
     const std::string ns = "avoidance.avoidance.longitudinal.";
     updateParam<double>(parameters, ns + "prepare_time", p->prepare_time);
+    updateParam<double>(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed);
+    updateParam<double>(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed);
   }
 
   {
     const std::string ns = "avoidance.stop.";
     updateParam<double>(parameters, ns + "max_distance", p->stop_max_distance);
     updateParam<double>(parameters, ns + "min_distance", p->stop_min_distance);
+    updateParam<double>(parameters, ns + "stop_buffer", p->stop_buffer);
   }
 
   {