feat(dynamic_obstacle_stop): add option to ignore unavoidable collisi…

…ons (autowarefoundation#6594)

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_dynamic_obstacle_stop_module/README.md

@@ -38,8 +38,11 @@ An object is considered by the module only if it meets all of the following cond
 - it is a vehicle (pedestrians are ignored);
 - it is moving at a velocity higher than defined by the `minimum_object_velocity` parameter;
 - it is not too close to the current position of the ego vehicle;
+- it is not unavoidable (only if parameter `ignore_unavoidable_collisions` is set to `true`);
 - it is close to the ego path.
 
+An object is considered unavoidable if it is heading towards the ego vehicle such that even if ego stops, a collision would still occur (assuming the object keeps driving in a straight line).
+
 For the last condition,
 the object is considered close enough if its lateral distance from the ego path is less than the threshold parameter `minimum_object_distance_from_ego_path` plus half the width of ego and of the object (including the `extra_object_width` parameter).
 In addition, the value of the `hysteresis` parameter is added to the minimum distance if a stop point was inserted in the previous iteration.
@@ -74,12 +77,13 @@ the stop point arc length is calculated to be the arc length of the previously f
 
 ### Module Parameters
 
-| Parameter                               | Type   | Description                                                                                |
-| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------ |
-| `extra_object_width`                    | double | [m] extra width around detected objects                                                    |
-| `minimum_object_velocity`               | double | [m/s] objects with a velocity bellow this value are ignored                                |
-| `stop_distance_buffer`                  | double | [m] extra distance to add between the stop point and the collision point                   |
-| `time_horizon`                          | double | [s] time horizon used for collision checks                                                 |
-| `hysteresis`                            | double | [m] once a collision has been detected, this hysteresis is used on the collision detection |
-| `decision_duration_buffer`              | double | [s] duration between no collision being detected and the stop decision being cancelled     |
-| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision |
+| Parameter                               | Type   | Description                                                                                                        |
+| --------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ |
+| `extra_object_width`                    | double | [m] extra width around detected objects                                                                            |
+| `minimum_object_velocity`               | double | [m/s] objects with a velocity bellow this value are ignored                                                        |
+| `stop_distance_buffer`                  | double | [m] extra distance to add between the stop point and the collision point                                           |
+| `time_horizon`                          | double | [s] time horizon used for collision checks                                                                         |
+| `hysteresis`                            | double | [m] once a collision has been detected, this hysteresis is used on the collision detection                         |
+| `decision_duration_buffer`              | double | [s] duration between no collision being detected and the stop decision being cancelled                             |
+| `minimum_object_distance_from_ego_path` | double | [m] minimum distance between the footprints of ego and an object to consider for collision                         |
+| `ignore_unavoidable_collisions`         | bool   | [-] if true, ignore collisions that cannot be avoided by stopping (assuming the obstacle continues going straight) |

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,37 @@ 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;
+    const auto collision_distance_threshold =
+      params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis;
+    // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_point_and_angle
+    const auto lat_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));
+    auto has_collision = lat_distance <= collision_distance_threshold;
+    if (ego_data.earliest_stop_pose) {
+      const auto collision_at_earliest_stop_pose =
+        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)) <=
+        collision_distance_threshold;
+      has_collision |= collision_at_earliest_stop_pose;
+    }
+    return opposite_heading && has_collision;
+  };
+  for (const auto & object : objects.objects) {
+    const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >=
+                                 params.minimum_object_velocity;
     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_vehicle(object) && is_not_too_slow && 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

@@ -44,7 +44,9 @@ DynamicObstacleStopModule::DynamicObstacleStopModule(
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param))
 {
-  prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type());
+  prev_stop_decision_time_ =
+    clock->now() -
+    rclcpp::Duration(std::chrono::duration<double>(params_.decision_duration_buffer));
   velocity_factor_.init(PlanningBehavior::UNKNOWN);
 }
 
@@ -65,10 +67,20 @@ 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 start_time = clock_->now();
   const auto has_decided_to_stop =
-    (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer;
+    (start_time - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer;
   if (!has_decided_to_stop) current_stop_pose_.reset();
   double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0;
   const auto dynamic_obstacles =
@@ -80,13 +92,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,14 +106,13 @@ 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,
                                                                current_stop_pose_->position) > 0.0;
       if (use_new_stop_pose) current_stop_pose_ = *stop_pose;
-      prev_stop_decision_time_ = clock_->now();
+      prev_stop_decision_time_ = start_time;
     }
   }
 

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