From 678199900c37ae38994a9a3c880853457198a653 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 7 Dec 2023 01:04:35 +0900 Subject: [PATCH] feat(start_planner): add surround moving obstacle check (#5782) * feat(start_planner): add surround moving obstacle check This commit introduces a new feature in the start_planner module for checking surrounding moving obstacles. - It adds parameters to specify the search radius and threshold velocity for moving obstacles, along with flags to indicate which types of objects should be checked. - The `noMovingObjectsAround` function has been added to filter dynamic objects within a certain radius based on their velocity. - If no moving objects are detected, the function returns true; otherwise, it returns false. - This feature enhances the safety of the start_planner by ensuring that the path can't be approved while surrond moving obstacles exist. --------- Signed-off-by: kyoichi-sugahara --- .../path_safety_checker/objects_filtering.hpp | 13 ++++++++++++ .../path_safety_checker/objects_filtering.cpp | 21 +++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 018e6bd2a3d6e..f66f425f1b7fa 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -134,6 +134,19 @@ void filterObjectsByPosition( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); +/** + * @brief Filter objects based on their distance from a reference point. + * + * This function filters out objects that are outside a specified radius from a reference point. + * + * @param objects The predicted objects to filter. + * @param reference_point The reference point (e.g., current pose of the ego vehicle). + * @param search_radius The radius within which to retain objects. + */ +void filterObjectsWithinRadius( + PredictedObjects & objects, const geometry_msgs::msg::Point & reference_point, + const double search_radius); + /** * @brief Filters the provided objects based on their classification. * diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 84c02b3b325de..188c50993e4ae 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -44,6 +44,15 @@ bool position_filter( return (backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); } + +bool is_within_circle( + const geometry_msgs::msg::Point & object_pos, const geometry_msgs::msg::Point & reference_point, + const double search_radius) +{ + const double dist = + std::hypot(reference_point.x - object_pos.x, reference_point.y - object_pos.y); + return dist < search_radius; +} } // namespace behavior_path_planner::utils::path_safety_checker::filter namespace behavior_path_planner::utils::path_safety_checker @@ -128,6 +137,18 @@ void filterObjectsByPosition( filterObjects(objects, filter); } +void filterObjectsWithinRadius( + PredictedObjects & objects, const geometry_msgs::msg::Point & reference_point, + const double search_radius) +{ + const auto filter = [&](const auto & object) { + return filter::is_within_circle( + object.kinematics.initial_pose_with_covariance.pose.position, reference_point, search_radius); + }; + + filterObjects(objects, filter); +} + void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) {