add option to ignore unavoidable collisions

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml

@@ -8,3 +8,4 @@
       hysteresis: 1.0  # [m] once a collision has been detected, this hysteresis is used on the collision detection
       decision_duration_buffer : 1.0  # [s] duration between no collision being detected and the stop decision being cancelled
       minimum_object_distance_from_ego_path: 1.0  # [m] minimum distance between the footprints of ego and an object to consider for collision
+      ignore_unavoidable_collisions : true  # if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight)

planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp

@@ -39,6 +39,8 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node
     getOrDeclareParameter<double>(node, ns + ".decision_duration_buffer");
   pp.minimum_object_distance_from_ego_path =
     getOrDeclareParameter<double>(node, ns + ".minimum_object_distance_from_ego_path");
+  pp.ignore_unavoidable_collisions =
+    getOrDeclareParameter<bool>(node, ns + ".ignore_unavoidable_collisions");
 
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
   pp.ego_lateral_offset =

planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -59,13 +59,34 @@ std::vector<autoware_auto_perception_msgs::msg::PredictedObject> filter_predicte
     return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx +
                               params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0;
   };
-  for (const auto & object : objects.objects)
+  const auto is_unavoidable = [&](const auto & o) {
+    const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose;
+    const auto o_yaw = tf2::getYaw(o_pose.orientation);
+    const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation);
+    const auto yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(o_yaw - ego_yaw));
+    const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4;
+    auto distance = std::abs(
+      (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) -
+      (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw));
+    if (ego_data.earliest_stop_pose) {
+      distance = std::min(
+        distance,
+        std::abs(
+          (o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) -
+          (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)));
+    }
+    return opposite_heading &&
+           distance <= params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis;
+  };
+  for (const auto & object : objects.objects) {
     if (
       is_vehicle(object) &&
       object.kinematics.initial_twist_with_covariance.twist.linear.x >=
         params.minimum_object_velocity &&
-      is_in_range(object) && is_not_too_close(object))
+      is_in_range(object) && is_not_too_close(object) &&
+      (!params.ignore_unavoidable_collisions || !is_unavoidable(object)))
       filtered_objects.push_back(object);
+  }
   return filtered_objects;
 }
 }  // namespace behavior_velocity_planner::dynamic_obstacle_stop

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

@@ -65,6 +65,15 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
     motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position);
   ego_data.longitudinal_offset_to_first_path_idx = motion_utils::calcLongitudinalOffsetToSegment(
     ego_data.path.points, ego_data.first_path_idx, ego_data.pose.position);
+  const auto min_stop_distance =
+    motion_utils::calcDecelDistWithJerkAndAccConstraints(
+      planner_data_->current_velocity->twist.linear.x, 0.0,
+      planner_data_->current_acceleration->accel.accel.linear.x,
+      planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold,
+      planner_data_->max_stop_jerk_threshold)
+      .value_or(0.0);
+  ego_data.earliest_stop_pose = motion_utils::calcLongitudinalOffsetPose(
+    ego_data.path.points, ego_data.pose.position, min_stop_distance);
 
   make_ego_footprint_rtree(ego_data, params_);
   const auto has_decided_to_stop =
@@ -80,13 +89,6 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
   const auto obstacle_forward_footprints =
     make_forward_footprints(dynamic_obstacles, params_, hysteresis);
   const auto footprints_duration_us = stopwatch.toc("footprints");
-  const auto min_stop_distance =
-    motion_utils::calcDecelDistWithJerkAndAccConstraints(
-      planner_data_->current_velocity->twist.linear.x, 0.0,
-      planner_data_->current_acceleration->accel.accel.linear.x,
-      planner_data_->max_stop_acceleration_threshold, -planner_data_->max_stop_jerk_threshold,
-      planner_data_->max_stop_jerk_threshold)
-      .value_or(0.0);
   stopwatch.tic("collisions");
   const auto collision =
     find_earliest_collision(ego_data, dynamic_obstacles, obstacle_forward_footprints, debug_data_);
@@ -101,8 +103,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
                              ? motion_utils::calcLongitudinalOffsetPose(
                                  ego_data.path.points, *collision,
                                  -params_.stop_distance_buffer - params_.ego_longitudinal_offset)
-                             : motion_utils::calcLongitudinalOffsetPose(
-                                 ego_data.path.points, ego_data.pose.position, min_stop_distance);
+                             : ego_data.earliest_stop_pose;
     if (stop_pose) {
       const auto use_new_stop_pose = !has_decided_to_stop || motion_utils::calcSignedArcLength(
                                                                path->points, stop_pose->position,

planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp

@@ -44,6 +44,7 @@ struct PlannerParam
   double ego_longitudinal_offset;
   double ego_lateral_offset;
   double minimum_object_distance_from_ego_path;
+  bool ignore_unavoidable_collisions;
 };
 
 struct EgoData
@@ -54,6 +55,7 @@ struct EgoData
   geometry_msgs::msg::Pose pose;
   tier4_autoware_utils::MultiPolygon2d path_footprints;
   Rtree rtree;
+  std::optional<geometry_msgs::msg::Pose> earliest_stop_pose;
 };
 
 /// @brief debug data