perf(drivable area expansion): use faster lateral offset and nearest …

…line calculations (autowarefoundation#960)

* [DEBUG] Launch planner with konsole + gdb

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

* [TMP] debug svg

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

* Improve performance of the in-loop calcLateralOffset

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

* Store uncrossable lines as segments in a rtree to improve performance

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

* [DEBUG] show uncrossable segments and the neirest bound point

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

* Use custom calcLateralOffset to improve performance

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

* Revert "[DEBUG] show uncrossable segments and the neirest bound point"

This reverts commit f1085cb.

* Revert "[TMP] debug svg"

This reverts commit 37ba0a7.

* Fix lateral distance calculation

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

* Revert "[DEBUG] Launch planner with konsole + gdb"

This reverts commit f4c9953.

---------

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

planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp

@@ -68,14 +68,13 @@ void apply_bound_change_rate_limit(
 /// @brief calculate the maximum distance by which a bound can be expanded
 /// @param [in] path_poses input path
 /// @param [in] bound bound points
-/// @param [in] uncrossable_lines lines that limit the bound expansion
+/// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree
 /// @param [in] uncrossable_polygons polygons that limit the bound expansion
 /// @param [in] params parameters with the buffer distance to keep with lines,
 /// and the static maximum expansion distance
 std::vector<double> calculate_maximum_distance(
   const std::vector<Pose> & path_poses, const std::vector<Point> bound,
-  const std::vector<LineString2d> & uncrossable_lines,
-  const std::vector<Polygon2d> & uncrossable_polygons,
+  const SegmentRtree & uncrossable_lines, const std::vector<Polygon2d> & uncrossable_polygons,
   const DrivableAreaExpansionParameters & params);
 
 /// @brief expand a bound by the given lateral distances away from the path

planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp

@@ -25,12 +25,12 @@
 
 namespace drivable_area_expansion
 {
-/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego
+/// @brief Extract uncrossable segments from the lanelet map that are in range of ego
 /// @param[in] lanelet_map lanelet map
 /// @param[in] ego_point point of the current ego position
 /// @param[in] params parameters with linestring types that cannot be crossed and maximum range
-/// @return the uncrossable linestrings
-MultiLineString2d extract_uncrossable_lines(
+/// @return the uncrossable segments stored in a rtree
+SegmentRtree extract_uncrossable_segments(
   const lanelet::LaneletMap & lanelet_map, const Point & ego_point,
   const DrivableAreaExpansionParameters & params);
 

planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp

@@ -22,6 +22,8 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/point.hpp>
 
+#include <boost/geometry/index/rtree.hpp>
+
 namespace drivable_area_expansion
 {
 using autoware_auto_perception_msgs::msg::PredictedObjects;
@@ -39,6 +41,8 @@ using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 using tier4_autoware_utils::Segment2d;
 
+typedef boost::geometry::index::rtree<Segment2d, boost::geometry::index::rstar<16>> SegmentRtree;
+
 struct PointDistance
 {
   Point2d point;

planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp

@@ -34,12 +34,10 @@ namespace drivable_area_expansion
 
 namespace
 {
-
 Point2d convert_point(const Point & p)
 {
   return Point2d{p.x, p.y};
 }
-
 }  // namespace
 
 void reuse_previous_poses(
@@ -59,11 +57,17 @@ void reuse_previous_poses(
     const auto deviation =
       motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position);
     if (first_idx && deviation < params.max_reuse_deviation) {
+      LineString2d path_ls;
+      for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position));
       for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) {
-        if (
-          motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) >
-          params.max_reuse_deviation)
-          break;
+        double lateral_offset = std::numeric_limits<double>::max();
+        for (auto segment_idx = 0LU; segment_idx + 1 < path_ls.size(); ++segment_idx) {
+          const auto projection = point_to_line_projection(
+            convert_point(prev_poses[idx].position), path_ls[segment_idx],
+            path_ls[segment_idx + 1]);
+          lateral_offset = std::min(projection.distance, lateral_offset);
+        }
+        if (lateral_offset > params.max_reuse_deviation) break;
         cropped_poses.push_back(prev_poses[idx]);
         cropped_curvatures.push_back(prev_curvatures[idx]);
       }
@@ -170,8 +174,7 @@ void apply_bound_change_rate_limit(
 
 std::vector<double> calculate_maximum_distance(
   const std::vector<Pose> & path_poses, const std::vector<Point> bound,
-  const std::vector<LineString2d> & uncrossable_lines,
-  const std::vector<Polygon2d> & uncrossable_polygons,
+  const SegmentRtree & uncrossable_segments, const std::vector<Polygon2d> & uncrossable_polygons,
   const DrivableAreaExpansionParameters & params)
 {
   // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?)
@@ -181,9 +184,13 @@ std::vector<double> calculate_maximum_distance(
   for (const auto & p : bound) bound_ls.push_back(convert_point(p));
   for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position));
   for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) {
-    const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]};
-    for (const auto & uncrossable_line : uncrossable_lines) {
-      const auto bound_to_line_dist = boost::geometry::distance(segment_ls, uncrossable_line);
+    const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]};
+    std::vector<Segment2d> query_result;
+    boost::geometry::index::query(
+      uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1),
+      std::back_inserter(query_result));
+    if (!query_result.empty()) {
+      const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front());
       const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist);
       maximum_distances[i] = std::min(maximum_distances[i], dist_limit);
       maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit);
@@ -265,7 +272,7 @@ void expand_drivable_area(
   // crop first/last non deviating path_poses
   const auto & params = planner_data->drivable_area_expansion_parameters;
   const auto & route_handler = *planner_data->route_handler;
-  const auto uncrossable_lines = extract_uncrossable_lines(
+  const auto uncrossable_segments = extract_uncrossable_segments(
     *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params);
   const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params);
   const auto preprocessing_ms = stop_watch.toc("preprocessing");
@@ -293,9 +300,9 @@ void expand_drivable_area(
 
   stop_watch.tic("max_dist");
   const auto max_left_expansions = calculate_maximum_distance(
-    path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons, params);
+    path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params);
   const auto max_right_expansions = calculate_maximum_distance(
-    path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params);
+    path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params);
   for (auto i = 0LU; i < left_expansions.size(); ++i)
     left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]);
   for (auto i = 0LU; i < right_expansions.size(); ++i)

planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp

@@ -24,22 +24,26 @@
 
 namespace drivable_area_expansion
 {
-MultiLineString2d extract_uncrossable_lines(
+SegmentRtree extract_uncrossable_segments(
   const lanelet::LaneletMap & lanelet_map, const Point & ego_point,
   const DrivableAreaExpansionParameters & params)
 {
-  MultiLineString2d uncrossable_lines_in_range;
+  SegmentRtree uncrossable_segments_in_range;
   LineString2d line;
   const auto ego_p = Point2d{ego_point.x, ego_point.y};
   for (const auto & ls : lanelet_map.lineStringLayer) {
     if (has_types(ls, params.avoid_linestring_types)) {
       line.clear();
       for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()});
-      if (boost::geometry::distance(line, ego_p) < params.max_path_arc_length)
-        uncrossable_lines_in_range.push_back(line);
+      for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) {
+        Segment2d segment = {line[segment_idx], line[segment_idx + 1]};
+        if (boost::geometry::distance(segment, ego_p) < params.max_path_arc_length) {
+          uncrossable_segments_in_range.insert(segment);
+        }
+      }
     }
   }
-  return uncrossable_lines_in_range;
+  return uncrossable_segments_in_range;
 }
 
 bool has_types(const lanelet::ConstLineString3d & ls, const std::vector<std::string> & types)