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

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

@@ -1085,9 +1085,29 @@
   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::shortest_distance_from_ego_footprint_to_objects_on_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_goal_planner_module/src/util.cpp

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -323,19 +323,7 @@
   if (target_objects.objects.empty()) {
     return false;
   }
 
-  // check collision roughly with {min_distance, max_distance} between ego footprint and objects
-  // footprint
-  std::pair<bool, bool> has_collision_rough =
-    utils::path_safety_checker::checkObjectsCollisionRough(
-      path, target_objects, collision_check_margin, behavior_path_parameters, false);
-  if (!has_collision_rough.first) {
-    return false;
-  }
-  if (has_collision_rough.second) {
-    return true;
-  }
-
   std::vector<Polygon2d> obj_polygons;
   for (const auto & object : target_objects.objects) {
     obj_polygons.push_back(autoware_utils::to_polygon2d(object));

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

@@ -252,35 +252,70 @@ double calc_obstacle_min_length(const Shape & shape);
 double calc_obstacle_max_length(const Shape & shape);
 
 /**
- * @brief Calculate collision roughly by comparing minimum/maximum distance with margin.
- * @param path The path of the ego vehicle.
- * @param objects The predicted objects.
- * @param margin Distance margin to judge collision.
- * @param parameters The common parameters used in behavior path planner.
- * @param use_offset_ego_point If true, the closest point to the object is calculated by
- * interpolating the path points.
- * @return Collision (rough) between minimum distance and maximum distance
+ * @brief Performs efficient rough collision check between ego vehicle and objects
+ * @details
+ * This function calculates two types of distances between ego vehicle and each object:
+ * - Minimum distance: The shortest possible distance between ego and object when positioned
+ * diagonally with their corners closest to each other (uses maximum extent of both objects)
+ * - Maximum distance: The largest possible distance between ego and object when positioned parallel
+ *   to each other (uses minimum extent of both objects)
+ * A collision is detected when:
+ * - min_distance < min_margin_threshold (first element of returned pair is true)
+ * - max_distance < max_margin_threshold (second element of returned pair is true)
+ * This approach provides a computationally efficient rough collision check that can be used
+ * before performing more expensive precise collision check algorithms.
+ * @param path The path of the ego vehicle
+ * @param objects The predicted objects to check for collision
+ * @param min_margin_threshold Threshold for minimum distance collision check
+ * @param max_margin_threshold Threshold for maximum distance collision check
+ * @param parameters The common parameters used in behavior path planner
+ * @param use_offset_ego_point If true, uses interpolated point on path closest to object
+ * @return A pair of boolean values {min_distance_collision, max_distance_collision}
  */
 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);
+
+/**
+ * @brief Calculate the shortest rough distance between ego vehicle and objects based on specific
+ * orientation cases
+ * @details
+ * This function calculates a rough estimate of the closest distance between the ego vehicle's path
+ * and objects by considering two specific orientation cases:
+ * - "min": Uses the maximum extents of both ego and object (diagonal orientation case)
+ *   This calculates the worst-case, shortest possible distance when objects are oriented to
+ *   minimize the gap between them (when corners face each other)
+ * - "max": Uses the minimum extents of both ego and object (parallel orientation case)
+ *   This calculates the best-case, longest possible distance when objects are oriented to
+ *   maximize the gap between them (when sides are parallel)
+ * @param path The path of the ego vehicle
+ * @param objects The predicted objects to calculate distance to
+ * @param parameters The common parameters used in behavior path planner
+ * @param use_offset_ego_point If true, uses interpolated point on path closest to object for more
+ * accurate calculation
+ * @param distance_type Either "min" or "max" to specify which orientation case to calculate
+ * @return The shortest rough distance between the ego vehicle and any object for the specified
+ * orientation case
+ */
+double calculateRoughDistanceToObjects(
+  const PathWithLaneId & path, const PredictedObjects & objects,
+  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point,
+  const std::string & distance_type);
 
 /**
- * @brief Calculate the rough distance between the ego vehicle and the objects.
+ * @param Calculate the distance between the path and the closest object
  * @param path The path of the ego vehicle.
  * @param objects The predicted objects.
  * @param parameters The common parameters used in behavior path planner.
  * @param use_offset_ego_point If true, the closest point to the object is calculated by
  * interpolating the path points.
- * @param distance_type The type of distance to calculate. "min" or "max". Calculate the distance
- * when the distance is minimized or maximized when the direction of the ego and the object is
- * changed.
- * @return The rough distance between the ego vehicle and the objects.
+ * @return The distance between the ego vehicle and the closest object.
  */
-double calculateRoughDistanceToObjects(
+double shortest_distance_from_ego_footprint_to_objects_on_path(
   const PathWithLaneId & path, const PredictedObjects & objects,
-  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_point,
-  const std::string & distance_type);
+  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 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 @@
 }
 
 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 @@
     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,39 @@
   return min_distance;
 }
 
+double shortest_distance_from_ego_footprint_to_objects_on_path(
+  const PathWithLaneId & path, const PredictedObjects & objects,
+  const BehaviorPathPlannerParameters & parameters, const bool use_offset_ego_pose)
+{
+  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)