perf(out_of_lane): use intersection with other lanes instead of difference with ego lane (#8870)

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

@@ -27,18 +27,18 @@ The length of the trajectory used for generating the footprints is limited by th
 
 ![ego_footprints](./docs/footprints.png)
 
-### 2. Ego lanelets
+### 2. Other lanelets
 
-In the second step, we calculate the lanelets followed by the ego trajectory.
-We select all lanelets crossed by the trajectory linestring (sequence of trajectory points), as well as their preceding lanelets.
+In the second step, we calculate the lanelets where collisions should be avoided.
+We consider all lanelets around the ego vehicle that are not crossed by the trajectory linestring (sequence of trajectory points) or their preceding lanelets.
 
-![ego_lane](./docs/ego_lane.png)
+![other_lanes](./docs/other_lanes.png)
 
-In the debug visualization the combination of all ego lanelets is shown as a blue polygon.
+In the debug visualization, these other lanelets are shown as blue polygons.
 
 ### 3. Out of lane areas
 
-Next, for each trajectory point, we create the corresponding out of lane areas by subtracting the ego lanelets (from step 2) from the trajectory point footprint (from step 1).
+Next, for each trajectory point, we create the corresponding out of lane areas by intersection the other lanelets (from step 2) with the trajectory point footprint (from step 1).
 Each area is associated with the lanelets overlapped by the area and with the corresponding ego trajectory point.
 
 ![out_of_lane_areas](./docs/out_of_lane_areas.png)

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png

@@ -21,8 +21,7 @@
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/boost_geometry.hpp>
 
-#include <autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp>
-#include <geometry_msgs/msg/detail/pose__struct.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
 #include <boost/geometry/algorithms/disjoint.hpp>
@@ -78,7 +77,7 @@ std::optional<geometry_msgs::msg::Pose> calculate_pose_ahead_of_collision(
     const auto interpolated_pose =
       motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l);
     const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose);
-    if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) {
+    if (!boost::geometry::disjoint(interpolated_footprint, point_to_avoid.out_overlaps)) {
       return interpolated_pose;
     }
   }

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png

@@ -21,11 +21,12 @@
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 
-#include <geometry_msgs/msg/detail/point__struct.hpp>
-#include <geometry_msgs/msg/detail/pose__struct.hpp>
-#include <std_msgs/msg/detail/color_rgba__struct.hpp>
-#include <visualization_msgs/msg/detail/marker__struct.hpp>
+#include <autoware_planning_msgs/msg/trajectory_point.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/color_rgba.hpp>
 #include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 
 #include <boost/geometry/algorithms/centroid.hpp>
 #include <boost/geometry/algorithms/for_each.hpp>
@@ -36,6 +37,7 @@
 #include <lanelet2_core/primitives/Polygon.h>
 
 #include <string>
+#include <vector>
 
 namespace autoware::motion_velocity_planner::out_of_lane::debug
 {
@@ -151,63 +153,88 @@ size_t add_stop_line_markers(
   }
   return max_id;
 }
-}  // namespace
 
-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)
+void add_out_lanelets(
+  visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
+  const lanelet::ConstLanelets & out_lanelets)
 {
-  const auto z = ego_data.pose.position.z;
-  visualization_msgs::msg::MarkerArray debug_marker_array;
-
-  auto base_marker = get_base_marker();
-  base_marker.pose.position.z = z + 0.5;
-  base_marker.ns = "footprints";
-  base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0);
-  // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation
-  // disabled to prevent performance issues when publishing the debug markers
-  // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints);
-
   lanelet::BasicPolygons2d drivable_lane_polygons;
-  for (const auto & poly : ego_data.drivable_lane_polygons) {
-    drivable_lane_polygons.push_back(poly.outer);
+  for (const auto & ll : out_lanelets) {
+    drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon());
   }
-  base_marker.ns = "ego_lane";
+  base_marker.ns = "out_lanelets";
   base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0);
-  add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons);
+  add_polygons_markers(marker_array, base_marker, drivable_lane_polygons);
+}
 
-  lanelet::BasicPolygons2d out_of_lane_areas;
-  for (const auto & p : out_of_lane_data.outside_points) {
-    out_of_lane_areas.push_back(p.outside_ring);
+void add_out_of_lane_overlaps(
+  visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
+  const std::vector<OutOfLanePoint> & outside_points,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory)
+{
+  lanelet::BasicPolygons2d out_of_lane_overlaps;
+  lanelet::BasicPolygon2d out_of_lane_overlap;
+  for (const auto & p : outside_points) {
+    for (const auto & overlap : p.out_overlaps) {
+      boost::geometry::convert(overlap, out_of_lane_overlap);
+      out_of_lane_overlaps.push_back(out_of_lane_overlap);
+    }
   }
   base_marker.ns = "out_of_lane_areas";
   base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0);
-  add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas);
-  for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) {
-    const auto & a = out_of_lane_data.outside_points[i];
-    debug_marker_array.markers.back().points.push_back(
-      ego_data.trajectory_points[a.trajectory_index].pose.position);
-    const auto centroid = boost::geometry::return_centroid<lanelet::BasicPoint2d>(a.outside_ring);
-    debug_marker_array.markers.back().points.push_back(
-      geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y()));
+  add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps);
+  for (const auto & p : outside_points) {
+    for (const auto & a : p.out_overlaps) {
+      marker_array.markers.back().points.push_back(trajectory[p.trajectory_index].pose.position);
+      const auto centroid = boost::geometry::return_centroid<lanelet::BasicPoint2d>(a);
+      marker_array.markers.back().points.push_back(
+        geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y()));
+    }
   }
