feat(avoidance): flexible avoidance safety check param (autowarefound…

…ation#4754)

Signed-off-by: satoshi-ota <[email protected]>

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

@@ -142,7 +142,8 @@
         # 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]

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

@@ -193,7 +193,8 @@ 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

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/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> {
@@ -1919,8 +1921,18 @@ bool AvoidanceModule::isSafePath(
     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(
+      shifted_path.path, getEgoPose(), p.vehicle_info, obj_polygon);
+
     const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj(
       object, parameters_->check_all_predicted_path);
+
+    const auto & ego_predicted_path =
+      is_object_front ? 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(

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

@@ -143,10 +143,15 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.check_current_lane = get_parameter<bool>(node, ns + "check_current_lane");
     p.check_shift_side_lane = get_parameter<bool>(node, ns + "check_shift_side_lane");
     p.check_other_side_lane = get_parameter<bool>(node, ns + "check_other_side_lane");
-    p.check_unavoidable_object = get_parameter<bool>(node, ns + "check_unavoidable_object");
+    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.check_all_predicted_path =
+      get_parameter<bool>(node, ns + "check_all_predicted_path");
+    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");

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -1310,7 +1310,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck(
 
 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)
 {
   if (path.points.empty()) {
     return {};
@@ -1319,7 +1319,8 @@ std::vector<PoseWithVelocityStamped> convertToPredictedPath(
   const auto & acceleration = parameters->max_acceleration;
   const auto & vehicle_pose = planner_data->self_odometry->pose.pose;
   const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x);
-  const auto & time_horizon = parameters->safety_check_time_horizon;
+  const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object
+                                              : parameters->time_horizon_for_rear_object;
   const auto & time_resolution = parameters->safety_check_time_resolution;
 
   const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points);
@@ -1350,7 +1351,8 @@ ExtendedPredictedObject transform(
   extended_object.shape = object.shape;
 
   const auto & obj_velocity = extended_object.initial_twist.twist.linear.x;
-  const auto & time_horizon = parameters->safety_check_time_horizon;
+  const auto & time_horizon =
+    std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object);
   const auto & time_resolution = parameters->safety_check_time_resolution;
 
   extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size());