feat(dynamic_obstacle_stop): check the obstacles behind the EGO

planning/behavior_velocity_dynamic_obstacle_stop_module/config/dynamic_obstacle_stop.param.yaml

@@ -2,11 +2,17 @@
   ros__parameters:
     dynamic_obstacle_stop: # module to stop or before entering the immediate path of a moving object
       extra_object_width: 1.0  # [m] extra width around detected objects
+      extra_object_footprint_width: 0.0 # [m] extra width around detected objects for footprint
       minimum_object_velocity: 0.5  # [m/s] objects with a velocity bellow this value are ignored
       stop_distance_buffer: 0.5  # [m] extra distance to add between the stop point and the collision point
       time_horizon: 5.0  # [s] time horizon used for collision checks
+      yaw_threshold: 2.35 # [rad] yaw threshold used for collision checks
+      yaw_threshold_behind_object: 1.0 # [rad] yaw threshold used for collision checks for objects behind the ego vehicle
       hysteresis: 1.0  # [m] once a collision has been detected, this hysteresis is used on the collision detection
       add_stop_duration_buffer : 0.5  # [s] duration where a collision must be continuously detected before a stop decision is added
       remove_stop_duration_buffer : 1.0  # [s] duration between no collision being detected and the stop decision being remove
       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)
+      ignore_objects_behind_ego: false  # if true, ignore objects that are behind the ego vehicle
+      behind_object_distance_threshold: 5.0  # [m] distance behind the ego vehicle to ignore objects
+      use_predicted_path: false  # if true, use the predicted path of the object to calculate the collision point

planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp

