feat(start_planner): add surround moving obstacle check (autowarefoun…

…dation#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 <[email protected]>

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.
  *

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)
 {