Feat/safety check for start goal planner

planning/behavior_path_planner/CMakeLists.txt

@@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED
   src/utils/path_utils.cpp
   src/utils/path_safety_checker/safety_check.cpp
   src/utils/path_safety_checker/objects_filtering.cpp
+  src/utils/start_goal_planner_common/utils.cpp
   src/utils/avoidance/utils.cpp
   src/utils/lane_change/utils.cpp
   src/utils/side_shift/util.cpp

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

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

planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp

@@ -131,6 +131,10 @@ MarkerArray createObjectsMarkerArray(
 MarkerArray createDrivableLanesMarkerArray(
   const std::vector<DrivableLanes> & drivable_lanes, std::string && ns);
 
+MarkerArray createPredictedPathMarkerArray(
+  const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info,
+  std::string && ns, const int32_t & id, const float & r, const float & g, const float & b);
+
 }  // namespace marker_utils
 
 #endif  // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_

planning/behavior_path_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,
@@ -65,7 +74,7 @@ enum class PathType {
   FREESPACE,
 };
 
-struct PUllOverStatus
+struct PullOverStatus
 {
   std::shared_ptr<PullOverPath> pull_over_path{};
   std::shared_ptr<PullOverPath> lane_parking_pull_over_path{};
@@ -76,7 +85,8 @@ struct PUllOverStatus
   lanelet::ConstLanelets pull_over_lanes{};
   std::vector<DrivableLanes> lanes{};  // current + pull_over
   bool has_decided_path{false};
-  bool is_safe{false};
+  bool is_safe_static_objects{false};   // current path is safe against static objects
+  bool is_safe_dynamic_objects{false};  // current path is safe against dynamic objects
   bool prev_is_safe{false};
   bool has_decided_velocity{false};
   bool has_requested_approval{false};
@@ -105,7 +115,14 @@ class GoalPlannerModule : public SceneModuleInterface
 
   void updateModuleParams(const std::shared_ptr<GoalPlannerParameters> & parameters)
   {
-    parameters_ = parameters;
+    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);
+    }
   }
 
   BehaviorModuleOutput run() override;
@@ -127,10 +144,24 @@ class GoalPlannerModule : public SceneModuleInterface
   CandidateOutput planCandidate() const override { return CandidateOutput{}; };
 
 private:
-  PUllOverStatus status_;
+  bool canTransitSuccessState() override { return false; }
+
+  bool canTransitFailureState() override { return false; }
+
+  bool canTransitIdleToRunningState() override { return false; }
+
+  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
   std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
   std::unique_ptr<PullOverPlannerBase> freespace_planner_;
@@ -198,7 +229,6 @@ class GoalPlannerModule : public SceneModuleInterface
     const Pose & search_start_offset_pose, PathWithLaneId & path) const;
   PathWithLaneId generateStopPath();
   PathWithLaneId generateFeasibleStopPath();
-  boost::optional<double> calcFeasibleDecelDistance(const double target_velocity) const;
   void keepStoppedWithCurrentPath(PathWithLaneId & path);
   double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;
 
@@ -267,6 +297,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;

planning/behavior_path_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;
 
@@ -53,7 +61,8 @@ struct PullOutStatus
   PlannerType planner_type{PlannerType::NONE};
   PathWithLaneId backward_path{};
   lanelet::ConstLanelets pull_out_lanes{};
-  bool is_safe{false};
+  bool is_safe_static_objects{false};   // current path is safe against static objects
+  bool is_safe_dynamic_objects{false};  // current path is safe against dynamic objects
   bool back_finished{false};
   Pose pull_out_start_pose{};
 
@@ -87,6 +96,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();
 
@@ -101,10 +117,14 @@ class StartPlannerModule : public SceneModuleInterface
 
 private:
   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_;
 
@@ -146,6 +166,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.
@@ -154,6 +178,7 @@ class StartPlannerModule : public SceneModuleInterface
   // generate BehaviorPathOutput with stopping path and update status
   BehaviorModuleOutput generateStopOutput();
 
+  SafetyCheckParams createSafetyCheckParams() const;
   // freespace planner
   void onFreespacePlannerTimer();
   bool planFreespacePath();

planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp

@@ -94,6 +94,10 @@ class GeometricParallelParking
 
   std::vector<PathWithLaneId> getArcPaths() const { return arc_paths_; }
   std::vector<PathWithLaneId> getPaths() const { return paths_; }
+  std::vector<std::pair<double, double>> getPairsTerminalVelocityAndAccel() const
+  {
+    return pairs_terminal_velocity_and_accel_;
+  }
   PathWithLaneId getPathByIdx(size_t const idx) const;
   PathWithLaneId getCurrentPath() const;
   PathWithLaneId getFullPath() const;
@@ -112,6 +116,7 @@ class GeometricParallelParking
 
   std::vector<PathWithLaneId> arc_paths_;
   std::vector<PathWithLaneId> paths_;
+  std::vector<std::pair<double, double>> pairs_terminal_velocity_and_accel_;
   size_t current_path_idx_ = 0;
 
   void clearPaths();

planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp

@@ -17,6 +17,7 @@
 #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_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 <freespace_planning_algorithms/abstract_algorithm.hpp>
 #include <freespace_planning_algorithms/astar_search.hpp>
@@ -99,6 +100,18 @@ struct GoalPlannerParameters
   AstarParam astar_parameters;
   RRTStarParam rrt_star_parameters;
 
+  // stop condition
+  double maximum_deceleration_for_stop;
+  double maximum_jerk_for_stop;
+
+  // hysteresis parameter
+  double hysteresis_factor_expand_rate;
+
+  // path safety checker
+  utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params;
+  utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params;
+  utils::path_safety_checker::SafetyCheckParams safety_check_params;
+
   // debug
   bool print_debug_info;
 };