diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index bca1105f4b4cc..9cec6e42bc8d7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1085,9 +1085,29 @@ 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 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()); if (it == margins_with_zero.end()) { path_id_to_rough_margin_map[path.id()] = margins_with_zero.back(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 8b2ed3bdf9e71..6cad6413495f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -324,18 +324,6 @@ bool checkObjectsCollision( return false; } - // check collision roughly with {min_distance, max_distance} between ego footprint and objects - // footprint - std::pair 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 obj_polygons; for (const auto & object : target_objects.objects) { obj_polygons.push_back(autoware_utils::to_polygon2d(object)); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index d642a8528efc5..19e9c6fbc5391 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/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 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( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 1be955bedcec6..276c892dd57d3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/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 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 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,39 @@ double calculateRoughDistanceToObjects( 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::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) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp index cf5cf25497dfe..cf92bbcf0bfb5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -676,20 +676,22 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, checkObjectsCollisionRough) auto path = generateTrajectory(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; @@ -697,26 +699,43 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, checkObjectsCollisionRough) 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)