feat(goal_planner): safer safety check (#5677)

typo

Signed-off-by: kosuke55 <[email protected]>

planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml

@@ -160,6 +160,7 @@
         safety_check_params:
           # safety check configuration
           enable_safety_check: true
+          method: "integral_predicted_polygon"
           keep_unsafe_time: 3.0
           # collision check parameters
           check_all_predicted_path: true
@@ -170,10 +171,15 @@
             lateral_distance_max_threshold: 2.0
             longitudinal_distance_min_threshold: 3.0
             longitudinal_velocity_delta_time: 0.8
+          integral_predicted_polygon_params:
+            forward_margin: 1.0
+            backward_margin: 1.0
+            lat_margin: 1.0
+            time_horizon: 10.0
           # hysteresis factor to expand/shrink polygon with the value
           hysteresis_factor_expand_rate: 1.0
           # temporary
-          backward_path_length: 30.0
+          backward_path_length: 100.0
           forward_path_length: 100.0
 
       # debug

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -244,15 +244,13 @@ struct PreviousPullOverData
     stop_path = nullptr;
     stop_path_after_approval = nullptr;
     found_path = false;
-    is_safe = false;
     safety_status = SafetyStatus{};
     has_decided_path = false;
   }
 
   std::shared_ptr<PathWithLaneId> stop_path{nullptr};
   std::shared_ptr<PathWithLaneId> stop_path_after_approval{nullptr};
   bool found_path{false};
-  bool is_safe{false};
   SafetyStatus safety_status{};
   bool has_decided_path{false};
 };

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp

@@ -134,6 +134,14 @@ struct RSSparams
   double rear_vehicle_deceleration{0.0};         ///< brake parameter
 };
 
+struct IntegralPredictedPolygonParams
+{
+  double forward_margin{0.0};   ///< Forward margin for extended ego polygon for collision check.
+  double backward_margin{0.0};  ///< Backward margin for extended ego polygon for collision check.
+  double lat_margin{0.0};       ///< Lateral margin for extended ego polygon for collision check.
+  double time_horizon{0.0};     ///< Time horizon for object's prediction.
+};
+
 /**
  * @brief Parameters for generating the ego vehicle's predicted path.
  */
@@ -173,12 +181,15 @@ struct ObjectsFilteringParams
 struct SafetyCheckParams
 {
   bool enable_safety_check{false};  ///< Enable safety checks.
-  double keep_unsafe_time{0.0};     ///< Time to keep unsafe before changing to safe.
+  std::string method{"RSS"};        /// Method to use for safety checks.
+  /// possible values: ["RSS", "integral_predicted_polygon"]
+  double keep_unsafe_time{0.0};  ///< Time to keep unsafe before changing to safe.
   double hysteresis_factor_expand_rate{
     0.0};                            ///< Hysteresis factor to expand/shrink polygon with the value.
   double backward_path_length{0.0};  ///< Length of the backward lane for path generation.
   double forward_path_length{0.0};   ///< Length of the forward path lane for path generation.
   RSSparams rss_params{};            ///< Parameters related to the RSS model.
+  IntegralPredictedPolygonParams integral_predicted_polygon_params{};  ///< Parameters for polygon.
   bool publish_debug_marker{false};  ///< Option to publish debug markers.
 };
 

planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp

@@ -85,6 +85,20 @@ boost::optional<PoseWithVelocityStamped> calcInterpolatedPoseWithVelocity(
 boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
   const std::vector<PoseWithVelocityStamped> & pred_path, const double current_time,
   const VehicleInfo & ego_info);
