Merge pull request #819 from satoshi-ota/hotfix/avoidance-safety-check

hotfix(avoidance): safety check improvement

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

@@ -142,10 +142,20 @@
         # collision check parameters
         check_all_predicted_path: false                  # [-]
         time_resolution: 0.5                             # [s]
-        time_horizon: 10.0                               # [s]
+        time_horizon_for_front_object: 10.0              # [s]
+        time_horizon_for_rear_object: 10.0               # [s]
         safety_check_backward_distance: 100.0            # [m]
-        safety_check_hysteresis_factor: 2.0              # [-]
         safety_check_ego_offset: 1.0                     # [m]
+        hysteresis_factor_expand_rate: 2.0               # [-]
+        hysteresis_factor_safe_count: 10                 # [-]
+        # rss parameters
+        expected_front_deceleration: -1.0                # [m/ss]
+        expected_rear_deceleration: -1.0                 # [m/ss]
+        rear_vehicle_reaction_time: 2.0                  # [s]
+        rear_vehicle_safety_time_margin: 1.0             # [s]
+        lateral_distance_max_threshold: 2.0              # [m]
+        longitudinal_distance_min_threshold: 3.0         # [m]
+        longitudinal_velocity_delta_time: 0.8            # [s]
 
       # For avoidance maneuver
       avoidance:

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -25,19 +25,6 @@
 
     visualize_maximum_drivable_area: true
 
-    lateral_distance_max_threshold: 2.0
-    longitudinal_distance_min_threshold: 3.0
-    longitudinal_velocity_delta_time: 0.8 # [s]
-
-    expected_front_deceleration: -1.0
-    expected_rear_deceleration: -1.0
-
-    expected_front_deceleration_for_abort: -1.0
-    expected_rear_deceleration_for_abort: -2.0
-
-    rear_vehicle_reaction_time: 2.0
-    rear_vehicle_safety_time_margin: 1.0
-
     lane_following:
       drivable_area_right_bound_offset: 0.0
       drivable_area_left_bound_offset: 0.0

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -27,6 +27,18 @@
       min_longitudinal_acc: -1.0
       max_longitudinal_acc: 1.0
 
+      # safety check
+      safety_check:
+        expected_front_deceleration: -1.0
+        expected_rear_deceleration: -1.0
+        expected_front_deceleration_for_abort: -1.0
+        expected_rear_deceleration_for_abort: -2.0
+        rear_vehicle_reaction_time: 2.0
+        rear_vehicle_safety_time_margin: 1.0
+        lateral_distance_max_threshold: 2.0
+        longitudinal_distance_min_threshold: 3.0
+        longitudinal_velocity_delta_time: 0.8
+
       # lateral acceleration map
       lateral_acceleration:
         velocity: [0.0, 4.0, 10.0]

planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp

@@ -142,20 +142,6 @@ struct BehaviorPathPlannerParameters
 
   // maximum drivable area visualization
   bool visualize_maximum_drivable_area;
-
-  // collision check
-  double lateral_distance_max_threshold;
-  double longitudinal_distance_min_threshold;
-  double longitudinal_velocity_delta_time;
-
-  double expected_front_deceleration;  // brake parameter under normal lane change
-  double expected_rear_deceleration;   // brake parameter under normal lane change
-
-  double expected_front_deceleration_for_abort;  // hard brake parameter for abort
-  double expected_rear_deceleration_for_abort;   // hard brake parameter for abort
-
-  double rear_vehicle_reaction_time;
-  double rear_vehicle_safety_time_margin;
 };
 
 #endif  // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_

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

@@ -555,6 +555,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   bool arrived_path_end_{false};
 
+  bool safe_{true};
+
   std::shared_ptr<AvoidanceParameters> parameters_;
 
   helper::avoidance::AvoidanceHelper helper_;
@@ -575,6 +577,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   ObjectDataArray registered_objects_;
 
+  mutable size_t safe_count_{0};
+
   mutable ObjectDataArray ego_stopped_objects_;
 
   mutable ObjectDataArray stopped_objects_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp

@@ -140,7 +140,7 @@ class NormalLaneChange : public LaneChangeBase
 
   PathSafetyStatus isLaneChangePathSafe(
     const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects,
-    const double front_decel, const double rear_decel,
+    const utils::path_safety_checker::RSSparams & rss_params,
     std::unordered_map<std::string, CollisionCheckDebug> & debug_data) const;
 
   rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());

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

