feat(avoidance): add safety check for avoidance path

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -10,6 +10,7 @@
       enable_avoidance_over_same_direction: true
       enable_avoidance_over_opposite_direction: true
       enable_update_path_when_object_is_gone: false
+      enable_safety_check: false
 
       # For target object filtering
       threshold_speed_object_is_stopped: 1.0      # [m/s]
@@ -23,11 +24,20 @@
       object_check_shiftable_ratio: 0.6           # [-]
       object_check_min_road_shoulder_width: 0.5   # [m]
 
+      # For safety check
+      safety_check_backward_distance: 50.0        # [m]
+      safety_check_time_horizon: 10.0             # [s]
+      safety_check_idling_time: 1.5               # [s]
+      safety_check_accel_for_rss: 2.5             # [m/ss]
+
       # For lateral margin
       object_envelope_buffer: 0.3                 # [m]
       lateral_collision_margin: 1.0               # [m]
       lateral_collision_safety_buffer: 0.7        # [m]
 
+      # For longitudinal margin
+      longitudinal_collision_margin_buffer: 0.0   # [m]
+
       prepare_time: 2.0                           # [s]
       min_prepare_distance: 1.0                   # [m]
       min_avoidance_distance: 10.0                # [m]
@@ -74,3 +84,9 @@
 
       # ---------- advanced parameters ----------
       avoidance_execution_lateral_threshold: 0.499
+
+      target_velocity_matrix:
+        col_size: 4
+        matrix:
+          [2.78, 5.56, 11.1, 13.9,   # velocity [m/s]
+           0.20, 0.90, 1.10, 1.20]   # margin [m]

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -238,7 +238,9 @@ class AvoidanceModule : public SceneModuleInterface
   // shift point generation: generator
   double getShiftLength(
     const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const;
-  AvoidLineArray calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const;
+
+  AvoidLineArray calcRawShiftLinesFromObjects(
+    const AvoidancePlanningData & data, DebugData & debug) const;
 
   // shift point generation: combiner
   AvoidLineArray combineRawShiftLinesWithUniqueCheck(
@@ -307,6 +309,30 @@ class AvoidanceModule : public SceneModuleInterface
   mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
   mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
 
+  double getLateralMarginFromVelocity(const double velocity) const;
+
+  double getRSSLongitudinalDistance(
+    const double v_ego, const double v_obj, const bool is_front_object) const;
+
+  ObjectDataArray getAdjacentLaneObjects(const lanelet::ConstLanelets & adjacent_lanes) const;
+
+  // ========= safety check ==============
+
+  lanelet::ConstLanelets getAdjacentLane(
+    const PathShifter & path_shifter, const double forward_distance,
+    const double backward_distance) const;
+
+  bool isSafePath(
+    const PathShifter & path_shifter, ShiftedPath & shifted_path, DebugData & debug) const;
+
+  bool isSafePath(
+    const PathWithLaneId & path, const lanelet::ConstLanelets & check_lanes,
+    DebugData & debug) const;
+
+  bool isEnoughMargin(
+    const PathPointWithLaneId & p_ego, const double t, const ObjectData & object,
+    MarginData & margin_data) const;
+
   // ========= helper functions ==========
 
   double getEgoSpeed() const

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -26,13 +26,15 @@
 
 #include <lanelet2_core/geometry/Lanelet.h>
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
 namespace behavior_path_planner
 {
 using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
 using tier4_autoware_utils::Point2d;
@@ -68,6 +70,9 @@ struct AvoidanceParameters
   // enable update path when if detected objects on planner data is gone.
   bool enable_update_path_when_object_is_gone{false};
 
+  // enable safety check. if avoidance path is NOT safe, the ego will execute yield maneuver
+  bool enable_safety_check{false};
+
   // Vehicles whose distance to the center of the path is
   // less than this will not be considered for avoidance.
   double threshold_distance_object_is_on_center;
@@ -99,6 +104,9 @@ struct AvoidanceParameters
   // don't ever set this value to 0
   double lateral_collision_safety_buffer{0.5};
 
+  // margin between object back and end point of avoid shift point
+  double longitudinal_collision_safety_buffer{0.0};
+
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction
   double longitudinal_collision_margin_min_distance;
@@ -107,6 +115,21 @@ struct AvoidanceParameters
   // for longitudinal direction
   double longitudinal_collision_margin_time;
 
+  // find adjacent lane vehicles
+  double safety_check_backward_distance;
+
+  // minimum longitudinal margin for vehicles in adjacent lane
+  double safety_check_min_longitudinal_margin;
+
+  // safety check time horizon
+  double safety_check_time_horizon;
+
+  // use in RSS calculation
+  double safety_check_idling_time;
+
+  // use in RSS calculation
+  double safety_check_accel_for_rss;
+
   // start avoidance after this time to avoid sudden path change
   double prepare_time;
 
@@ -163,6 +186,12 @@ struct AvoidanceParameters
   // avoidance path will be generated unless their lateral margin difference exceeds this value.
   double avoidance_execution_lateral_threshold;
 
+  // target velocity matrix
+  std::vector<double> target_velocity_matrix;
+
+  // matrix col size
+  size_t col_size;
+
   // true by default
   bool avoid_car{true};      // avoidance is performed for type object car
   bool avoid_truck{true};    // avoidance is performed for type object truck
@@ -234,6 +263,9 @@ struct ObjectData  // avoidance target
 
   // lateral distance from overhang to the road shoulder
   double to_road_shoulder_distance{0.0};
+
+  // unavoidable reason
+  std::string reason{""};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 
@@ -301,6 +333,8 @@ struct AvoidancePlanningData
   // new shift point
   AvoidLineArray unapproved_new_sl{};
 
+  bool safe{false};
+
   bool avoiding_now{false};
 };
 
@@ -328,6 +362,27 @@ struct ShiftLineData
   std::vector<std::vector<double>> shift_line_history;
 };
 
+/*
+ * Data struct for longitudinal margin
+ */
+struct MarginData
+{
+  Pose pose{};
+
+  bool enough_lateral_margin{true};
+
+  double longitudinal_distance{std::numeric_limits<double>::max()};
+
+  double longitudinal_margin{std::numeric_limits<double>::lowest()};
+
+  double vehicle_width;
+
+  double base_link2front;
+
+  double base_link2rear;
+};
+using MarginDataArray = std::vector<MarginData>;
+
 /*
  * Debug information for marker array
  */
@@ -356,6 +411,17 @@ struct DebugData
   std::vector<double> total_shift;
   std::vector<double> output_shift;
 
+  bool exist_adjacent_objects{false};
+
+  // margin
+  MarginDataArray margin_data_array;
+
+  // avoidance require objects
+  ObjectDataArray unavoidable_objects;
+
+  // avoidance unsafe objects
+  ObjectDataArray unsafe_objects;
+
   // tmp for plot
   PathWithLaneId center_line;
   AvoidanceDebugMsgArray avoidance_debug_msg_array;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp

@@ -36,6 +36,7 @@ namespace marker_utils::avoidance_marker
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using behavior_path_planner::AvoidancePlanningData;
 using behavior_path_planner::AvoidLineArray;
 using behavior_path_planner::ObjectDataArray;
 using behavior_path_planner::ShiftLineArray;
@@ -44,15 +45,20 @@ using geometry_msgs::msg::Polygon;
 using geometry_msgs::msg::Pose;
 using visualization_msgs::msg::MarkerArray;
 
+MarkerArray createEgoStatusMarkerArray(
+  const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns);
+
 MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g,
   const float & b, const double & w);
 
