fix(goal_planner): use precise distance to objects for sorting candidate paths

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Author: kosuke55

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1085,9 +1085,28 @@ void sortPullOverPaths(
   const auto & target_objects = static_target_objects;
   for (const size_t i : sorted_path_indices) {
     const auto & path = pull_over_path_candidates[i];
-    const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects(
-      path.parking_path(), target_objects, planner_data->parameters, false, "max");
-    auto it = std::lower_bound(
+
+    // check collision roughly with {min_distance, max_distance} between ego footprint and objects
+    // footprint
+    const std::pair<bool, bool> has_collision_rough =
+      utils::path_safety_checker::checkObjectsCollisionRough(
+        path.parking_path(), target_objects, soft_margins.front(), hard_margins.back(),
+        planner_data->parameters, false);
+    // min_distance > soft_margin.front() means no collision with any margin
+    if (!has_collision_rough.first) {
+      path_id_to_rough_margin_map[path.id()] = soft_margins.front();
+      continue;
+    }
+    // max_distance < hard_margin.front() means collision with any margin
+    if (has_collision_rough.second) {
+      path_id_to_rough_margin_map[path.id()] = 0.0;
+      continue;
+    }
+    // calculate the precise distance to object footprint from the path footprint
+    const double distance = utils::path_safety_checker::calculate_distance_to_objects_from_path(
+      path.parking_path(), target_objects, planner_data->parameters, true);
+
+    const auto it = std::lower_bound(
       margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater<double>());
     if (it == margins_with_zero.end()) {
       path_id_to_rough_margin_map[path.id()] = margins_with_zero.back();

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp

@@ -281,6 +281,11 @@ double calculateRoughDistanceToObjects(
   const PathWithLaneId & path, const PredictedObjects & objects,
   const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point,
   const std::string & distance_type);
+
+double calculate_distance_to_objects_from_path(
+  const PathWithLaneId & path, const PredictedObjects & objects,
+  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_pose);
+
 // debug
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
 void updateCollisionCheckDebugMap(

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp

@@ -41,6 +41,7 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker
 namespace bg = boost::geometry;
 
 using autoware::motion_utils::calcLongitudinalOffsetPoint;
+using autoware::motion_utils::calcLongitudinalOffsetPose;
 using autoware::motion_utils::calcLongitudinalOffsetToSegment;
 using autoware::motion_utils::findNearestIndex;
 using autoware::motion_utils::findNearestSegmentIndex;
@@ -774,8 +775,9 @@ double calc_obstacle_max_length(const Shape & shape)
 }
 
 std::pair<bool, bool> checkObjectsCollisionRough(
-  const PathWithLaneId & path, const PredictedObjects & objects, const double margin,
-  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point)
+  const PathWithLaneId & path, const PredictedObjects & objects, const double min_margin_threshold,
+  const double max_margin_threshold, const BehaviorPathPlannerParameters & parameters,
+  const bool use_offset_ego_point)
 {
   const auto & points = path.points;
 
@@ -815,10 +817,10 @@ std::pair<bool, bool> checkObjectsCollisionRough(
     const double min_distance = distance - object_max_length - ego_max_length;
     const double max_distance = distance - object_min_length - ego_min_length;
 
-    if (min_distance < margin) {
+    if (min_distance < min_margin_threshold) {
       has_collision.first = true;
     }
-    if (max_distance < margin) {
+    if (max_distance < max_margin_threshold) {
       has_collision.second = true;
     }
   }
@@ -877,6 +879,42 @@ double calculateRoughDistanceToObjects(
   return min_distance;
 }
 
+double calculate_distance_to_objects_from_path(
+  const PathWithLaneId & path, const PredictedObjects & objects,
+  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_pose)
+
+{
+  if (objects.objects.empty()) return 0.0;
+
+  const auto & p = parameters;
+  double min_distance = std::numeric_limits<double>::max();
+  for (const auto & object : objects.objects) {
+    const auto & object_point = object.kinematics.initial_pose_with_covariance.pose.position;
+
+    const auto ego_pose = std::invoke([&]() -> Pose {
+      if (use_offset_ego_pose) {
+        const size_t nearest_segment_idx = findNearestSegmentIndex(path.points, object_point);
+        const double offset_length =
+          calcLongitudinalOffsetToSegment(path.points, nearest_segment_idx, object_point);
+        const auto ego_pose_opt =
+          calcLongitudinalOffsetPose(path.points, nearest_segment_idx, offset_length);
+        if (ego_pose_opt.has_value()) return ego_pose_opt.value();
+      }
+      const auto ego_nearest_idx = findNearestIndex(path.points, object_point);
+      return path.points.at(ego_nearest_idx).point.pose;
+    });
+
+    const auto ego_footprint =
+      autoware_utils::to_footprint(ego_pose, p.base_link2front, p.base_link2rear, p.vehicle_width);
+
+    const double distance =
+      boost::geometry::distance(ego_footprint, autoware_utils::to_polygon2d(object));
+    min_distance = std::min(min_distance, distance);
+  }
+
+  return min_distance;
+}
+
 autoware_internal_planning_msgs::msg::SafetyFactorArray to_safety_factor_array(
   const CollisionCheckDebugMap & debug_map)
 {

planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp

@@ -676,47 +676,66 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, checkObjectsCollisionRough)
 
   auto path = generateTrajectory<PathWithLaneId>(10, 1.0);
   autoware_perception_msgs::msg::PredictedObjects objs;
-  double margin = 0.1;
+  double min_margin_threshold = 0.1;
+  double max_margin_threshold = 0.1;
   BehaviorPathPlannerParameters param;
   param.vehicle_width = 2.0;
   param.front_overhang = 1.0;
   param.rear_overhang = 1.0;
   bool use_offset_ego_point = true;
 
-  // Condition: no object
-  auto rough_object_collision =
-    checkObjectsCollisionRough(path, objs, margin, param, use_offset_ego_point);
+  // Condition: no objects
+  auto rough_object_collision = checkObjectsCollisionRough(
+    path, objs, min_margin_threshold, max_margin_threshold, param, use_offset_ego_point);
   EXPECT_FALSE(rough_object_collision.first);
   EXPECT_FALSE(rough_object_collision.second);
 
   // Condition: collides with minimum distance
+  // min_distance = 0.00464761, max_distance = 2.0
   autoware_perception_msgs::msg::PredictedObject obj;
   obj.kinematics.initial_pose_with_covariance.pose = createPose(8.0, 3.0, 0.0, 0.0, 0.0, 0.0);
   obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
   obj.shape.dimensions.x = 3.0;
   obj.shape.dimensions.y = 1.0;
   objs.objects.push_back(obj);
 
-  rough_object_collision =
-    checkObjectsCollisionRough(path, objs, margin, param, use_offset_ego_point);
+  rough_object_collision = checkObjectsCollisionRough(
+    path, objs, min_margin_threshold, max_margin_threshold, param, use_offset_ego_point);
   EXPECT_TRUE(rough_object_collision.first);
   EXPECT_FALSE(rough_object_collision.second);
 
   // Condition: collides with both distance
+  // min_distance: -1.99535, max_distance: 0.0
   obj.kinematics.initial_pose_with_covariance.pose = createPose(2.0, 1.0, 0.0, 0.0, 0.0, 0.0);
   objs.objects.clear();
   objs.objects.push_back(obj);
-  rough_object_collision =
-    checkObjectsCollisionRough(path, objs, margin, param, use_offset_ego_point);
+  rough_object_collision = checkObjectsCollisionRough(
+    path, objs, min_margin_threshold, max_margin_threshold, param, use_offset_ego_point);
   EXPECT_TRUE(rough_object_collision.first);
   EXPECT_TRUE(rough_object_collision.second);
 
   // Condition: use_offset_ego_point set to false
   use_offset_ego_point = false;
-  rough_object_collision =
-    checkObjectsCollisionRough(path, objs, margin, param, use_offset_ego_point);
+  rough_object_collision = checkObjectsCollisionRough(
+    path, objs, min_margin_threshold, max_margin_threshold, param, use_offset_ego_point);
   EXPECT_TRUE(rough_object_collision.first);
   EXPECT_TRUE(rough_object_collision.second);
+
+  // Condition: no collision with lenient min_margin_threshold and
+  //            collision with strict max_margin_threshold.
+  // min_distance = 0.00464761, max_distance = 2.0
+  min_margin_threshold = 0.001;
+  max_margin_threshold = 2.1;
+  obj.kinematics.initial_pose_with_covariance.pose = createPose(8.0, 3.0, 0.0, 0.0, 0.0, 0.0);
+  obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX;
+  obj.shape.dimensions.x = 3.0;
+  obj.shape.dimensions.y = 1.0;
+  objs.objects.clear();
+  objs.objects.push_back(obj);
+  rough_object_collision = checkObjectsCollisionRough(
+    path, objs, min_margin_threshold, max_margin_threshold, param, use_offset_ego_point);
+  EXPECT_FALSE(rough_object_collision.first);
+  EXPECT_TRUE(rough_object_collision.second);
 }
 
 TEST(BehaviorPathPlanningSafetyUtilsTest, calculateRoughDistanceToObjects)