+boost::optional<PoseWithVelocityAndPolygonStamped> getInterpolatedPoseWithVelocityAndPolygonStamped(
+  const std::vector<PoseWithVelocityStamped> & pred_path, const double current_time,
+  const Shape & shape);
+template <typename T, typename F>
+std::vector<T> filterPredictedPathByTimeHorizon(
+  const std::vector<T> & path, const double time_horizon, const F & interpolateFunc);
+std::vector<PoseWithVelocityStamped> filterPredictedPathByTimeHorizon(
+  const std::vector<PoseWithVelocityStamped> & path, const double time_horizon);
+ExtendedPredictedObject filterObjectPredictedPathByTimeHorizon(
+  const ExtendedPredictedObject & object, const double time_horizon,
+  const bool check_all_predicted_path);
+ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon(
+  const ExtendedPredictedObjects & objects, const double time_horizon,
+  const bool check_all_predicted_path);
 /**
  * @brief Iterate the points in the ego and target's predicted path and
  *        perform safety check for each of the iterated points.
@@ -131,12 +145,15 @@ std::vector<Polygon2d> getCollidedPolygons(
 
 bool checkPolygonsIntersects(
   const std::vector<Polygon2d> & polys_1, const std::vector<Polygon2d> & polys_2);
+bool checkSafetyWithIntegralPredictedPolygon(
+  const std::vector<PoseWithVelocityStamped> & ego_predicted_path, const VehicleInfo & vehicle_info,
+  const ExtendedPredictedObjects & objects, const bool check_all_predicted_path,
+  const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map);
 
 // debug
 CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj);
 void updateCollisionCheckDebugMap(
   CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe);
-
 }  // namespace behavior_path_planner::utils::path_safety_checker
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_

planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp

@@ -769,7 +769,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const
 void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const
 {
   // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path
-  if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) {
+  if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) {
     auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath();
     const auto stop_path =
       behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath(
@@ -1817,8 +1817,23 @@ std::pair<bool, bool> GoalPlannerModule::isSafePath() const
   const double hysteresis_factor =
     prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate;
 
-  const bool current_is_safe = checkSafetyWithRSS(
-    pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, hysteresis_factor);
+  const bool current_is_safe = std::invoke([&]() {
+    if (parameters_->safety_check_params.method == "RSS") {
+      return checkSafetyWithRSS(
+        pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
+        hysteresis_factor);
+    } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") {
+      return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon(
+        ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane,
+        objects_filtering_params_->check_all_predicted_path,
+        parameters_->safety_check_params.integral_predicted_polygon_params,
+        goal_planner_data_.collision_check);
+    }
+    RCLCPP_ERROR(
+      getLogger(), " [pull_over] invalid safety check method: %s",
+      parameters_->safety_check_params.method.c_str());
+    throw std::domain_error("[pull_over] invalid safety check method");
+  });
 
   /*　　
    *　　                    ==== is_safe
@@ -1940,11 +1955,34 @@ void GoalPlannerModule::setDebugData()
         goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9));
     }
 
-    add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info"));
+    if (parameters_->safety_check_params.method == "RSS") {
+      add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info"));
+    }
     add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path"));
     add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation"));
     utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
       goal_planner_data_.collision_check);
+
+    // visualize safety status maker
+    {
+      visualization_msgs::msg::MarkerArray marker_array{};
+      const auto color = prev_data_.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99)
+                                                          : createMarkerColor(1.0, 0.0, 0.0, 0.99);
+      auto marker = createDefaultMarker(
+        header.frame_id, header.stamp, "safety_status", 0,
+        visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color);
+
+      marker.pose = planner_data_->self_odometry->pose.pose;
+      marker.text += "is_safe: " + std::to_string(prev_data_.safety_status.is_safe) + "\n";
+      if (prev_data_.safety_status.safe_start_time) {
+        const double elapsed_time_from_safe_start =
+          (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds();
+        marker.text +=
+          "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n";
+      }
+      marker_array.markers.push_back(marker);
+      add(marker_array);
+    }
   }
 
   // Visualize planner type text

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

@@ -336,6 +336,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
       node->declare_parameter<bool>(safety_check_ns + "enable_safety_check");
     p.safety_check_params.keep_unsafe_time =
       node->declare_parameter<double>(safety_check_ns + "keep_unsafe_time");
+    p.safety_check_params.method = node->declare_parameter<std::string>(safety_check_ns + "method");
     p.safety_check_params.hysteresis_factor_expand_rate =
       node->declare_parameter<double>(safety_check_ns + "hysteresis_factor_expand_rate");
     p.safety_check_params.backward_path_length =
@@ -361,6 +362,19 @@ GoalPlannerModuleManager::GoalPlannerModuleManager(
       node->declare_parameter<double>(rss_ns + "longitudinal_velocity_delta_time");
   }
 
+  // IntegralPredictedPolygonParams
+  std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params.";
+  {
+    p.safety_check_params.integral_predicted_polygon_params.forward_margin =
+      node->declare_parameter<double>(integral_ns + "forward_margin");
+    p.safety_check_params.integral_predicted_polygon_params.backward_margin =
+      node->declare_parameter<double>(integral_ns + "backward_margin");
+    p.safety_check_params.integral_predicted_polygon_params.lat_margin =
+      node->declare_parameter<double>(integral_ns + "lat_margin");
+    p.safety_check_params.integral_predicted_polygon_params.time_horizon =
+      node->declare_parameter<double>(integral_ns + "time_horizon");
+  }
+
   // debug
   {
     const std::string ns = base_ns + "debug.";