feat(behavior_path_planner): add safety check against dynamic objects…

… for start/goal planner (#4873) * add safety check for start/goal planner Signed-off-by: kyoichi-sugahara <[email protected]> * fix typo Signed-off-by: kyoichi-sugahara <[email protected]> * update comments Signed-off-by: kyoichi-sugahara <[email protected]> * add const Signed-off-by: kosuke55 <[email protected]> * fix empty check Signed-off-by: kyoichi-sugahara <[email protected]> --------- Signed-off-by: kyoichi-sugahara <[email protected]> Signed-off-by: kosuke55 <[email protected]> Co-authored-by: kosuke55 <[email protected]>
autowarefoundation · Sep 5, 2023 · a0ce39a · a0ce39a
1 parent d087680
commit a0ce39a
Show file tree

Hide file tree

Showing 11 changed files with 630 additions and 22 deletions.
diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml
@@ -104,6 +104,66 @@
             neighbor_radius: 8.0
             margin: 1.0
 
+      stop_condition:
+        maximum_deceleration_for_stop: 1.0
+        maximum_jerk_for_stop: 1.0
+      path_safety_check:
+        # EgoPredictedPath
+        ego_predicted_path:
+          acceleration: 1.0
+          time_horizon: 10.0
+          time_resolution: 0.5
+          min_slow_speed: 0.0
+          delay_until_departure: 1.0
+          target_velocity: 1.0
+        # For target object filtering
+        target_filtering:
+          safety_check_time_horizon: 5.0
+          safety_check_time_resolution: 1.0
+          # detection range
+          object_check_forward_distance: 10.0
+          object_check_backward_distance: 100.0
+          ignore_object_velocity_threshold: 0.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
+          # ObjectLaneConfiguration
+          object_lane_configuration:
+            check_current_lane: true
+            check_right_side_lane: true
+            check_left_side_lane: true
+            check_shoulder_lane: true
+            check_other_lane: false
+          include_opposite_lane: false
+          invert_opposite_lane: false
+          check_all_predicted_path: true
+          use_all_predicted_path: true
+          use_predicted_path_outside_lanelet: false
+
+        # For safety check
+        safety_check_params:
+          # safety check configuration
+          enable_safety_check: true
+          # collision check parameters
+          check_all_predicted_path: true
+          publish_debug_marker: false
+          rss_params:
+            rear_vehicle_reaction_time: 1.0
+            rear_vehicle_safety_time_margin: 1.0
+            lateral_distance_max_threshold: 1.0
+            longitudinal_distance_min_threshold: 1.0
+            longitudinal_velocity_delta_time: 1.0
+          # temporary
+          backward_path_length: 30.0
+          forward_path_length: 100.0
+
       # debug
       debug:
         print_debug_info: false
diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml
@@ -72,3 +72,63 @@
           max_planning_time: 150.0
           neighbor_radius: 8.0
           margin: 1.0
+
+      stop_condition:
+        maximum_deceleration_for_stop: 1.0
+        maximum_jerk_for_stop: 1.0
+      path_safety_check:
+        # EgoPredictedPath
+        ego_predicted_path:
+          acceleration: 1.0
+          time_horizon: 10.0
+          time_resolution: 0.5
+          min_slow_speed: 0.0
+          delay_until_departure: 1.0
+          target_velocity: 1.0
+        # For target object filtering
+        target_filtering:
+          safety_check_time_horizon: 5.0
+          safety_check_time_resolution: 1.0
+          # detection range
+          object_check_forward_distance: 10.0
+          object_check_backward_distance: 100.0
+          ignore_object_velocity_threshold: 0.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
+          # ObjectLaneConfiguration
+          object_lane_configuration:
+            check_current_lane: true
+            check_right_side_lane: true
+            check_left_side_lane: true
+            check_shoulder_lane: true
+            check_other_lane: false
+          include_opposite_lane: false
+          invert_opposite_lane: false
+          check_all_predicted_path: true
+          use_all_predicted_path: true
+          use_predicted_path_outside_lanelet: false
+
+        # For safety check
+        safety_check_params:
+          # safety check configuration
+          enable_safety_check: true
+          # collision check parameters
+          check_all_predicted_path: true
+          publish_debug_marker: false
+          rss_params:
+            rear_vehicle_reaction_time: 1.0
+            rear_vehicle_safety_time_margin: 1.0
+            lateral_distance_max_threshold: 1.0
+            longitudinal_distance_min_threshold: 1.0
+            longitudinal_velocity_delta_time: 1.0
+          # temporary
+          backward_path_length: 30.0
+          forward_path_length: 100.0
diff --git a/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
@@ -24,6 +24,9 @@
 #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp"
 #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp"
 #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
+#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <freespace_planning_algorithms/astar_search.hpp>
 #include <freespace_planning_algorithms/rrtstar.hpp>
@@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam;
 using freespace_planning_algorithms::RRTStar;
 using freespace_planning_algorithms::RRTStarParam;
 
+using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
+using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
+using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
+using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
+using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
+
 enum class PathType {
   NONE = 0,
   SHIFT,
@@ -107,6 +116,13 @@ class GoalPlannerModule : public SceneModuleInterface
   void updateModuleParams(const std::any & parameters) override
   {
     parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
+    if (parameters_->safety_check_params.enable_safety_check) {
+      ego_predicted_path_params_ =
+        std::make_shared<EgoPredictedPathParams>(parameters_->ego_predicted_path_params);
+      objects_filtering_params_ =
+        std::make_shared<ObjectsFilteringParams>(parameters_->objects_filtering_params);
+      safety_check_params_ = std::make_shared<SafetyCheckParams>(parameters_->safety_check_params);
+    }
   }
 
   // TODO(someone): remove this, and use base class function
@@ -137,8 +153,14 @@ class GoalPlannerModule : public SceneModuleInterface
 
   PullOverStatus status_;
 
+  mutable StartGoalPlannerData goal_planner_data_;
+
   std::shared_ptr<GoalPlannerParameters> parameters_;
 
+  mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
+  mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
+  mutable std::shared_ptr<SafetyCheckParams> safety_check_params_;
+
   vehicle_info_util::VehicleInfo vehicle_info_;
 
   // planner
@@ -276,6 +298,13 @@ class GoalPlannerModule : public SceneModuleInterface
   // rtc
   std::pair<double, double> calcDistanceToPathChange() const;
 
+  // safety check
+  SafetyCheckParams createSafetyCheckParams() const;
+  void updateSafetyCheckTargetObjectsData(
+    const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
+    const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
+  bool isSafePath() const;
+
   // debug
   void setDebugData();
   void printParkingPositionError() const;

diff --git a/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
@@ -17,12 +17,15 @@
 
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp"
 #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp"
 #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp"
 #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
 #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp"
 #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp"
+#include "behavior_path_planner/utils/utils.hpp"
 
 #include <lane_departure_checker/lane_departure_checker.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -43,6 +46,11 @@
 
 namespace behavior_path_planner
 {
+using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
+using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams;
+using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
+using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams;
+using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
 using geometry_msgs::msg::PoseArray;
 using lane_departure_checker::LaneDepartureChecker;
 
@@ -90,6 +98,13 @@ class StartPlannerModule : public SceneModuleInterface
   void setParameters(const std::shared_ptr<StartPlannerParameters> & parameters)
   {
     parameters_ = parameters;
+    if (parameters->safety_check_params.enable_safety_check) {
+      ego_predicted_path_params_ =
+        std::make_shared<EgoPredictedPathParams>(parameters_->ego_predicted_path_params);
+      objects_filtering_params_ =
+        std::make_shared<ObjectsFilteringParams>(parameters_->objects_filtering_params);
+      safety_check_params_ = std::make_shared<SafetyCheckParams>(parameters_->safety_check_params);
+    }
   }
   void resetStatus();
 
@@ -110,10 +125,14 @@ class StartPlannerModule : public SceneModuleInterface
   bool canTransitIdleToRunningState() override { return false; }
 
   std::shared_ptr<StartPlannerParameters> parameters_;
+  mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
+  mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
+  mutable std::shared_ptr<SafetyCheckParams> safety_check_params_;
   vehicle_info_util::VehicleInfo vehicle_info_;
 
   std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
   PullOutStatus status_;
+  mutable StartGoalPlannerData start_planner_data_;
 
   std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_;
 
@@ -155,6 +174,10 @@ class StartPlannerModule : public SceneModuleInterface
   bool isStopped();
   bool isStuck();
   bool hasFinishedCurrentPath();
+  void updateSafetyCheckTargetObjectsData(
+    const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
+    const std::vector<PoseWithVelocityStamped> & ego_predicted_path) const;
+  bool isSafePath() const;
   void setDrivableAreaInfo(BehaviorModuleOutput & output) const;
 
   // check if the goal is located behind the ego in the same route segment.
@@ -163,6 +186,7 @@ class StartPlannerModule : public SceneModuleInterface
   // generate BehaviorPathOutput with stopping path and update status
   BehaviorModuleOutput generateStopOutput();
 
+  SafetyCheckParams createSafetyCheckParams() const;
   // freespace planner
   void onFreespacePlannerTimer();
   bool planFreespacePath();

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/...vior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp
@@ -63,23 +63,23 @@ void updateEgoPredictedPathParams(
 
 void updateEgoPredictedPathParams(
   std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
-  const std::shared_ptr<GoalPlannerParameters> & start_planner_params);
+  const std::shared_ptr<GoalPlannerParameters> & goal_planner_params);
 
 void updateSafetyCheckParams(
   std::shared_ptr<SafetyCheckParams> & safety_check_params,
   const std::shared_ptr<StartPlannerParameters> & start_planner_params);
 
 void updateSafetyCheckParams(
   std::shared_ptr<SafetyCheckParams> & safety_check_params,
-  const std::shared_ptr<GoalPlannerParameters> & start_planner_params);
+  const std::shared_ptr<GoalPlannerParameters> & goal_planner_params);
 
 void updateObjectsFilteringParams(
   std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
   const std::shared_ptr<StartPlannerParameters> & start_planner_params);
 
 void updateObjectsFilteringParams(
   std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
-  const std::shared_ptr<GoalPlannerParameters> & start_planner_params);
+  const std::shared_ptr<GoalPlannerParameters> & goal_planner_params);
 
 void updatePathProperty(
   std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp
@@ -16,6 +16,9 @@
 #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_
 
 #include "behavior_path_planner/data_manager.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp"
+#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp"
+#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp"
 
 #include <route_handler/route_handler.hpp>
 
@@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams;
 using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using route_handler::RouteHandler;