-
+}
+void add_predicted_paths(
+  visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker,
+  const autoware_perception_msgs::msg::PredictedObjects & objects,
+  const geometry_msgs::msg::Pose & ego_pose)
+{
+  base_marker.ns = "objects";
+  base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0);
   lanelet::BasicPolygons2d object_polygons;
+  constexpr double max_draw_distance = 50.0;
   for (const auto & o : objects.objects) {
     for (const auto & path : o.kinematics.predicted_paths) {
       for (const auto & pose : path.path) {
         // limit the draw distance to improve performance
-        if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) {
+        if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) {
           const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer();
           lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end());
           object_polygons.push_back(ll_poly);
         }
       }
     }
   }
-  base_marker.ns = "objects";
-  base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0);
-  add_polygons_markers(debug_marker_array, base_marker, object_polygons);
+  add_polygons_markers(marker_array, base_marker, object_polygons);
+}
+}  // namespace
+
+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;
+  visualization_msgs::msg::MarkerArray debug_marker_array;
+
+  auto base_marker = get_base_marker();
+  base_marker.pose.position.z = z + 0.5;
+  base_marker.ns = "footprints";
+  base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0);
+  // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation
+  // disabled to prevent performance issues when publishing the debug markers
+  // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints);
+  add_out_lanelets(debug_marker_array, base_marker, ego_data.out_lanelets);
+  add_out_of_lane_overlaps(
+    debug_marker_array, base_marker, out_of_lane_data.outside_points, ego_data.trajectory_points);
+  add_predicted_paths(debug_marker_array, base_marker, objects, ego_data.pose);
 
   add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z);
 

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.cpp

@@ -18,6 +18,8 @@
 
 #include <geometry_msgs/msg/pose.hpp>
 
+#include <boost/geometry/algorithms/correct.hpp>
+
 #include <lanelet2_core/geometry/Polygon.h>
 #include <tf2/utils.h>
 
@@ -37,6 +39,7 @@ universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool
     {p.front_offset + front_offset, p.right_offset - right_offset},
     {p.rear_offset - rear_offset, p.right_offset - right_offset},
     {p.rear_offset - rear_offset, p.left_offset + left_offset}};
+  boost::geometry::correct(base_footprint);
   return base_footprint;
 }
 

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp

@@ -16,16 +16,24 @@
 
 #include "types.hpp"
 
+#include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <boost/geometry/algorithms/buffer.hpp>
 #include <boost/geometry/algorithms/disjoint.hpp>
+#include <boost/geometry/algorithms/envelope.hpp>
 #include <boost/geometry/algorithms/union.hpp>
+#include <boost/geometry/strategies/cartesian/buffer_point_square.hpp>
 
 #include <lanelet2_core/Forward.h>
+#include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/primitives/BoundingBox.h>
 #include <lanelet2_routing/RoutingGraph.h>
 
 #include <algorithm>
