feat(out_of_lane): add option to use stop lines defined in the vector map (#9584)

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md

@@ -91,6 +91,10 @@ Whether it is decided to slow down or stop is determined by the distance between
 If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used.
 If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used.
 
+In addition, if parameter `action.use_map_stop_lines` is set to `true`,
+then the stop point may be moved to the earliest stop line preceding the stop point where ego can comfortably stop.
+Stop lines are defined in the vector map and must be attached to one of the lanelet followed by the ego trajectory.
+
 ![avoid_collision](./docs/ttcs_avoid.png)
 
 ### About stability of the stop/slowdown pose
@@ -126,6 +130,7 @@ Otherwise, the stop or slowdown pose will only be discarded after no out of lane
 
 | Parameter /action              | Type   | Description                                                           |
 | ------------------------------ | ------ | --------------------------------------------------------------------- |
+| `use_map_stop_lines`           | bool   | [-] if true, try to stop at stop lines defined in the vector map      |
 | `precision`                    | double | [m] precision when inserting a stop pose in the trajectory            |
 | `longitudinal_distance_buffer` | double | [m] safety distance buffer to keep in front of the ego vehicle        |
 | `lateral_distance_buffer`      | double | [m] safety distance buffer to keep on the side of the ego vehicle     |

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/config/out_of_lane.param.yaml

@@ -18,6 +18,7 @@
         ignore_behind_ego: false # if true, objects behind the ego vehicle are ignored
 
       action:  # action to insert in the trajectory if an object causes a conflict at an overlap
+        use_map_stop_lines: true  # if true, try to stop at stop lines defined in the vector map
         precision: 0.1  # [m] precision when inserting a stop pose in the trajectory
         longitudinal_distance_buffer: 1.5  # [m] safety distance buffer to keep in front of the ego vehicle
         lateral_distance_buffer: 1.0  # [m] safety distance buffer to keep on the side of the ego vehicle

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp

@@ -84,48 +84,71 @@ std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
   return std::nullopt;
 }
 
-std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
-  const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params)
+std::optional<geometry_msgs::msg::Pose> calculate_last_in_lane_pose(
+  const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, PlannerParam params)
 {
-  const auto point_to_avoid_it = std::find_if(
-    out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(),
-    [&](const auto & p) { return p.to_avoid; });
-  if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) {
-    return std::nullopt;
-  }
+  std::optional<geometry_msgs::msg::Pose> last_in_lane_pose;
+
+  const auto outside_idx = point_to_avoid.trajectory_index;
+  const auto outside_arc_length =
+    motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, outside_idx);
+
   const auto raw_footprint = make_base_footprint(params, true);  // ignore extra footprint offsets
   const auto base_footprint = make_base_footprint(params);
   params.extra_front_offset += params.lon_dist_buffer;
   params.extra_right_offset += params.lat_dist_buffer;
   params.extra_left_offset += params.lat_dist_buffer;
   const auto expanded_footprint = make_base_footprint(params);  // with added distance buffers
   lanelet::BasicPolygons2d polygons_to_avoid;
-  for (const auto & ll : point_to_avoid_it->overlapped_lanelets) {
+  for (const auto & ll : point_to_avoid.overlapped_lanelets) {
     polygons_to_avoid.push_back(ll.polygon2d().basicPolygon());
   }
-  // points are ordered by trajectory index so the first one has the smallest index and arc length
-  const auto first_outside_idx = out_of_lane_data.outside_points.front().trajectory_index;
-  const auto first_outside_arc_length =
-    motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0UL, first_outside_idx);
-
-  std::optional<geometry_msgs::msg::Pose> slowdown_point;
   // search for the first slowdown decision for which a stop point can be inserted
   // we first try to use the expanded footprint (distance buffers + extra footprint offsets)