@@ -16,6 +16,7 @@
 #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_
 
 #include "behavior_path_planner/marker_utils/utils.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 <rclcpp/rclcpp.hpp>
@@ -192,14 +193,16 @@ struct AvoidanceParameters
 
   // parameters for collision check.
   bool check_all_predicted_path{false};
-  double safety_check_time_horizon{0.0};
+  double time_horizon_for_front_object{0.0};
+  double time_horizon_for_rear_object{0.0};
   double safety_check_time_resolution{0.0};
 
   // find adjacent lane vehicles
   double safety_check_backward_distance;
 
   // transit hysteresis (unsafe to safe)
-  double safety_check_hysteresis_factor;
+  size_t hysteresis_factor_safe_count;
+  double hysteresis_factor_expand_rate;
 
   // don't output new candidate path if the offset between ego and path is larger than this.
   double safety_check_ego_offset;
@@ -302,6 +305,9 @@ struct AvoidanceParameters
   // parameters depend on object class
   std::unordered_map<uint8_t, ObjectParameter> object_parameters;
 
+  // rss parameters
+  utils::path_safety_checker::RSSparams rss_params;
+
   // clip left and right bounds for objects
   bool enable_bound_clipping{false};
 

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp

@@ -88,7 +88,7 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
 
 std::vector<PoseWithVelocityStamped> convertToPredictedPath(
   const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
+  const bool is_object_front, const std::shared_ptr<AvoidanceParameters> & parameters);
 
 double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v);
 

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp

@@ -76,6 +76,10 @@ struct LaneChangeParameters
   bool check_motorcycle{true};  // check object motorbike
   bool check_pedestrian{true};  // check object pedestrian
 
+  // safety check
+  utils::path_safety_checker::RSSparams rss_params;
+  utils::path_safety_checker::RSSparams rss_params_for_abort;
+
   // abort
   LaneChangeCancelParameters cancel;
 

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

@@ -123,6 +123,8 @@ struct RSSparams
   double longitudinal_distance_min_threshold{
     0.0};                                        ///< Minimum threshold for longitudinal distance.
   double longitudinal_velocity_delta_time{0.0};  ///< Delta time for longitudinal velocity.