+#include <vector>
 
 namespace autoware::motion_velocity_planner::out_of_lane
 {
@@ -77,15 +85,13 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets(
 }
 
 lanelet::ConstLanelets calculate_trajectory_lanelets(
-  const EgoData & ego_data, const route_handler::RouteHandler & route_handler)
+  const universe_utils::LineString2d & trajectory_ls,
+  const route_handler::RouteHandler & route_handler)
 {
   const auto lanelet_map_ptr = route_handler.getLaneletMapPtr();
   lanelet::ConstLanelets trajectory_lanelets;
-  lanelet::BasicLineString2d trajectory_ls;
-  for (const auto & p : ego_data.trajectory_points)
-    trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y);
-  const auto candidates =
-    lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls));
+  const auto candidates = lanelet_map_ptr->laneletLayer.search(
+    boost::geometry::return_envelope<lanelet::BoundingBox2d>(trajectory_ls));
   for (const auto & ll : candidates) {
     if (
       is_road_lanelet(ll) &&
@@ -115,51 +121,77 @@ lanelet::ConstLanelets calculate_ignored_lanelets(
   return ignored_lanelets;
 }
 
-void calculate_drivable_lane_polygons(
-  EgoData & ego_data, const route_handler::RouteHandler & route_handler)
+lanelet::ConstLanelets calculate_out_lanelets(
+  const lanelet::LaneletLayer & lanelet_layer,
+  const universe_utils::MultiPolygon2d & trajectory_footprints,
+  const lanelet::ConstLanelets & trajectory_lanelets,
+  const lanelet::ConstLanelets & ignored_lanelets)
 {
-  const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler);
-  const auto ignored_lanelets =
-    out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler);
-  for (const auto & ll : route_lanelets) {
-    out_of_lane::Polygons tmp;
-    boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp);
-    ego_data.drivable_lane_polygons = tmp;
-  }
-  for (const auto & ll : ignored_lanelets) {
-    out_of_lane::Polygons tmp;
-    boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp);
-    ego_data.drivable_lane_polygons = tmp;
+  lanelet::ConstLanelets out_lanelets;
+  const auto candidates = lanelet_layer.search(
+    boost::geometry::return_envelope<lanelet::BoundingBox2d>(trajectory_footprints));
+  for (const auto & lanelet : candidates) {
+    const auto id = lanelet.id();
+    if (
+      contains_lanelet(trajectory_lanelets, id) || contains_lanelet(ignored_lanelets, id) ||
+      !is_road_lanelet(lanelet)) {
+      continue;
+    }
+    if (!boost::geometry::disjoint(trajectory_footprints, lanelet.polygon2d().basicPolygon())) {
+      out_lanelets.push_back(lanelet);
+    }
   }
+  return out_lanelets;
 }
 
-void calculate_overlapped_lanelets(
-  OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler)
+OutLaneletRtree calculate_out_lanelet_rtree(const lanelet::ConstLanelets & lanelets)
 {
-  out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets();
-  const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search(
-    lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring));
-  for (const auto & ll : candidates) {
-    if (
-      is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) &&
-      boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) {
-      out_of_lane_point.overlapped_lanelets.push_back(ll);
-    }
+  std::vector<LaneletNode> nodes;
+  nodes.reserve(lanelets.size());
+  for (auto i = 0UL; i < lanelets.size(); ++i) {
+    nodes.emplace_back(
+      boost::geometry::return_envelope<universe_utils::Box2d>(
+        lanelets[i].polygon2d().basicPolygon()),
+      i);
   }
+  return {nodes.begin(), nodes.end()};
 }
 
-void calculate_overlapped_lanelets(
-  OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler)
+void calculate_out_lanelet_rtree(
+  EgoData & ego_data, const route_handler::RouteHandler & route_handler,
+  const PlannerParam & params)
 {
-  for (auto it = out_of_lane_data.outside_points.begin();
-       it != out_of_lane_data.outside_points.end();) {
-    calculate_overlapped_lanelets(*it, route_handler);
-    if (it->overlapped_lanelets.empty()) {
-      // do not keep out of lane points that do not overlap any lanelet
-      out_of_lane_data.outside_points.erase(it);
-    } else {
-      ++it;
-    }
+  universe_utils::LineString2d trajectory_ls;
+  for (const auto & p : ego_data.trajectory_points) {
+    trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y);
   }
+  // add a point beyond the last trajectory point to account for the ego front offset
+  const auto pose_beyond = universe_utils::calcOffsetPose(
+    ego_data.trajectory_points.back().pose, params.front_offset, 0.0, 0.0, 0.0);
+  trajectory_ls.emplace_back(pose_beyond.position.x, pose_beyond.position.y);
+  const auto trajectory_lanelets = calculate_trajectory_lanelets(trajectory_ls, route_handler);
+  const auto ignored_lanelets = calculate_ignored_lanelets(trajectory_lanelets, route_handler);
+
+  const auto max_ego_footprint_offset = std::max({
+    params.front_offset + params.extra_front_offset,
+    params.left_offset + params.extra_left_offset,
+    params.right_offset + params.extra_right_offset,
+    params.rear_offset + params.extra_rear_offset,
+  });
+  universe_utils::MultiPolygon2d trajectory_footprints;
+  const boost::geometry::strategy::buffer::distance_symmetric<double> distance_strategy(
+    max_ego_footprint_offset);
+  const boost::geometry::strategy::buffer::join_miter join_strategy;
+  const boost::geometry::strategy::buffer::end_flat end_strategy;
+  const boost::geometry::strategy::buffer::point_square circle_strategy;
+  const boost::geometry::strategy::buffer::side_straight side_strategy;
+  boost::geometry::buffer(
+    trajectory_ls, trajectory_footprints, distance_strategy, side_strategy, join_strategy,
+    end_strategy, circle_strategy);
+
+  ego_data.out_lanelets = calculate_out_lanelets(
+    route_handler.getLaneletMapPtr()->laneletLayer, trajectory_footprints, trajectory_lanelets,
+    ignored_lanelets);
+  ego_data.out_lanelets_rtree = calculate_out_lanelet_rtree(ego_data.out_lanelets);
 }
 }  // namespace autoware::motion_velocity_planner::out_of_lane