feat(avoidance): expand safety buffer by distance

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

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

@@ -316,6 +316,9 @@ struct ObjectData  // avoidance target
   // lateral shiftable ratio
   double shiftable_ratio{0.0};
 
+  // distance factor for perception noise (0.0~1.0)
+  double distance_factor{0.0};
+
   // count up when object disappeared. Removed when it exceeds threshold.
   rclcpp::Time last_seen;
   double lost_time{0.0};

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

@@ -88,7 +88,7 @@ void insertDecelPoint(
 
 void fillObjectEnvelopePolygon(
   ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose,
-  const Pose & ego_pose, const std::shared_ptr<AvoidanceParameters> & parameters);
+  const std::shared_ptr<AvoidanceParameters> & parameters);
 
 void fillObjectMovingTime(
   ObjectData & object_data, ObjectDataArray & stopped_objects,

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

@@ -372,14 +372,22 @@ 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);
 
   ObjectData object_data{};
 
   object_data.object = object;
 
+  const auto lower = parameters_->lower_distance_for_polygon_expansion;
+  const auto upper = parameters_->upper_distance_for_polygon_expansion;
+  const auto clamp =
+    std::clamp(calcDistance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper;
+  object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0;
+
   // Calc envelop polygon.
   utils::avoidance::fillObjectEnvelopePolygon(
-    object_data, registered_objects_, object_closest_pose, getEgoPose(), parameters_);
+    object_data, registered_objects_, object_closest_pose, parameters_);
 
   // calc object centroid.
   object_data.centroid = return_centroid<Point2d>(object_data.envelope_poly);
@@ -395,10 +403,9 @@ ObjectData AvoidanceModule::createObjectData(
     object_data, object_closest_pose, object_data.overhang_pose.position);
 
   // Check whether the the ego should avoid the object.
-  const auto t = utils::getHighestProbLabel(object.classification);
-  const auto object_parameter = parameters_->object_parameters.at(t);
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
-  const auto safety_margin = 0.5 * vehicle_width + object_parameter.safety_buffer_lateral;
+  const auto safety_margin =
+    0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;
   object_data.avoid_required =
     (utils::avoidance::isOnRight(object_data) &&
      std::abs(object_data.overhang_dist) < safety_margin) ||
@@ -602,7 +609,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat
   const auto & base_link2front = planner_data_->parameters.base_link2front;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
-  const auto max_avoid_margin = object_parameter.safety_buffer_lateral +
+  const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
                                 parameters_->lateral_collision_margin + 0.5 * vehicle_width;
 
   const auto variable = helper_.getSharpAvoidanceDistance(
@@ -3265,8 +3272,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
   const auto t = utils::getHighestProbLabel(object.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(t);
 
-  const auto avoid_margin =
-    object_parameter.safety_buffer_lateral + p->lateral_collision_margin + 0.5 * vehicle_width;
+  const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
+                            p->lateral_collision_margin + 0.5 * vehicle_width;
   const auto variable = helper_.getMinimumAvoidanceDistance(
     helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
   const auto constant =

planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp

@@ -181,20 +181,29 @@ ObjectData AvoidanceByLaneChange::createObjectData(
   const AvoidancePlanningData & data, const PredictedObject & object) const
 {
   using boost::geometry::return_centroid;
+  using motion_utils::findNearestIndex;
+  using tier4_autoware_utils::calcDistance2d;
 
   const auto & path_points = data.reference_path.points;
   const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
-  const auto object_closest_index =
-    motion_utils::findNearestIndex(path_points, object_pose.position);
+  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 = avoidance_parameters_->object_parameters.at(t);
 
   ObjectData object_data{};
 
   object_data.object = object;
 
+  const auto lower = avoidance_parameters_->lower_distance_for_polygon_expansion;
+  const auto upper = avoidance_parameters_->upper_distance_for_polygon_expansion;
+  const auto clamp =
+    std::clamp(calcDistance2d(getEgoPose(), object_pose) - lower, 0.0, upper) / upper;
+  object_data.distance_factor = object_parameter.max_expand_ratio * clamp + 1.0;
+
   // Calc envelop polygon.
   utils::avoidance::fillObjectEnvelopePolygon(
-    object_data, registered_objects_, object_closest_pose, getEgoPose(), avoidance_parameters_);
+    object_data, registered_objects_, object_closest_pose, avoidance_parameters_);
 
   // calc object centroid.
   object_data.centroid = return_centroid<Point2d>(object_data.envelope_poly);
@@ -211,10 +220,9 @@ ObjectData AvoidanceByLaneChange::createObjectData(
     object_data, object_closest_pose, object_data.overhang_pose.position);
 
   // Check whether the the ego should avoid the object.
-  const auto t = utils::getHighestProbLabel(object.classification);
-  const auto object_parameter = avoidance_parameters_->object_parameters.at(t);
   const auto vehicle_width = planner_data_->parameters.vehicle_width;
-  const auto safety_margin = 0.5 * vehicle_width + object_parameter.safety_buffer_lateral;
+  const auto safety_margin =
+    0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor;
   object_data.avoid_required =
     (utils::avoidance::isOnRight(object_data) &&
      std::abs(object_data.overhang_dist) < safety_margin) ||

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

@@ -483,22 +483,15 @@ void insertDecelPoint(
 
 void fillObjectEnvelopePolygon(
   ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose,
-  const Pose & ego_pose, const std::shared_ptr<AvoidanceParameters> & parameters)
+  const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   using boost::geometry::within;
 
   const auto t = utils::getHighestProbLabel(object_data.object.classification);
   const auto object_parameter = parameters->object_parameters.at(t);
 
-  const auto & obj_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
-  const auto lower = parameters->lower_distance_for_polygon_expansion;
-  const auto upper = parameters->upper_distance_for_polygon_expansion;
-  const auto distance_factor = object_parameter.max_expand_ratio *
-                               std::clamp(calcDistance2d(ego_pose, obj_pose) - lower, 0.0, upper) /
-                               upper;
-
   const auto & envelope_buffer_margin =
-    object_parameter.envelope_buffer_margin * (1.0 + distance_factor);
+    object_parameter.envelope_buffer_margin * object_data.distance_factor;
 
   const auto id = object_data.object.object_id;
   const auto same_id_obj = std::find_if(
@@ -781,7 +774,7 @@ void filterTargetObjects(
 
     // calculate avoid_margin dynamically
     // NOTE: This calculation must be after calculating to_road_shoulder_distance.
-    const double max_avoid_margin = object_parameter.safety_buffer_lateral +
+    const double max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor +
                                     parameters->lateral_collision_margin + 0.5 * vehicle_width;
     const double min_safety_lateral_distance =
       object_parameter.safety_buffer_lateral + 0.5 * vehicle_width;