feat(behavior_velocity_run_out_module): exclude obstacle crossing ego back line

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 behind ego: if a dynamic obstacle's predicted path intersects this lane, 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
+      egos_cut_line_length: 3.0   # The width of the ego's cut lane
 
       # Parameter to create abstracted dynamic obstacles
       dynamic_obstacle:

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::pushEgoCutLane(const std::vector<geometry_msgs::msg::Point> & lane)
+{
+  for (const auto & point : lane) {
+    const auto point_with_height = createPoint(point.x, point.y, height_);
+    ego_back_lane_.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_back_lane_.clear();
 }
 
 visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray()
@@ -265,6 +274,16 @@ visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray
       &msg);
   }
 
+  if (!ego_back_lane_.empty()) {
+    auto marker = createDefaultMarker(
+      "map", current_time, "ego_back_lane", 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_back_lane_) {
+      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,

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 pushEgoCutLane(const std::vector<geometry_msgs::msg::Point> & lane);
   void pushDetectionAreaPolygons(const Polygon2d & debug_polygon);
   void pushMandatoryDetectionAreaPolygons(const Polygon2d & debug_polygon);
   void pushTravelTimeTexts(
@@ -134,12 +135,14 @@ 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_back_lane_;
   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_;
   std::vector<std::vector<geometry_msgs::msg::Point>> collision_obstacle_polygons_;
   std::vector<std::vector<geometry_msgs::msg::Point>> detection_area_polygons_;
   std::vector<std::vector<geometry_msgs::msg::Point>> mandatory_detection_area_polygons_;
+
   std::vector<TextWithPosition> travel_time_texts_;
   DebugValues debug_values_;
   AccelReason accel_reason_;

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.egos_cut_line_length = getOrDeclareParameter<double>(node, ns + ".egos_cut_line_length");
   }
 
   {

planning/behavior_velocity_run_out_module/src/scene.cpp

@@ -102,9 +102,18 @@ bool RunOutModule::modifyPathVelocity(
   const auto dynamic_obstacles = dynamic_obstacle_creator_->createDynamicObstacles();
   debug_ptr_->setDebugValues(DebugValues::TYPE::NUM_OBSTACLES, dynamic_obstacles.size());
 
-  // extract obstacles using lanelet information
-  const auto partition_excluded_obstacles =
-    excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose);
+  const auto filtered_obstacles = std::invoke([&]() {
+    // extract obstacles using lanelet information
+    const auto partition_excluded_obstacles =
+      excludeObstaclesOutSideOfPartition(dynamic_obstacles, extended_smoothed_path, current_pose);
+
+    // extract obstacles that cross the ego's back lane
+    if (!planner_param_.run_out.use_ego_cut_line) return partition_excluded_obstacles;
+
+    const auto ego_back_line_excluded_obstacles =
+      excludeObstaclesCrossingEgoBackLine(partition_excluded_obstacles, current_pose);
+    return ego_back_line_excluded_obstacles;
+  });
 
   // record time for obstacle creation
   const auto t_obstacle_creation = std::chrono::system_clock::now();
@@ -122,7 +131,7 @@ bool RunOutModule::modifyPathVelocity(
                                         planner_data_->route_handler_->getOverallGraphPtr())
                                     : std::vector<std::pair<int64_t, lanelet::ConstLanelet>>();
   const auto dynamic_obstacle =
-    detectCollision(partition_excluded_obstacles, extended_smoothed_path, crosswalk_lanelets);
+    detectCollision(filtered_obstacles, extended_smoothed_path, crosswalk_lanelets);
 
   // record time for collision check
   const auto t_collision_check = std::chrono::system_clock::now();
@@ -147,8 +156,7 @@ bool RunOutModule::modifyPathVelocity(
     applyMaxJerkLimit(current_pose, current_vel, current_acc, *path);
   }
 
-  publishDebugValue(
-    extended_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose);
+  publishDebugValue(extended_smoothed_path, filtered_obstacles, dynamic_obstacle, current_pose);
 
   // record time for collision check
   const auto t_path_planning = std::chrono::system_clock::now();
@@ -810,6 +818,24 @@ void RunOutModule::applyMaxJerkLimit(
   run_out_utils::insertPathVelocityFromIndex(stop_point_idx.value(), jerk_limited_vel, path.points);
 }
 
+std::vector<DynamicObstacle> RunOutModule::excludeObstaclesCrossingEgoBackLine(
+  const std::vector<DynamicObstacle> & dynamic_obstacles,
+  const geometry_msgs::msg::Pose & current_pose) const
+{
+  std::vector<DynamicObstacle> extracted_obstacles;
+  std::vector<geometry_msgs::msg::Point> ego_cut_lane;
+  const double ego_cut_lane_half_width = planner_param_.run_out.egos_cut_line_length / 2.0;
+  std::for_each(dynamic_obstacles.begin(), dynamic_obstacles.end(), [&](const auto & o) {
+    const auto predicted_path = run_out_utils::getHighestConfidencePath(o.predicted_paths);
+    if (!run_out_utils::pathIntersectsEgoCutLine(
+          predicted_path, current_pose, ego_cut_lane_half_width, ego_cut_lane)) {
+      extracted_obstacles.push_back(o);
+    }
+  });
+  debug_ptr_->pushEgoCutLane(ego_cut_lane);
+  return extracted_obstacles;
+}
+
 std::vector<DynamicObstacle> RunOutModule::excludeObstaclesOutSideOfPartition(
   const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
   const geometry_msgs::msg::Pose & current_pose) const

planning/behavior_velocity_run_out_module/src/scene.hpp

@@ -136,6 +136,10 @@ class RunOutModule : public SceneModuleInterface
     const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
     PathWithLaneId & path) const;
 
+  std::vector<DynamicObstacle> excludeObstaclesCrossingEgoBackLine(
+    const std::vector<DynamicObstacle> & dynamic_obstacles,
+    const geometry_msgs::msg::Pose & current_pose) const;
+
   std::vector<DynamicObstacle> excludeObstaclesOutSideOfPartition(
     const std::vector<DynamicObstacle> & dynamic_obstacles, const PathWithLaneId & path,
     const geometry_msgs::msg::Pose & current_pose) const;

planning/behavior_velocity_run_out_module/src/utils.cpp

@@ -176,6 +176,28 @@ std::optional<size_t> findFirstStopPointIdx(PathPointsWithLaneId & path_points)
   return {};
 }
 
+bool pathIntersectsEgoCutLine(
+  const std::vector<geometry_msgs::msg::Pose> & path, const geometry_msgs::msg::Pose & ego_pose,
+  const double half_line_length, std::vector<geometry_msgs::msg::Point> & ego_cut_lane)
+{
+  if (path.size() < 2) return false;
+  const auto p1 =
+    tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, half_line_length, 0.0).position;
+  const auto p2 =
+    tier4_autoware_utils::calcOffsetPose(ego_pose, 0.0, -half_line_length, 0.0).position;
+  ego_cut_lane = {p1, p2};
+
+  for (size_t i = 1; i < path.size(); ++i) {
+    const auto & p3 = path.at(i).position;
+    const auto & p4 = path.at(i - 1).position;
+    const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4);
+    if (intersection.has_value()) {
+      return true;
+    }
+  }
+  return false;
+}
+
 LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly)
 {
   LineString2d line_string;

planning/behavior_velocity_run_out_module/src/utils.hpp

@@ -19,6 +19,14 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include "tier4_autoware_utils/geometry/geometry.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>
@@ -56,9 +64,11 @@ struct RunOutParam
   bool use_partition_lanelet;
   bool suppress_on_crosswalk;
   bool specify_decel_jerk;
+  bool use_ego_cut_line;
   double stop_margin;
   double passing_margin;
   double deceleration_jerk;
+  double egos_cut_line_length;
   float detection_distance;
   float detection_span;
   float min_vel_ego_kmph;
@@ -194,6 +204,27 @@ struct DynamicObstacleData
 
 Polygon2d createBoostPolyFromMsg(const std::vector<geometry_msgs::msg::Point> & input_poly);
 
+inline geometry_msgs::msg::Vector3 rpyFromQuat(const geometry_msgs::msg::Quaternion & q)
+{
+  tf2::Quaternion quat(q.x, q.y, q.z, q.w);
+  tf2::Matrix3x3 mat(quat);
+  double roll, pitch, yaw;
+  mat.getRPY(roll, pitch, yaw);
+  geometry_msgs::msg::Vector3 rpy;
+  rpy.x = roll;
+  rpy.y = pitch;
+  rpy.z = yaw;
+  return rpy;
+}
+
+bool pathIntersectsEgoCutLine(
+  const std::vector<geometry_msgs::msg::Pose> & path, const geometry_msgs::msg::Pose & ego_pose,
+  const double half_line_length, std::vector<geometry_msgs::msg::Point> & ego_cut_lane);
+
+// LineString2d createLineStringFromPath(const PredictedPath & path);
+
+// LineString2d createEgoCutLine(const geometry_msgs::msg::Pose & ego_pose, const double offset);
+
 std::uint8_t getHighestProbLabel(const std::vector<ObjectClassification> & classification);
 
 std::vector<geometry_msgs::msg::Pose> getHighestConfidencePath(