feat(run_out_module): exclude obstacles on path (autowarefoundation#6703

)

* WIP add check for target object in path

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* WIP exclude obstacles on ego path

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* use lat deviation to check if obstacle is on path or not

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* delete unused methods and variables

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* readme and comments

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add uuids to dynamic obstacle type

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* use lib funct to generate uuid

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add timeout to keep last run out candidate for some time

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* add time threshold to maintain obstacles that enter path

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* change logic, ignore obstacle in path after some time passes

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

* readme

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

---------

Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>

planning/behavior_velocity_run_out_module/README.md

@@ -108,6 +108,14 @@ You can choose whether to use this feature by parameter of `use_partition_lanele
 
 ![brief](./docs/exclude_obstacles_by_partition.svg)
 
+##### Exclude obstacles by label
+
+This module only acts on target obstacles defined by the `target_obstacle_types` parameter. If an obstacle of a type not specified by said parameter is detected, it will be ignored by this module.
+
+##### Exclude obstacles that are already on the ego vehicle's path
+
+In the cases were an obstacle is already on the ego vehicle's path, it cannot "cut in" into the ego's path anymore (which is the situation this module tries to handle) so it might be useful to exclude obstacles already on the vehicle's footprint path. By setting the parameter `exclude_obstacles_already_in_path` to true, this module will exclude the obstacles that are considered to be already on the ego vehicle's path for more than `keep_obstacle_on_path_time_threshold`. The module considers the ego vehicle's closest path point to each obstacle's current position, and determines the lateral distance between the obstacle and the right and left sides of the ego vehicle. If the obstacle is located within the left and right extremes of the vehicle for that pose, then it is considered to be inside the ego vehicle's footprint path and it is excluded. The virtual width of the vehicle used to detect if an obstacle is within the path or not, can be adjusted by the `ego_footprint_extra_margin` parameter.
+
 ##### Exclude obstacles that cross the ego vehicle's "cut line"
 
 This module can exclude obstacles that have predicted paths that will cross the back side of the ego vehicle. It excludes obstacles if their predicted path crosses the ego's "cut line". The "cut line" is a virtual line segment that is perpendicular to the ego vehicle and that passes through the ego's base link.

planning/behavior_velocity_run_out_module/config/run_out.param.yaml

@@ -3,6 +3,7 @@
     run_out:
       detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
       target_obstacle_types: ["PEDESTRIAN", "BICYCLE", "MOTORCYCLE"] # [-] Obstacle types considered by this module
+      exclude_obstacles_already_in_path: true # If the obstacle is already on the ego's path footprint, ignore it.
       use_partition_lanelet: true # [-] whether to use partition lanelet map data
       suppress_on_crosswalk: true # [-] whether to suppress on crosswalk lanelet
       use_ego_cut_line: true      # [-] filter obstacles that pass the backside of ego: if a dynamic obstacle's predicted path intersects this line, it is ignored
@@ -14,6 +15,8 @@
       detection_span: 1.0         # [m] calculate collision with this span to reduce calculation time
       min_vel_ego_kmph: 3.6       # [km/h] min velocity to calculate time to collision
       ego_cut_line_length: 3.0    # The width of the ego's cut line
+      ego_footprint_extra_margin: 0.5 # [m] expand the ego vehicles' footprint by this value on all sides when building the ego footprint path
+      keep_obstacle_on_path_time_threshold: 1.0 # [s] How much time a previous run out target obstacle is kept in the run out candidate list if it enters the ego path.
 
       # Parameter to create abstracted dynamic obstacles
       dynamic_obstacle:

planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp

@@ -18,6 +18,7 @@
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/geometry/pose_deviation.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 #include <tier4_autoware_utils/transform/transforms.hpp>
 
 #include <boost/geometry/algorithms/covered_by.hpp>
@@ -373,6 +374,7 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObject::createDynamicObsta
     }
     dynamic_obstacle.classifications = predicted_object.classification;
     dynamic_obstacle.shape = predicted_object.shape;
+    dynamic_obstacle.uuid = predicted_object.object_id;
 
     // get predicted paths of predicted_objects
     for (const auto & path : predicted_object.kinematics.predicted_paths) {
@@ -418,6 +420,7 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForObjectWithoutPath::createD
       param_.max_prediction_time);
     predicted_path.confidence = 1.0;
     dynamic_obstacle.predicted_paths.emplace_back(predicted_path);
+    dynamic_obstacle.uuid = predicted_object.object_id;
 
     dynamic_obstacles.emplace_back(dynamic_obstacle);
   }
@@ -490,7 +493,7 @@ std::vector<DynamicObstacle> DynamicObstacleCreatorForPoints::createDynamicObsta
       param_.max_prediction_time);
     predicted_path.confidence = 1.0;
     dynamic_obstacle.predicted_paths.emplace_back(predicted_path);
-
+    dynamic_obstacle.uuid = tier4_autoware_utils::generateUUID();
     dynamic_obstacles.emplace_back(dynamic_obstacle);
   }
 

planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp

@@ -22,6 +22,7 @@
 #include <behavior_velocity_planner_common/utilization/path_utilization.hpp>
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <message_filters/subscriber.h>

planning/behavior_velocity_run_out_module/src/manager.cpp

@@ -62,6 +62,8 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
       getOrDeclareParameter<std::vector<std::string>>(node, ns + ".target_obstacle_types");
     p.use_partition_lanelet = getOrDeclareParameter<bool>(node, ns + ".use_partition_lanelet");
     p.use_ego_cut_line = getOrDeclareParameter<bool>(node, ns + ".use_ego_cut_line");
+    p.exclude_obstacles_already_in_path =
+      getOrDeclareParameter<bool>(node, ns + ".exclude_obstacles_already_in_path");
     p.suppress_on_crosswalk = getOrDeclareParameter<bool>(node, ns + ".suppress_on_crosswalk");
     p.specify_decel_jerk = getOrDeclareParameter<bool>(node, ns + ".specify_decel_jerk");
     p.stop_margin = getOrDeclareParameter<double>(node, ns + ".stop_margin");
@@ -71,6 +73,10 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     p.detection_span = getOrDeclareParameter<double>(node, ns + ".detection_span");
     p.min_vel_ego_kmph = getOrDeclareParameter<double>(node, ns + ".min_vel_ego_kmph");
     p.ego_cut_line_length = getOrDeclareParameter<double>(node, ns + ".ego_cut_line_length");
+    p.ego_footprint_extra_margin =
+      getOrDeclareParameter<double>(node, ns + ".ego_footprint_extra_margin");
+    p.keep_obstacle_on_path_time_threshold =
+      getOrDeclareParameter<double>(node, ns + ".keep_obstacle_on_path_time_threshold");
   }
 
   {

planning/behavior_velocity_run_out_module/src/scene.cpp

@@ -22,6 +22,7 @@
 #include <motion_utils/distance/distance.hpp>
 #include <motion_utils/trajectory/trajectory.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
 
 #include <boost/geometry/algorithms/intersection.hpp>
 #include <boost/geometry/algorithms/within.hpp>
@@ -36,6 +37,7 @@
 namespace behavior_velocity_planner
 {
 namespace bg = boost::geometry;
+using object_recognition_utils::convertLabelToString;
 
 RunOutModule::RunOutModule(
   const int64_t module_id, const std::shared_ptr<const PlannerData> & planner_data,
@@ -103,16 +105,18 @@ bool RunOutModule::modifyPathVelocity(
   debug_ptr_->setDebugValues(DebugValues::TYPE::NUM_OBSTACLES, dynamic_obstacles.size());
 
   const auto filtered_obstacles = std::invoke([&]() {
+    // Only target_obstacle_types are considered.
+    const auto target_obstacles = excludeObstaclesBasedOnLabel(dynamic_obstacles);
     // extract obstacles using lanelet information
     const auto partition_excluded_obstacles =
-      excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose);
-
-    if (!planner_param_.run_out.use_ego_cut_line) return partition_excluded_obstacles;
-
+      excludeObstaclesOutSideOfPartition(target_obstacles, extended_smoothed_path, current_pose);
     // extract obstacles that cross the ego's cut line
     const auto ego_cut_line_excluded_obstacles =
       excludeObstaclesCrossingEgoCutLine(partition_excluded_obstacles, current_pose);
-    return ego_cut_line_excluded_obstacles;
+    // extract obstacles already on the ego's path
+    const auto obstacles_outside_ego_path =
+      excludeObstaclesOnEgoPath(ego_cut_line_excluded_obstacles, extended_smoothed_path);
+    return obstacles_outside_ego_path;
   });
 
   // record time for obstacle creation
@@ -327,27 +331,79 @@ std::vector<geometry_msgs::msg::Point> RunOutModule::createVehiclePolygon(
   return vehicle_poly;
 }
 
+std::vector<DynamicObstacle> RunOutModule::excludeObstaclesOnEgoPath(
+  const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path)
+{
+  using motion_utils::findNearestIndex;
+  using tier4_autoware_utils::calcOffsetPose;
+  if (!planner_param_.run_out.exclude_obstacles_already_in_path || path.points.empty()) {
+    return dynamic_obstacles;
+  }
+  const auto footprint_extra_margin = planner_param_.run_out.ego_footprint_extra_margin;
+  const auto vehicle_width = planner_param_.vehicle_param.width;
+  const auto vehicle_with_with_margin_halved = (vehicle_width + footprint_extra_margin) / 2.0;
+  const double time_threshold_for_prev_collision_obstacle =
+    planner_param_.run_out.keep_obstacle_on_path_time_threshold;
+
+  std::vector<DynamicObstacle> obstacles_outside_of_path;
+  std::for_each(dynamic_obstacles.begin(), dynamic_obstacles.end(), [&](const auto & o) {
+    const auto idx = findNearestIndex(path.points, o.pose);
+    if (!idx) {
+      obstacles_outside_of_path.push_back(o);
+      return;
+    }
+    const auto object_position = o.pose.position;
+    const auto closest_ego_pose = path.points.at(*idx).point.pose;
+    const auto vehicle_left_pose =
+      tier4_autoware_utils::calcOffsetPose(closest_ego_pose, 0, vehicle_with_with_margin_halved, 0);
+    const auto vehicle_right_pose = tier4_autoware_utils::calcOffsetPose(
+      closest_ego_pose, 0, -vehicle_with_with_margin_halved, 0);
+    const double signed_distance_from_left =
+      tier4_autoware_utils::calcLateralDeviation(vehicle_left_pose, object_position);
+    const double signed_distance_from_right =
+      tier4_autoware_utils::calcLateralDeviation(vehicle_right_pose, object_position);
+
+    // If object is outside of the ego path, include it in list of possible target objects
+    // It is also deleted from the path of objects inside ego path
+    const auto obstacle_uuid_hex = tier4_autoware_utils::toHexString(o.uuid);
+    if (signed_distance_from_left > 0.0 || signed_distance_from_right < 0.0) {
+      obstacles_outside_of_path.push_back(o);
+      obstacle_in_ego_path_times_.erase(obstacle_uuid_hex);
+      return;
+    }
+
+    const auto it = obstacle_in_ego_path_times_.find(obstacle_uuid_hex);
+    // This obstacle is first detected inside the ego path, we keep it.
+    if (it == obstacle_in_ego_path_times_.end()) {
+      const auto now = clock_->now().seconds();
+      obstacle_in_ego_path_times_.insert(std::make_pair(obstacle_uuid_hex, now));
+      obstacles_outside_of_path.push_back(o);
+      return;
+    }
+
+    // if the obstacle has been on the ego path for more than a threshold time, it is excluded
+    const auto first_detection_inside_path_time = it->second;
+    const auto now = clock_->now().seconds();
+    const double elapsed_time_since_detection_inside_of_path =
+      (now - first_detection_inside_path_time);
+    if (elapsed_time_since_detection_inside_of_path < time_threshold_for_prev_collision_obstacle) {
+      obstacles_outside_of_path.push_back(o);
+      return;
+    }
+  });
+  return obstacles_outside_of_path;
+}
+
 std::vector<DynamicObstacle> RunOutModule::checkCollisionWithObstacles(
   const std::vector<DynamicObstacle> & dynamic_obstacles,
   std::vector<geometry_msgs::msg::Point> poly, const float travel_time,
   const std::vector<std::pair<int64_t, lanelet::ConstLanelet>> & crosswalk_lanelets) const
 {
-  using object_recognition_utils::convertLabelToString;
   const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly);
 
   // check collision for each objects
   std::vector<DynamicObstacle> obstacles_collision;
   for (const auto & obstacle : dynamic_obstacles) {
-    // get classification that has highest probability
-    const auto classification =
-      convertLabelToString(run_out_utils::getHighestProbLabel(obstacle.classifications));
-    const auto & target_obstacle_types = planner_param_.run_out.target_obstacle_types;
-    // detect only pedestrians, bicycles or motorcycles
-    const auto it =
-      std::find(target_obstacle_types.begin(), target_obstacle_types.end(), classification);
-    if (it == target_obstacle_types.end()) {
-      continue;
-    }
     // calculate predicted obstacle pose for min velocity and max velocity
     const auto predicted_obstacle_pose_min_vel =
       calcPredictedObstaclePose(obstacle.predicted_paths, travel_time, obstacle.min_velocity_mps);
@@ -823,6 +879,7 @@ std::vector<DynamicObstacle> RunOutModule::excludeObstaclesCrossingEgoCutLine(
   const std::vector<DynamicObstacle> & dynamic_obstacles,
   const geometry_msgs::msg::Pose & current_pose) const
 {
+  if (!planner_param_.run_out.use_ego_cut_line) return dynamic_obstacles;
   std::vector<DynamicObstacle> extracted_obstacles;
   std::vector<geometry_msgs::msg::Point> ego_cut_line;
   const double ego_cut_line_half_width = planner_param_.run_out.ego_cut_line_length / 2.0;
@@ -837,6 +894,25 @@ std::vector<DynamicObstacle> RunOutModule::excludeObstaclesCrossingEgoCutLine(
   return extracted_obstacles;
 }
 
+std::vector<DynamicObstacle> RunOutModule::excludeObstaclesBasedOnLabel(
+  const std::vector<DynamicObstacle> & dynamic_obstacles) const
+{
+  std::vector<DynamicObstacle> output;
+  const auto & target_obstacle_types = planner_param_.run_out.target_obstacle_types;
+  std::for_each(dynamic_obstacles.begin(), dynamic_obstacles.end(), [&](const auto obstacle) {
+    // get classification that has highest probability
+    const auto classification =
+      convertLabelToString(run_out_utils::getHighestProbLabel(obstacle.classifications));
+    // include only pedestrians, bicycles or motorcycles
+    const auto it =
+      std::find(target_obstacle_types.begin(), target_obstacle_types.end(), classification);
+    if (it != target_obstacle_types.end()) {
+      output.push_back(obstacle);
+    }
+  });
+  return output;
+}
+
 std::vector<DynamicObstacle> RunOutModule::excludeObstaclesOutSideOfPartition(
   const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
   const geometry_msgs::msg::Pose & current_pose) const
@@ -909,6 +985,7 @@ bool RunOutModule::isMomentaryDetection()
     return false;
   }
 
+  // No detection until now
   if (!first_detected_time_) {
     first_detected_time_ = std::make_shared<rclcpp::Time>(clock_->now());
     return true;

planning/behavior_velocity_run_out_module/src/scene.hpp

@@ -25,6 +25,8 @@
 
 #include <memory>
 #include <optional>
+#include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
@@ -64,6 +66,8 @@ class RunOutModule : public SceneModuleInterface
   std::shared_ptr<RunOutDebug> debug_ptr_;
   std::unique_ptr<run_out_utils::StateMachine> state_machine_;
   std::shared_ptr<rclcpp::Time> first_detected_time_;
+  std::optional<unique_identifier_msgs::msg::UUID> last_stop_obstacle_uuid_;
+  std::unordered_map<std::string, double> obstacle_in_ego_path_times_;
 
   // Function
   Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const;
@@ -148,10 +152,16 @@ class RunOutModule : public SceneModuleInterface
     const std::vector<DynamicObstacle> & dynamic_obstacles,
     const geometry_msgs::msg::Pose & current_pose) const;
 
+  std::vector<DynamicObstacle> excludeObstaclesBasedOnLabel(
+    const std::vector<DynamicObstacle> & dynamic_obstacles) const;
+
   std::vector<DynamicObstacle> excludeObstaclesOutSideOfPartition(
     const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
     const geometry_msgs::msg::Pose & current_pose) const;
 
+  std::vector<DynamicObstacle> excludeObstaclesOnEgoPath(
+    const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path);
+
   void publishDebugValue(
     const PathWithLaneId & path, const std::vector<DynamicObstacle> extracted_obstacles,
     const std::optional<DynamicObstacle> & dynamic_obstacle,

planning/behavior_velocity_run_out_module/src/utils.hpp

@@ -21,13 +21,14 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/shape.hpp>
 #include <autoware_auto_planning_msgs/msg/path_point.hpp>
 #include <tier4_debug_msgs/msg/float32_stamped.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
 
 #include <string>
 #include <vector>
-
 namespace behavior_velocity_planner
 {
 namespace run_out_utils
@@ -60,10 +61,13 @@ struct RunOutParam
   bool suppress_on_crosswalk;
   bool specify_decel_jerk;
   bool use_ego_cut_line;
+  bool exclude_obstacles_already_in_path;
   double stop_margin;
   double passing_margin;
   double deceleration_jerk;
   double ego_cut_line_length;
+  double ego_footprint_extra_margin;
+  double keep_obstacle_on_path_time_threshold;
   float detection_distance;
   float detection_span;
   float min_vel_ego_kmph;
@@ -187,6 +191,7 @@ struct DynamicObstacle
   std::vector<ObjectClassification> classifications;
   Shape shape;
   std::vector<PredictedPath> predicted_paths;
+  unique_identifier_msgs::msg::UUID uuid;
 };
 
 struct DynamicObstacleData