-MarkerArray createTargetObjectsMarkerArray(
-  const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
+MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
 
-MarkerArray createOtherObjectsMarkerArray(
-  const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
+MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
+
+MarkerArray createUnsafeObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
+
+MarkerArray createUnavoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns);
 
 MarkerArray makeOverhangToRoadShoulderMarkerArray(
   const behavior_path_planner::ObjectDataArray & objects, std::string && ns);

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -295,6 +295,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true);
   p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true);
   p.enable_update_path_when_object_is_gone = dp("enable_update_path_when_object_is_gone", false);
+  p.enable_safety_check = dp("enable_safety_check", false);
 
   p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0);
   p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0);
@@ -306,6 +307,13 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.object_envelope_buffer = dp("object_envelope_buffer", 0.1);
   p.lateral_collision_margin = dp("lateral_collision_margin", 2.0);
   p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5);
+  p.longitudinal_collision_safety_buffer = dp("longitudinal_collision_safety_buffer", 0.0);
+
+  p.safety_check_min_longitudinal_margin = dp("safety_check_min_longitudinal_margin", 0.0);
+  p.safety_check_backward_distance = dp("safety_check_backward_distance", 0.0);
+  p.safety_check_time_horizon = dp("safety_check_time_horizon", 10.0);
+  p.safety_check_idling_time = dp("safety_check_idling_time", 1.5);
+  p.safety_check_accel_for_rss = dp("safety_check_accel_for_rss", 2.5);
 
   p.prepare_time = dp("prepare_time", 3.0);
   p.min_prepare_distance = dp("min_prepare_distance", 10.0);
@@ -334,6 +342,13 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.publish_debug_marker = dp("publish_debug_marker", false);
   p.print_debug_info = dp("print_debug_info", false);
 
+  // velocity matrix
+  {
+    std::string ns = "avoidance.target_velocity_matrix.";
+    p.col_size = declare_parameter<int>(ns + "col_size");
+    p.target_velocity_matrix = declare_parameter<std::vector<double>>(ns + "matrix");
+  }
+
   p.avoid_car = dp("target_object.car", true);
   p.avoid_truck = dp("target_object.truck", true);
   p.avoid_bus = dp("target_object.bus", true);