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/config/start_planner/start_planner.param.yaml

@@ -138,6 +138,20 @@
           backward_path_length: 30.0
           forward_path_length: 100.0
 
+      surround_moving_obstacle_check:
+          search_radius: 10.0
+          th_moving_obstacle_velocity: 1.0
+          # ObjectTypesToCheck
+          object_types_to_check:
+            check_car: true
+            check_truck: true
+            check_bus: true
+            check_trailer: true
+            check_bicycle: true
+            check_motorcycle: true
+            check_pedestrian: true
+            check_unknown: false
+
       # debug
       debug:
         print_debug_info: false

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -137,6 +137,17 @@ class StartPlannerModule : public SceneModuleInterface
   void initializeSafetyCheckParameters();
 
   bool requiresDynamicObjectsCollisionDetection() const;
+
+  /**
+   * @brief Check if there are no moving objects around within a certain radius.
+   *
+   * This function filters the dynamic objects within a certain radius and then filters them by
+   * their velocity. If there are no moving objects around, it returns true. Otherwise, it returns
+   * false.
+   *
+   * @return True if there are no moving objects around. False otherwise.
+   */
+  bool noMovingObjectsAround() const;
   bool receivedNewRoute() const;
 
   bool isModuleRunning() const;

planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp

@@ -93,6 +93,12 @@ struct StartPlannerParameters
   utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{};
   utils::path_safety_checker::SafetyCheckParams safety_check_params{};
 
+  // surround moving obstacle check
+  double search_radius{0.0};
+  double th_moving_obstacle_velocity{0.0};
+  behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck
+    surround_moving_obstacles_type_to_check{};
+
   bool print_debug_info{false};
 };
 

planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp

@@ -273,6 +273,35 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
       node->declare_parameter<double>(rss_ns + "longitudinal_velocity_delta_time");
   }
 
+  // surround moving obstacle check
+  std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check.";
+  {
+    p.search_radius =
+      node->declare_parameter<double>(surround_moving_obstacle_check_ns + "search_radius");
+    p.th_moving_obstacle_velocity = node->declare_parameter<double>(
+      surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity");
+    // ObjectTypesToCheck
+    std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check.";
+    {
+      p.surround_moving_obstacles_type_to_check.check_car =
+        node->declare_parameter<bool>(obj_types_ns + "check_car");
+      p.surround_moving_obstacles_type_to_check.check_truck =
+        node->declare_parameter<bool>(obj_types_ns + "check_truck");
+      p.surround_moving_obstacles_type_to_check.check_bus =
+        node->declare_parameter<bool>(obj_types_ns + "check_bus");
+      p.surround_moving_obstacles_type_to_check.check_trailer =
+        node->declare_parameter<bool>(obj_types_ns + "check_trailer");
+      p.surround_moving_obstacles_type_to_check.check_unknown =
+        node->declare_parameter<bool>(obj_types_ns + "check_unknown");
+      p.surround_moving_obstacles_type_to_check.check_bicycle =
+        node->declare_parameter<bool>(obj_types_ns + "check_bicycle");
+      p.surround_moving_obstacles_type_to_check.check_motorcycle =
+        node->declare_parameter<bool>(obj_types_ns + "check_motorcycle");
+      p.surround_moving_obstacles_type_to_check.check_pedestrian =
+        node->declare_parameter<bool>(obj_types_ns + "check_pedestrian");
+    }
+  }
+
   // debug
   std::string debug_ns = ns + "debug.";
   {

planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp

@@ -181,6 +181,21 @@ bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const
   return parameters_->safety_check_params.enable_safety_check && status_.driving_forward;
 }
 
+bool StartPlannerModule::noMovingObjectsAround() const
+{
+  auto dynamic_objects = *(planner_data_->dynamic_object);
+  utils::path_safety_checker::filterObjectsWithinRadius(
+    dynamic_objects, planner_data_->self_odometry->pose.pose.position, parameters_->search_radius);
+  utils::path_safety_checker::filterObjectsByClass(
+    dynamic_objects, parameters_->surround_moving_obstacles_type_to_check);
+  const auto filtered_objects = utils::path_safety_checker::filterObjectsByVelocity(
+    dynamic_objects, parameters_->th_moving_obstacle_velocity, false);
+  if (!filtered_objects.objects.empty()) {
+    DEBUG_PRINT("Moving objects exist in the safety check area");
+  }
+  return filtered_objects.objects.empty();
+}
+
 bool StartPlannerModule::hasCollisionWithDynamicObjects() const
 {
   // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid
@@ -273,20 +288,24 @@ bool StartPlannerModule::isStopped()
 bool StartPlannerModule::isExecutionReady() const
 {
   bool is_safe = true;
-
   // Evaluate safety. The situation is not safe if any of the following conditions are met:
   // 1. pull out path has not been found
-  // 2. waiting for approval and there is a collision with dynamic objects
+  // 2. there is a moving objects around ego
+  // 3. waiting for approval and there is a collision with dynamic objects
   if (!status_.found_pull_out_path) {
     is_safe = false;
+  } else if (isWaitingApproval()) {
+    if (!noMovingObjectsAround()) {
+      is_safe = false;
+    } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) {
+      is_safe = false;
+    }
   }
 
-  if (requiresDynamicObjectsCollisionDetection() && isWaitingApproval()) {
-    is_safe = !hasCollisionWithDynamicObjects();
+  if (!is_safe) {
+    stop_pose_ = planner_data_->self_odometry->pose.pose;
   }
 
-  if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose;
-
   return is_safe;
 }
 

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