feat(behavior_velocity_run_out_module): exclude obstacle crossing ego…

… back line (autowarefoundation#6680) * add method to ignore target obstacles that cross ego cut lane Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * WIP add debug support Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add params and finish debug marker Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * change lane to line Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * use autoware utils to get the cut line Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * simplify code wit calcOffsetPose Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * Update readme and eliminate unused code Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update readme Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * eliminate unused function Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * readme Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * comments and readme Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * eliminate unused include Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * typo Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * rename param for consistency Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * change lane to line for consistency Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * rename for clarity, add brief Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * fix indentation Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update description Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * lane ->line Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * lane -> line Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
Interplai · Jun 3, 2024 · 07ab68c · 07ab68c
1 parent e6fdd9e
commit 07ab68c
Show file tree

Hide file tree

Showing 10 changed files with 235 additions and 6 deletions.
diff --git a/planning/behavior_velocity_run_out_module/README.md b/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 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.
+
+You can choose whether to use this feature by setting the parameter `use_ego_cut_line` to `true` or `false`. The width of the line can be tuned with the parameter `ego_cut_line_length`.
+
+![brief](./docs/ego_cut_line.svg)
+
 #### Collision detection
 
 ##### Detect collision with dynamic obstacles

diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml
@@ -4,13 +4,15 @@
       detection_method: "Object"  # [-] candidate: Object, ObjectWithoutPath, Points
       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
       specify_decel_jerk: true    # [-] whether to specify jerk when ego decelerates
       stop_margin: 2.5            # [m] the vehicle decelerates to be able to stop with this margin
       passing_margin: 1.1         # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin
       deceleration_jerk: -0.3     # [m/s^3] ego decelerates with this jerk when stopping for obstacles
       detection_distance: 45.0    # [m] ahead distance from ego to detect the obstacles
       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
 
       # Parameter to create abstracted dynamic obstacles
       dynamic_obstacle:

diff --git a/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg b/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg
diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp
@@ -86,6 +86,14 @@ void RunOutDebug::pushCollisionPoints(const geometry_msgs::msg::Point & point)
   collision_points_.push_back(point_with_height);
 }
 
+void RunOutDebug::pushEgoCutLine(const std::vector<geometry_msgs::msg::Point> & line)
+{
+  for (const auto & point : line) {
+    const auto point_with_height = createPoint(point.x, point.y, height_);
+    ego_cut_line_.push_back(point_with_height);
+  }
+}
+
 void RunOutDebug::pushCollisionPoints(const std::vector<geometry_msgs::msg::Point> & points)
 {
   for (const auto & p : points) {
@@ -160,6 +168,7 @@ void RunOutDebug::clearDebugMarker()
   predicted_obstacle_polygons_.clear();
   collision_obstacle_polygons_.clear();
   travel_time_texts_.clear();
+  ego_cut_line_.clear();
 }
 
 visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray()
@@ -265,6 +274,16 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray
       &msg);
   }
 
+  if (!ego_cut_line_.empty()) {
+    auto marker = createDefaultMarker(
+      "map", current_time, "ego_cut_line", 0, visualization_msgs::msg::Marker::LINE_LIST,
+      createMarkerScale(0.2, 0.2, 0.2), createMarkerColor(0.7, 0.0, 0.7, 0.999));
+    for (const auto & p : ego_cut_line_) {
+      marker.points.push_back(p);
+    }
+    msg.markers.push_back(marker);
+  }
+
   if (!travel_time_texts_.empty()) {
     auto marker = createDefaultMarker(
       "map", current_time, "travel_time_texts", 0,

diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp
@@ -109,6 +109,7 @@ class RunOutDebug
   void pushPredictedVehiclePolygons(const std::vector<geometry_msgs::msg::Point> & polygon);
   void pushPredictedObstaclePolygons(const std::vector<geometry_msgs::msg::Point> & polygon);
   void pushCollisionObstaclePolygons(const std::vector<geometry_msgs::msg::Point> & polygon);
+  void pushEgoCutLine(const std::vector<geometry_msgs::msg::Point> & line);
   void pushDetectionAreaPolygons(const Polygon2d & debug_polygon);
   void pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_polygon);
   void pushTravelTimeTexts(
@@ -134,6 +135,7 @@ class RunOutDebug
   rclcpp::Publisher<PointCloud2>::SharedPtr pub_debug_pointcloud_;
   std::vector<geometry_msgs::msg::Point> collision_points_;
   std::vector<geometry_msgs::msg::Point> nearest_collision_point_;
+  std::vector<geometry_msgs::msg::Point> ego_cut_line_;
   std::vector<geometry_msgs::msg::Pose> stop_pose_;
   std::vector<std::vector<geometry_msgs::msg::Point>> predicted_vehicle_polygons_;
   std::vector<std::vector<geometry_msgs::msg::Point>> predicted_obstacle_polygons_;

diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp
@@ -58,6 +58,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     auto & p = planner_param_.run_out;
     p.detection_method = getOrDeclareParameter<std::string>(node, ns + ".detection_method");
     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.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");
@@ -66,6 +67,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node)
     p.detection_distance = getOrDeclareParameter<double>(node, ns + ".detection_distance");
     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");
   }
 
   {