-  for (const auto & footprint : {expanded_footprint, base_footprint, raw_footprint}) {
-    slowdown_point = calculate_last_avoiding_pose(
-      ego_data.trajectory_points, footprint, polygons_to_avoid, ego_data.min_stop_arc_length,
-      first_outside_arc_length, params.precision);
-    if (slowdown_point) {
+  // then we use the base footprint (with distance buffers)
+  // finally, we use the raw footprint
+  for (const auto & ego_footprint : {expanded_footprint, base_footprint, raw_footprint}) {
+    last_in_lane_pose = calculate_last_avoiding_pose(
+      ego_data.trajectory_points, ego_footprint, polygons_to_avoid, ego_data.min_stop_arc_length,
+      outside_arc_length, params.precision);
+    if (last_in_lane_pose) {
       break;
     }
   }
   // fallback to simply stopping ahead of the collision to avoid (regardless of being out of lane or
   // not)
-  if (!slowdown_point) {
-    slowdown_point = calculate_pose_ahead_of_collision(
-      ego_data, *point_to_avoid_it, expanded_footprint, params.precision);
+  if (!last_in_lane_pose) {
+    last_in_lane_pose = calculate_pose_ahead_of_collision(
+      ego_data, point_to_avoid, expanded_footprint, params.precision);
+  }
+  return last_in_lane_pose;
+}
+
+std::optional<geometry_msgs::msg::Pose> calculate_slowdown_pose(
+  const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, const PlannerParam & params)
+{
+  // points are ordered by trajectory index so the first one has the smallest index and arc length
+  const auto point_to_avoid_it = std::find_if(
+    out_of_lane_data.outside_points.cbegin(), out_of_lane_data.outside_points.cend(),
+    [&](const auto & p) { return p.to_avoid; });
+  if (point_to_avoid_it == out_of_lane_data.outside_points.cend()) {
+    return std::nullopt;
+  }
+  auto slowdown_pose = calculate_last_in_lane_pose(ego_data, *point_to_avoid_it, params);
+  if (slowdown_pose && params.use_map_stop_lines) {
+    // try to use a map stop line ahead of the stop pose
+    auto stop_arc_length =
+      motion_utils::calcSignedArcLength(ego_data.trajectory_points, 0LU, slowdown_pose->position);
+    for (const auto & stop_point : ego_data.map_stop_points) {
+      if (
+        stop_point.ego_trajectory_arc_length < stop_arc_length &&
+        stop_point.ego_trajectory_arc_length >= ego_data.min_stop_arc_length) {
+        slowdown_pose = stop_point.ego_stop_pose;
+        stop_arc_length = stop_point.ego_trajectory_arc_length;
+      }
+    }
   }
-  return slowdown_point;
+  return slowdown_pose;
 }
 
 void calculate_min_stop_and_slowdown_distances(

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp

@@ -26,17 +26,6 @@
 
 namespace autoware::motion_velocity_planner::out_of_lane
 {
-/// @brief calculate the last pose along the trajectory where ego does not go out of lane
-/// @param [in] ego_data ego data
-/// @param [in] footprint the ego footprint
-/// @param [in] min_arc_length minimum arc length for the search
-/// @param [in] max_arc_length maximum arc length for the search
-/// @param [in] precision [m] search precision
-/// @return the last pose that is not out of lane (if found)
-std::optional<geometry_msgs::msg::Pose> calculate_last_in_lane_pose(
-  const EgoData & ego_data, const autoware_utils::Polygon2d & footprint,
-  const double min_arc_length, const double max_arc_length, const double precision);
-
 /// @brief calculate the slowdown pose just ahead of a point to avoid
 /// @param ego_data ego data (trajectory, velocity, etc)
 /// @param point_to_avoid the point to avoid
@@ -47,13 +36,22 @@ std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
   const EgoData & ego_data, const OutOfLanePoint & point_to_avoid,
   const autoware_utils::Polygon2d & footprint, const double precision);
 
+/// @brief calculate the last pose staying inside the lane and before the given point
+/// @details if no pose staying inside the lane can be found, return a pose before the given point
+/// @param [in] ego_data ego data (trajectory)
+/// @param [in] point_to_avoid the out of lane point to avoid
+/// @param [in] params parameters (ego footprint offsets, precision)
+/// @return optional slowdown pose
+std::optional<geometry_msgs::msg::Pose> calculate_last_in_lane_pose(
+  const EgoData & ego_data, const OutOfLanePoint & point_to_avoid, PlannerParam params);
+
 /// @brief calculate the slowdown point to insert in the trajectory
 /// @param ego_data ego data (trajectory, velocity, etc)
 /// @param out_of_lane_data data about out of lane areas
 /// @param params parameters
-/// @return optional slowdown point to insert in the trajectory
-std::optional<geometry_msgs::msg::Pose> calculate_slowdown_point(
-  const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, PlannerParam params);
+/// @return optional slowdown pose to insert in the trajectory
+std::optional<geometry_msgs::msg::Pose> calculate_slowdown_pose(
+  const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, const PlannerParam & params);
 
 /// @brief calculate the minimum stop and slowdown distances of ego
 /// @param [inout] ego_data ego data where minimum stop and slowdown distances are set

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp

@@ -123,7 +123,7 @@ size_t add_stop_line_markers(
 {
   auto debug_marker = get_base_marker();
   debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
-  debug_marker.ns = "stop_lines";
+  debug_marker.ns = "object_stop_lines";
   const auto & add_lanelets_markers = [&](const auto & lanelets) {
     for (const auto & ll : lanelets) {
       debug_marker.points.clear();
@@ -155,6 +155,31 @@ size_t add_stop_line_markers(
   return max_id;
 }
 
+void add_stop_line_markers(
+  visualization_msgs::msg::MarkerArray & debug_marker_array,
+  const std::vector<StopPoint> & map_stop_points, const double z)
+{
+  auto debug_marker = get_base_marker();
+  debug_marker.type = visualization_msgs::msg::Marker::LINE_LIST;
+  debug_marker.ns = "ego_stop_lines";
+  debug_marker.scale = autoware_utils::create_marker_scale(0.2, 0.2, 0.2);
+  debug_marker.color = autoware_utils::create_marker_color(0.5, 0.0, 0.7, 1.0);
+  geometry_msgs::msg::Point p1;
+  geometry_msgs::msg::Point p2;
+  p1.z = p2.z = z;
+  for (const auto & stop_point : map_stop_points) {
+    for (auto i = 0UL; i + 1 < stop_point.stop_line.size(); ++i) {
+      p1.x = stop_point.stop_line[i].x();
+      p1.y = stop_point.stop_line[i].y();
+      p2.x = stop_point.stop_line[i + 1].x();
+      p2.y = stop_point.stop_line[i + 1].y();
+      debug_marker.points.push_back(p1);
+      debug_marker.points.push_back(p2);
+    }
+  }
+  debug_marker_array.markers.push_back(debug_marker);
+}
+
 void add_out_lanelets(
   visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
   const lanelet::ConstLanelets & out_lanelets)
@@ -222,11 +247,11 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(
   const EgoData & ego_data, const OutOfLaneData & out_of_lane_data,
   const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data)
 {
-  const auto z = ego_data.pose.position.z;
+  const auto z = ego_data.pose.position.z + 0.5;
   visualization_msgs::msg::MarkerArray debug_marker_array;
 
   auto base_marker = get_base_marker();
-  base_marker.pose.position.z = z + 0.5;
+  base_marker.pose.position.z = z;
   base_marker.ns = "footprints";
   base_marker.color = autoware_utils::create_marker_color(1.0, 1.0, 1.0, 1.0);
   // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation
@@ -245,6 +270,8 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array(
   debug_data.prev_stop_line = add_stop_line_markers(
     debug_marker_array, ego_data.stop_lines_rtree, z, debug_data.prev_stop_line);
 
+  add_stop_line_markers(debug_marker_array, ego_data.map_stop_points, z);
+
   return debug_marker_array;
 }