@@ -26,33 +26,45 @@
 
 namespace behavior_velocity_planner::dynamic_obstacle_stop
 {
-
 std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
-  const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose,
-  const tier4_autoware_utils::Polygon2d & object_footprint)
+  const EgoData & ego_data, const autoware_perception_msgs::msg::PredictedObject & object,
+  const tier4_autoware_utils::MultiPolygon2d & object_footprints, const PlannerParam & params)
 {
   std::optional<geometry_msgs::msg::Point> closest_collision_point;
   auto closest_dist = std::numeric_limits<double>::max();
   std::vector<BoxIndexPair> rough_collisions;
-  ego_data.rtree.query(
-    boost::geometry::index::intersects(object_footprint), std::back_inserter(rough_collisions));
-  for (const auto & rough_collision : rough_collisions) {
-    const auto path_idx = rough_collision.second;
-    const auto & ego_footprint = ego_data.path_footprints[path_idx];
-    const auto & ego_pose = ego_data.path.points[path_idx].point.pose;
-    const auto angle_diff = tier4_autoware_utils::normalizeRadian(
-      tf2::getYaw(ego_pose.orientation) - tf2::getYaw(object_pose.orientation));
-    if (std::abs(angle_diff) > (M_PI_2 + M_PI_4)) {  // TODO(Maxime): make this angle a parameter
-      tier4_autoware_utils::MultiPoint2d collision_points;
-      boost::geometry::intersection(
-        object_footprint.outer(), ego_footprint.outer(), collision_points);
-      for (const auto & coll_p : collision_points) {
-        auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y());
-        const auto dist_to_ego =
-          motion_utils::calcSignedArcLength(ego_data.path.points, ego_data.pose.position, p);
-        if (dist_to_ego < closest_dist) {
-          closest_dist = dist_to_ego;
-          closest_collision_point = p;
+  for (const auto & object_footprint : object_footprints) {
+    ego_data.rtree.query(
+      boost::geometry::index::intersects(object_footprint), std::back_inserter(rough_collisions));
+    for (const auto & rough_collision : rough_collisions) {
+      const auto path_idx = rough_collision.second;
+      const auto & ego_footprint = ego_data.path_footprints[path_idx];
+      const auto & ego_pose = ego_data.path.points[path_idx].point.pose;
+      const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose;
+      const auto angle_diff = tier4_autoware_utils::normalizeRadian(
+        tf2::getYaw(ego_pose.orientation) - tf2::getYaw(object_pose.orientation));
+      if (
+        (!params.ignore_objects_behind_ego &&
+         std::abs(angle_diff) < params.yaw_threshold_behind_object) ||
+        std::abs(angle_diff) > params.yaw_threshold) {
+        tier4_autoware_utils::MultiPoint2d collision_points;
+        boost::geometry::intersection(object_footprints, ego_footprint.outer(), collision_points);
+        for (const auto & coll_p : collision_points) {
+          auto p = geometry_msgs::msg::Point().set__x(coll_p.x()).set__y(coll_p.y());
+          const auto dist_ego_to_coll =
+            motion_utils::calcSignedArcLength(ego_data.path.points, ego_pose.position, p);
+          const auto dist_obj_to_coll =
+            tier4_autoware_utils::calcDistance2d(object_pose.position, p);
+          const auto tta_cp_npc =
+            abs(dist_obj_to_coll) / object.kinematics.initial_twist_with_covariance.twist.linear.x;
+          const auto tta_cp_ego =
+            dist_ego_to_coll / ego_data.path.points[path_idx].point.longitudinal_velocity_mps;
+          if (
+            abs(dist_ego_to_coll) < closest_dist &&
+            std::abs(tta_cp_npc - tta_cp_ego) < params.time_horizon) {
+            closest_dist = dist_ego_to_coll;
+            closest_collision_point = p;
+          }
         }
       }
     }
@@ -63,18 +75,21 @@
 std::vector<Collision> find_collisions(
   const EgoData & ego_data,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
-  const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints)
+  const std::vector<tier4_autoware_utils::MultiPolygon2d> & object_forward_footprints,
+  const PlannerParam & params)
 {
   std::vector<Collision> collisions;
-  for (auto object_idx = 0UL; object_idx < objects.size(); ++object_idx) {
-    const auto & object_pose = objects[object_idx].kinematics.initial_pose_with_covariance.pose;
-    const auto & object_footprint = object_forward_footprints[object_idx];
-    const auto collision = find_closest_collision_point(ego_data, object_pose, object_footprint);
-    if (collision) {
-      Collision c;
-      c.object_uuid = tier4_autoware_utils::toHexString(objects[object_idx].object_id);
-      c.point = *collision;
-      collisions.push_back(c);
+  std::optional<geometry_msgs::msg::Point> collision;
+  for (const auto & object : objects) {
+    tier4_autoware_utils::MultiPolygon2d object_footprint;
+    for (const auto & polygon : object_forward_footprints) {
+      collision = find_closest_collision_point(ego_data, object, polygon, params);
+      if (collision) {
+        Collision c;
+        c.object_uuid = tier4_autoware_utils::toHexString(object.object_id);
+        c.point = *collision;
+        collisions.push_back(c);
+      }
     }
   }
   return collisions;

planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp

@@ -28,8 +28,8 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop
 /// @param [in] object_footprint footprint of the object used for finding a collision
 /// @return the collision point closest to ego (if any)
 std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
-  const EgoData & ego_data, const geometry_msgs::msg::Pose & object_pose,
-  const tier4_autoware_utils::Polygon2d & object_footprint);
+  const EgoData & ego_data, const autoware_perception_msgs::msg::PredictedObject & object,
+  const tier4_autoware_utils::MultiPolygon2d & object_footprints, const PlannerParam & params);
 
 /// @brief find the earliest collision along the ego path
 /// @param [in] ego_data ego data including its path and footprint
@@ -39,7 +39,8 @@ std::optional<geometry_msgs::msg::Point> find_closest_collision_point(
 std::vector<Collision> find_collisions(
   const EgoData & ego_data,
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
-  const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints);
+  const std::vector<tier4_autoware_utils::MultiPolygon2d> & object_forward_footprints,
+  const PlannerParam & params);
 
 }  // namespace behavior_velocity_planner::dynamic_obstacle_stop
 

planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp

@@ -16,18 +16,21 @@
 
 #include <tier4_autoware_utils/geometry/boost_polygon_utils.hpp>
 
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
-#include <boost/geometry/algorithms/envelope.hpp>
+#include <boost/geometry.hpp>
 
 #include <lanelet2_core/geometry/Polygon.h>
 #include <tf2/utils.h>
 
+#include <algorithm>
 #include <utility>
 #include <vector>
 
 namespace behavior_velocity_planner::dynamic_obstacle_stop
 {
+using Point2d = tier4_autoware_utils::Point2d;
 tier4_autoware_utils::MultiPolygon2d make_forward_footprints(
   const std::vector<autoware_perception_msgs::msg::PredictedObject> & obstacles,
   const PlannerParam & params, const double hysteresis)
@@ -58,6 +61,47 @@ tier4_autoware_utils::Polygon2d make_forward_footprint(
     {}};
 }
 
+// create footprint using predicted_path of object
+tier4_autoware_utils::Polygon2d translate_polygon(
+  const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y)
+{
+  tier4_autoware_utils::Polygon2d translated_polygon;
+  const boost::geometry::strategy::transform::translate_transformer<double, 2, 2> translation(x, y);
+  boost::geometry::transform(polygon, translated_polygon, translation);
+  return translated_polygon;
+}
+
+tier4_autoware_utils::Polygon2d create_footprint(
+  const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint)
+{
+  const auto angle = tf2::getYaw(pose.orientation);
+  return translate_polygon(
+    tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y);
+}
+
+tier4_autoware_utils::MultiPolygon2d create_object_footprints(
+  const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
+  const PlannerParam & params)
+{
+  tier4_autoware_utils::MultiPolygon2d footprints;
+
+  for (const auto & object : objects) {
+    const auto front = object.shape.dimensions.x / 2 + params.extra_object_footprint_width;
+    const auto rear = -object.shape.dimensions.x / 2 - params.extra_object_footprint_width;
+    const auto left = object.shape.dimensions.y / 2 + params.extra_object_footprint_width;
+    const auto right = -object.shape.dimensions.y / 2 - params.extra_object_footprint_width;
+    tier4_autoware_utils::Polygon2d base_footprint;
+    base_footprint.outer() = {
+      Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left},
+      Point2d{front, left}};
+    for (const auto & path : object.kinematics.predicted_paths)
+      for (const auto & pose : path.path)
+        footprints.push_back(create_footprint(pose, base_footprint));
+  }
+
+  return footprints;
+}
+
 tier4_autoware_utils::Polygon2d project_to_pose(
   const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose)
 {

planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp

@@ -19,6 +19,8 @@
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
+
 #include <vector>
 
 namespace behavior_velocity_planner::dynamic_obstacle_stop
@@ -40,6 +42,13 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints(
 tier4_autoware_utils::Polygon2d make_forward_footprint(
   const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params,
   const double hysteresis);
+tier4_autoware_utils::MultiPolygon2d create_object_footprints(
+  const std::vector<autoware_perception_msgs::msg::PredictedObject> & objects,
+  const PlannerParam & params);
+tier4_autoware_utils::Polygon2d translate_polygon(
+  const tier4_autoware_utils::Polygon2d & polygon, const double x, const double y);
+tier4_autoware_utils::Polygon2d create_footprint(
+  const geometry_msgs::msg::Pose & pose, const tier4_autoware_utils::Polygon2d & base_footprint);
 /// @brief project a footprint to the given pose
 /// @param [in] base_footprint footprint to project
 /// @param [in] pose projection pose

planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp

@@ -31,18 +31,27 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node
   auto & pp = planner_param_;
 
   pp.extra_object_width = getOrDeclareParameter<double>(node, ns + ".extra_object_width");
+  pp.extra_object_footprint_width =
+    getOrDeclareParameter<double>(node, ns + ".extra_object_footprint_width");
   pp.minimum_object_velocity = getOrDeclareParameter<double>(node, ns + ".minimum_object_velocity");
   pp.stop_distance_buffer = getOrDeclareParameter<double>(node, ns + ".stop_distance_buffer");
   pp.time_horizon = getOrDeclareParameter<double>(node, ns + ".time_horizon");
+  pp.yaw_threshold = getOrDeclareParameter<double>(node, ns + ".yaw_threshold");
+  pp.yaw_threshold_behind_object =
+    getOrDeclareParameter<double>(node, ns + ".yaw_threshold_behind_object");
   pp.hysteresis = getOrDeclareParameter<double>(node, ns + ".hysteresis");
   pp.add_duration_buffer = getOrDeclareParameter<double>(node, ns + ".add_stop_duration_buffer");
   pp.remove_duration_buffer =
     getOrDeclareParameter<double>(node, ns + ".remove_stop_duration_buffer");
   pp.minimum_object_distance_from_ego_path =
     getOrDeclareParameter<double>(node, ns + ".minimum_object_distance_from_ego_path");
+  pp.behind_object_distance_threshold =
+    getOrDeclareParameter<double>(node, ns + ".behind_object_distance_threshold");
   pp.ignore_unavoidable_collisions =
     getOrDeclareParameter<bool>(node, ns + ".ignore_unavoidable_collisions");
-
+  pp.ignore_objects_behind_ego =
+    getOrDeclareParameter<bool>(node, ns + ".ignore_objects_behind_ego");
+  pp.use_predicted_path = getOrDeclareParameter<bool>(node, ns + ".use_predicted_path");
   const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo();
   pp.ego_lateral_offset =
     std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m);

planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -55,8 +55,22 @@
     const auto obj_arc_length = motion_utils::calcSignedArcLength(
       ego_data.path.points, ego_data.pose.position,
       o.kinematics.initial_pose_with_covariance.pose.position);
-    return obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx +
-                              params.ego_longitudinal_offset + o.shape.dimensions.x / 2.0;
+
+    bool is_behind_object =
+      obj_arc_length < 0.0 && std::abs(obj_arc_length) < params.behind_object_distance_threshold +
+                                                           o.shape.dimensions.x / 2.0;
+    bool is_near_ego =
+      obj_arc_length > 0.0 &&
+      obj_arc_length < params.behind_object_distance_threshold + o.shape.dimensions.x / 2.0;
+    bool is_in_front_of_ego = obj_arc_length > o.shape.dimensions.x / 2.0;
+    bool is_past_ego_end = obj_arc_length > ego_data.longitudinal_offset_to_first_path_idx +
+                                              params.ego_longitudinal_offset +
+                                              o.shape.dimensions.x / 2.0;
+
+    if (!params.ignore_objects_behind_ego)
+      return is_behind_object || (is_near_ego || (is_in_front_of_ego && is_past_ego_end));
+
+    return is_in_front_of_ego || is_past_ego_end;
   };
   const auto is_unavoidable = [&](const auto & o) {
     const auto & o_pose = o.kinematics.initial_pose_with_covariance.pose;

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

@@ -86,48 +86,70 @@
 
   const auto preprocessing_duration_us = stopwatch.toc("preprocessing");
 
-  stopwatch.tic("footprints");
-  const auto obstacle_forward_footprints =
-    make_forward_footprints(dynamic_obstacles, params_, hysteresis);
-  const auto footprints_duration_us = stopwatch.toc("footprints");
-  stopwatch.tic("collisions");
-  auto collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints);
+  std::vector<tier4_autoware_utils::MultiPolygon2d> obstacle_forward_footprints;
+  std::vector<tier4_autoware_utils::MultiPolygon2d> obstacle_predicted_footprints;
+  std::vector<Collision> collisions;
+  double footprints_duration_us;
+  if (params_.use_predicted_path) {
+    stopwatch.tic("footprints");
+    obstacle_predicted_footprints.push_back(create_object_footprints(dynamic_obstacles, params_));
+    footprints_duration_us = stopwatch.toc("footprints");
+    stopwatch.tic("collisions");
+    collisions =
+      find_collisions(ego_data, dynamic_obstacles, obstacle_predicted_footprints, params_);
+    for (const auto & footprint : obstacle_predicted_footprints) {
+      for (const auto & fp : footprint) {
+        debug_data_.obstacle_footprints.push_back(fp);
+      }
+    }
+  } else {
+    stopwatch.tic("footprints");
+    obstacle_forward_footprints.push_back(
+      make_forward_footprints(dynamic_obstacles, params_, hysteresis));
+    footprints_duration_us = stopwatch.toc("footprints");
+    stopwatch.tic("collisions");
+    collisions = find_collisions(ego_data, dynamic_obstacles, obstacle_forward_footprints, params_);
+    for (const auto & footprint : obstacle_forward_footprints) {
+      for (const auto & fp : footprint) {
+        debug_data_.obstacle_footprints.push_back(fp);
+      }
+    }
+  }
   update_object_map(object_map_, collisions, clock_->now(), ego_data.path.points, params_);
   std::optional<geometry_msgs::msg::Point> earliest_collision =
     find_earliest_collision(object_map_, ego_data);
   const auto collisions_duration_us = stopwatch.toc("collisions");
   if (earliest_collision) {
     const auto arc_length_diff = motion_utils::calcSignedArcLength(
       ego_data.path.points, *earliest_collision, ego_data.pose.position);
     const auto can_stop_before_limit = arc_length_diff < min_stop_distance -
                                                            params_.ego_longitudinal_offset -
                                                            params_.stop_distance_buffer;
     const auto stop_pose = can_stop_before_limit
                              ? motion_utils::calcLongitudinalOffsetPose(
                                  ego_data.path.points, *earliest_collision,
                                  -params_.stop_distance_buffer - params_.ego_longitudinal_offset)
                              : ego_data.earliest_stop_pose;
     debug_data_.stop_pose = stop_pose;
     if (stop_pose) {
       motion_utils::insertStopPoint(*stop_pose, 0.0, path->points);
       const auto stop_pose_reached =
         planner_data_->current_velocity->twist.linear.x < 1e-3 &&
         tier4_autoware_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3;
       velocity_factor_.set(
         path->points, ego_data.pose, *stop_pose,
         stop_pose_reached ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING,
         "dynamic_obstacle_stop");
     }
   }
 
   const auto total_time_us = stopwatch.toc();
   RCLCPP_DEBUG(
     logger_,
     "Total time = %2.2fus\n\tpreprocessing = %2.2fus\n\tfootprints = "
     "%2.2fus\n\tcollisions = %2.2fus\n",
     total_time_us, preprocessing_duration_us, footprints_duration_us, collisions_duration_us);
   debug_data_.ego_footprints = ego_data.path_footprints;
-  debug_data_.obstacle_footprints = obstacle_forward_footprints;
   debug_data_.z = ego_data.pose.position.z;
   return true;
 }

planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp

@@ -38,16 +38,22 @@ using Rtree = boost::geometry::index::rtree<BoxIndexPair, boost::geometry::index
 struct PlannerParam
 {
   double extra_object_width;
+  double extra_object_footprint_width;
   double minimum_object_velocity;
   double stop_distance_buffer;
   double time_horizon;
+  double yaw_threshold;
+  double yaw_threshold_behind_object;
   double hysteresis;
   double add_duration_buffer;
   double remove_duration_buffer;
   double ego_longitudinal_offset;
   double ego_lateral_offset;
   double minimum_object_distance_from_ego_path;
+  double behind_object_distance_threshold;
   bool ignore_unavoidable_collisions;
+  bool ignore_objects_behind_ego;
+  bool use_predicted_path;
 };
 
 struct EgoData