+  double front_vehicle_deceleration;             ///< brake parameter
+  double rear_vehicle_deceleration;              ///< brake parameter
 };
 
 /**

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

@@ -52,6 +52,9 @@ using vehicle_info_util::VehicleInfo;
 
 namespace bg = boost::geometry;
 
+bool isTargetObjectFront(
+  const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon,
+  const vehicle_info_util::VehicleInfo & vehicle_info);
 bool isTargetObjectFront(
   const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose,
   const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon);
@@ -65,8 +68,7 @@ Polygon2d createExtendedPolygon(
 
 double calcRssDistance(
   const double front_object_velocity, const double rear_object_velocity,
-  const double front_object_deceleration, const double rear_object_deceleration,
-  const BehaviorPathPlannerParameters & params);
+  const RSSparams & rss_params);
 
 double calcMinimumLongitudinalLength(
   const double front_object_velocity, const double rear_object_velocity,
@@ -98,8 +100,8 @@ bool checkCollision(
   const std::vector<PoseWithVelocityStamped> & predicted_ego_path,
   const ExtendedPredictedObject & target_object,
   const PredictedPathWithPolygon & target_object_path,
-  const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration,
-  const double rear_object_deceleration, CollisionCheckDebug & debug);
+  const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters,
+  const double hysteresis_factor, CollisionCheckDebug & debug);
 
 /**
  * @brief Check collision between ego path footprints with extra longitudinal stopping margin and

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -391,23 +391,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.ego_nearest_dist_threshold = declare_parameter<double>("ego_nearest_dist_threshold");
   p.ego_nearest_yaw_threshold = declare_parameter<double>("ego_nearest_yaw_threshold");
 
-  p.lateral_distance_max_threshold = declare_parameter<double>("lateral_distance_max_threshold");
-  p.longitudinal_distance_min_threshold =
-    declare_parameter<double>("longitudinal_distance_min_threshold");
-  p.longitudinal_velocity_delta_time =
-    declare_parameter<double>("longitudinal_velocity_delta_time");
-
-  p.expected_front_deceleration = declare_parameter<double>("expected_front_deceleration");
-  p.expected_rear_deceleration = declare_parameter<double>("expected_rear_deceleration");
-
-  p.expected_front_deceleration_for_abort =
-    declare_parameter<double>("expected_front_deceleration_for_abort");
-  p.expected_rear_deceleration_for_abort =
-    declare_parameter<double>("expected_rear_deceleration_for_abort");
-
-  p.rear_vehicle_reaction_time = declare_parameter<double>("rear_vehicle_reaction_time");
-  p.rear_vehicle_safety_time_margin = declare_parameter<double>("rear_vehicle_safety_time_margin");
-
   if (p.backward_length_buffer_for_end_of_lane < 1.0) {
     RCLCPP_WARN_STREAM(
       get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer.");

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -1891,8 +1891,10 @@ bool AvoidanceModule::isSafePath(
     return true;  // if safety check is disabled, it always return safe.
   }
 
-  const auto ego_predicted_path =
-    utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_);
+  const auto ego_predicted_path_for_front_object =
+    utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_);
+  const auto ego_predicted_path_for_rear_object =
+    utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_);
 
   const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points);
   const auto is_right_shift = [&]() -> std::optional<bool> {
@@ -1915,23 +1917,42 @@ bool AvoidanceModule::isSafePath(
     return true;
   }
 
+  const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate;
+
   const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects(
     avoidance_data_, planner_data_, parameters_, is_right_shift.value());
 
   for (const auto & object : safety_check_target_objects) {
+    const auto obj_polygon =
+      tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape);
+
+    const auto is_object_front =
+      utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info);
+
+    const auto is_object_incoming =
+      std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2;
+
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, parameters_->check_all_predicted_path);
+
+    const auto & ego_predicted_path = is_object_front && !is_object_incoming
+                                        ? ego_predicted_path_for_front_object
+                                        : ego_predicted_path_for_rear_object;
+
     for (const auto & obj_path : obj_predicted_paths) {
       CollisionCheckDebug collision{};
       if (!utils::path_safety_checker::checkCollision(
-            shifted_path.path, ego_predicted_path, object, obj_path, p,
-            p.expected_front_deceleration, p.expected_rear_deceleration, collision)) {
+            shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params,
+            hysteresis_factor, collision)) {
+        safe_count_ = 0;
         return false;
       }
     }
   }
 
-  return true;
+  safe_count_++;
+
+  return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count;
 }
 
 void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const
@@ -2577,6 +2598,8 @@ void AvoidanceModule::updateData()
   fillShiftLine(avoidance_data_, debug_data_);
   fillEgoStatus(avoidance_data_, debug_data_);
   fillDebugData(avoidance_data_, debug_data_);
+
+  safe_ = avoidance_data_.safe;
 }
 
 void AvoidanceModule::processOnEntry()

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

@@ -136,7 +136,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.object_last_seen_threshold = get_parameter<double>(node, ns + "object_last_seen_threshold");
   }
 
-  // safety check
+  // safety check general params
   {
     std::string ns = "avoidance.safety_check.";
     p.enable_safety_check = get_parameter<bool>(node, ns + "enable");
@@ -146,13 +146,36 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.check_unavoidable_object = get_parameter<bool>(node, ns + "check_unavoidable_object");
     p.check_other_object = get_parameter<bool>(node, ns + "check_other_object");
     p.check_all_predicted_path = get_parameter<bool>(node, ns + "check_all_predicted_path");
-    p.safety_check_time_horizon = get_parameter<double>(node, ns + "time_horizon");
+    p.time_horizon_for_front_object =
+      get_parameter<double>(node, ns + "time_horizon_for_front_object");
+    p.time_horizon_for_rear_object =
+      get_parameter<double>(node, ns + "time_horizon_for_rear_object");
     p.safety_check_time_resolution = get_parameter<double>(node, ns + "time_resolution");
     p.safety_check_backward_distance =
       get_parameter<double>(node, ns + "safety_check_backward_distance");
-    p.safety_check_hysteresis_factor =
-      get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
     p.safety_check_ego_offset = get_parameter<double>(node, ns + "safety_check_ego_offset");
+    p.hysteresis_factor_expand_rate =
+      get_parameter<double>(node, ns + "hysteresis_factor_expand_rate");
+    p.hysteresis_factor_safe_count = get_parameter<int>(node, ns + "hysteresis_factor_safe_count");
+  }
+
+  // safety check rss params
+  {
+    std::string ns = "avoidance.safety_check.";
+    p.rss_params.longitudinal_distance_min_threshold =
+      get_parameter<double>(node, ns + "longitudinal_distance_min_threshold");
+    p.rss_params.longitudinal_velocity_delta_time =
+      get_parameter<double>(node, ns + "longitudinal_velocity_delta_time");
+    p.rss_params.front_vehicle_deceleration =
+      get_parameter<double>(node, ns + "expected_front_deceleration");
+    p.rss_params.rear_vehicle_deceleration =
+      get_parameter<double>(node, ns + "expected_rear_deceleration");
+    p.rss_params.rear_vehicle_reaction_time =
+      get_parameter<double>(node, ns + "rear_vehicle_reaction_time");
+    p.rss_params.rear_vehicle_safety_time_margin =
+      get_parameter<double>(node, ns + "rear_vehicle_safety_time_margin");
+    p.rss_params.lateral_distance_max_threshold =
+      get_parameter<double>(node, ns + "lateral_distance_max_threshold");
   }
 
   // avoidance maneuver (lateral)

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

@@ -85,6 +85,36 @@ LaneChangeModuleManager::LaneChangeModuleManager(
     get_parameter<bool>(node, parameter("check_objects_on_other_lanes"));
   p.use_all_predicted_path = get_parameter<bool>(node, parameter("use_all_predicted_path"));
 
+  p.rss_params.longitudinal_distance_min_threshold =
+    get_parameter<double>(node, parameter("safety_check.longitudinal_distance_min_threshold"));
+  p.rss_params.longitudinal_velocity_delta_time =
+    get_parameter<double>(node, parameter("safety_check.longitudinal_velocity_delta_time"));
+  p.rss_params.front_vehicle_deceleration =
+    get_parameter<double>(node, parameter("safety_check.expected_front_deceleration"));
+  p.rss_params.rear_vehicle_deceleration =
+    get_parameter<double>(node, parameter("safety_check.expected_rear_deceleration"));
+  p.rss_params.rear_vehicle_reaction_time =
+    get_parameter<double>(node, parameter("safety_check.rear_vehicle_reaction_time"));
+  p.rss_params.rear_vehicle_safety_time_margin =
+    get_parameter<double>(node, parameter("safety_check.rear_vehicle_safety_time_margin"));
+  p.rss_params.lateral_distance_max_threshold =
+    get_parameter<double>(node, parameter("safety_check.lateral_distance_max_threshold"));
+
+  p.rss_params_for_abort.longitudinal_distance_min_threshold =
+    get_parameter<double>(node, parameter("safety_check.longitudinal_distance_min_threshold"));
+  p.rss_params_for_abort.longitudinal_velocity_delta_time =
+    get_parameter<double>(node, parameter("safety_check.longitudinal_velocity_delta_time"));
+  p.rss_params_for_abort.front_vehicle_deceleration =
+    get_parameter<double>(node, parameter("safety_check.expected_front_deceleration_for_abort"));
+  p.rss_params_for_abort.rear_vehicle_deceleration =
+    get_parameter<double>(node, parameter("safety_check.expected_rear_deceleration_for_abort"));
+  p.rss_params_for_abort.rear_vehicle_reaction_time =
+    get_parameter<double>(node, parameter("safety_check.rear_vehicle_reaction_time"));
+  p.rss_params_for_abort.rear_vehicle_safety_time_margin =
+    get_parameter<double>(node, parameter("safety_check.rear_vehicle_safety_time_margin"));
+  p.rss_params_for_abort.lateral_distance_max_threshold =
+    get_parameter<double>(node, parameter("safety_check.lateral_distance_max_threshold"));
+
   // target object
   {
     std::string ns = "lane_change.target_object.";
@@ -264,8 +294,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
   // safety check
   {
     std::string ns = "avoidance.safety_check.";
-    p.safety_check_hysteresis_factor =
-      get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
+    p.hysteresis_factor_expand_rate =
+      get_parameter<double>(node, ns + "hysteresis_factor_expand_rate");
   }
 
   avoidance_parameters_ = std::make_shared<AvoidanceByLCParameters>(p);