perf(out_of_lane): use rtree to get stop lines and trajectory lanelets

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -15,20 +15,23 @@
 #include "filter_predicted_objects.hpp"
 
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <traffic_light_utils/traffic_light_utils.hpp>
 
 #include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/index/predicates.hpp>
 
 #include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
 #include <algorithm>
+#include <vector>
 
 namespace autoware::motion_velocity_planner::out_of_lane
 {
 void cut_predicted_path_beyond_line(
   autoware_perception_msgs::msg::PredictedPath & predicted_path,
-  const lanelet::BasicLineString2d & stop_line, const double object_front_overhang)
+  const universe_utils::LineString2d & stop_line, const double object_front_overhang)
 {
   if (predicted_path.path.empty() || stop_line.size() < 2) return;
 
@@ -58,43 +61,43 @@ void cut_predicted_path_beyond_line(
   }
 }
 
-std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
-  const autoware_perception_msgs::msg::PredictedPath & path,
-  const std::shared_ptr<const PlannerData> planner_data)
+std::optional<universe_utils::LineString2d> find_next_stop_line(
+  const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data)
 {
-  lanelet::ConstLanelets lanelets;
-  lanelet::BasicLineString2d query_line;
-  for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y);
-  const auto query_results = lanelet::geometry::findWithin2d(
-    planner_data->route_handler->getLaneletMapPtr()->laneletLayer, query_line);
-  for (const auto & r : query_results) lanelets.push_back(r.second);
-  for (const auto & ll : lanelets) {
-    for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) {
-      const auto traffic_signal_stamped = planner_data->get_traffic_signal(element->id());
-      if (
-        traffic_signal_stamped.has_value() && element->stopLine().has_value() &&
-        traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) {
-        lanelet::BasicLineString2d stop_line;
-        for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y());
-        return stop_line;
+  universe_utils::LineString2d query_path;
+  for (const auto & p : path.path) query_path.emplace_back(p.position.x, p.position.y);
+  std::vector<StopLineNode> query_results;
+  ego_data.stop_lines_rtree.query(
+    boost::geometry::index::intersects(query_path), std::back_inserter(query_results));
+  auto earliest_intersecting_index = query_path.size();
+  std::optional<universe_utils::LineString2d> earliest_stop_line;
+  universe_utils::Segment2d path_segment;
+  for (const auto & [_, stop_line] : query_results) {
+    for (auto index = 0UL; index + 1 < query_path.size(); ++index) {
+      path_segment.first = query_path[index];
+      path_segment.second = query_path[index + 1];
+      if (boost::geometry::intersects(path_segment, stop_line.stop_line)) {
+        bool within_stop_line_lanelet =
+          std::any_of(stop_line.lanelets.begin(), stop_line.lanelets.end(), [&](const auto & ll) {
+            return boost::geometry::within(path_segment.first, ll.polygon2d().basicPolygon());
+          });
+        if (within_stop_line_lanelet) {
+          earliest_intersecting_index = std::min(index, earliest_intersecting_index);
+          earliest_stop_line = stop_line.stop_line;
+        }
       }
     }
   }
-  return std::nullopt;
+  return earliest_stop_line;
 }
 
 void cut_predicted_path_beyond_red_lights(
-  autoware_perception_msgs::msg::PredictedPath & predicted_path,
-  const std::shared_ptr<const PlannerData> planner_data, const double object_front_overhang)
+  autoware_perception_msgs::msg::PredictedPath & predicted_path, const EgoData & ego_data,
+  const double object_front_overhang)
 {
-  const auto stop_line = find_next_stop_line(predicted_path, planner_data);
+  const auto stop_line = find_next_stop_line(predicted_path, ego_data);
   if (stop_line) {
-    // we use a longer stop line to also cut predicted paths that slightly go around the stop line
-    auto longer_stop_line = *stop_line;
-    const auto diff = stop_line->back() - stop_line->front();
-    longer_stop_line.front() -= diff * 0.5;
-    longer_stop_line.back() += diff * 0.5;
-    cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang);
+    cut_predicted_path_beyond_line(predicted_path, *stop_line, object_front_overhang);
   }
 }
 
@@ -141,7 +144,7 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects(
       if (params.objects_cut_predicted_paths_beyond_red_lights)
         for (auto & predicted_path : predicted_paths)
           cut_predicted_path_beyond_red_lights(
-            predicted_path, planner_data, filtered_object.shape.dimensions.x);
+            predicted_path, ego_data, filtered_object.shape.dimensions.x);
       predicted_paths.erase(
         std::remove_if(
           predicted_paths.begin(), predicted_paths.end(),

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp

@@ -30,22 +30,21 @@ namespace autoware::motion_velocity_planner::out_of_lane
 /// @param [in] object_front_overhang extra distance to cut ahead of the stop line
 void cut_predicted_path_beyond_line(
   autoware_perception_msgs::msg::PredictedPath & predicted_path,
-  const lanelet::BasicLineString2d & stop_line, const double object_front_overhang);
+  const universe_utils::LineString2d & stop_line, const double object_front_overhang);
 
 /// @brief find the next red light stop line along the given path
 /// @param [in] path predicted path to check for a stop line
-/// @param [in] planner_data planner data with stop line information
+/// @param [in] ego_data ego data with the stop lines information
 /// @return the first red light stop line found along the path (if any)
-std::optional<const lanelet::BasicLineString2d> find_next_stop_line(
-  const autoware_perception_msgs::msg::PredictedPath & path,
-  const std::shared_ptr<const PlannerData> planner_data);
+std::optional<universe_utils::LineString2d> find_next_stop_line(
+  const autoware_perception_msgs::msg::PredictedPath & path, const EgoData & ego_data);
 
 /// @brief cut predicted path beyond stop lines of red lights
 /// @param [inout] predicted_path predicted path to cut
-/// @param [in] planner_data planner data to get the map and traffic light information
+/// @param [in] ego_data ego data with the stop lines information
 void cut_predicted_path_beyond_red_lights(
-  autoware_perception_msgs::msg::PredictedPath & predicted_path,
-  const std::shared_ptr<const PlannerData> planner_data, const double object_front_overhang);
+  autoware_perception_msgs::msg::PredictedPath & predicted_path, const EgoData & ego_data,
+  const double object_front_overhang);
 
 /// @brief filter predicted objects and their predicted paths
 /// @param [in] planner_data planner data

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp

@@ -16,6 +16,9 @@
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <boost/geometry/algorithms/disjoint.hpp>
+
+#include <lanelet2_core/geometry/BoundingBox.h>
 #include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_routing/RoutingGraph.h>
 
@@ -71,10 +74,12 @@ lanelet::ConstLanelets calculate_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);
-  for (const auto & dist_lanelet :
-       lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, trajectory_ls)) {
-    if (!contains_lanelet(trajectory_lanelets, dist_lanelet.second.id()))
-      trajectory_lanelets.push_back(dist_lanelet.second);
+  const auto candidates =
+    lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls));
+  for (const auto & ll : candidates) {
+    if (!boost::geometry::disjoint(trajectory_ls, ll.polygon2d().basicPolygon())) {
+      trajectory_lanelets.push_back(ll);
+    }
   }
   const auto missing_lanelets =
     get_missing_lane_change_lanelets(trajectory_lanelets, route_handler);

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp

@@ -25,13 +25,19 @@
 
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/motion_velocity_planner_common/planner_data.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 #include <autoware/universe_utils/ros/parameter.hpp>
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware/universe_utils/system/stop_watch.hpp>
+#include <traffic_light_utils/traffic_light_utils.hpp>
 
+#include <boost/geometry/algorithms/envelope.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
 
-#include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Polygon.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
 
 #include <map>
 #include <memory>
@@ -155,6 +161,42 @@ void OutOfLaneModule::update_parameters(const std::vector<rclcpp::Parameter> & p
   updateParam(parameters, ns_ + ".ego.extra_right_offset", pp.extra_right_offset);
 }
 
+void prepare_stop_lines_rtree(
+  out_of_lane::EgoData & ego_data, const PlannerData & planner_data, const double search_distance)
+{
+  std::vector<out_of_lane::StopLineNode> rtree_nodes;
+  const auto bbox = lanelet::BoundingBox2d(
+    lanelet::BasicPoint2d{
+      ego_data.pose.position.x - search_distance, ego_data.pose.position.y - search_distance},
+    lanelet::BasicPoint2d{
+      ego_data.pose.position.x + search_distance, ego_data.pose.position.y + search_distance});
+  out_of_lane::StopLineNode stop_line_node;
+  for (const auto & ll :
+       planner_data.route_handler->getLaneletMapPtr()->laneletLayer.search(bbox)) {
+    for (const auto & element : ll.regulatoryElementsAs<lanelet::TrafficLight>()) {
+      const auto traffic_signal_stamped = planner_data.get_traffic_signal(element->id());
+      if (
+        traffic_signal_stamped.has_value() && element->stopLine().has_value() &&
+        traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) {
+        stop_line_node.second.stop_line.clear();
+        for (const auto & p : element->stopLine()->basicLineString()) {
+          stop_line_node.second.stop_line.emplace_back(p.x(), p.y());
+        }
+        // use a longer stop line to also cut predicted paths that slightly go around the stop line
+        const auto diff =
+          stop_line_node.second.stop_line.back() - stop_line_node.second.stop_line.front();
+        stop_line_node.second.stop_line.front() -= diff * 0.5;
+        stop_line_node.second.stop_line.back() += diff * 0.5;
+        stop_line_node.second.lanelets = planner_data.route_handler->getPreviousLanelets(ll);
+        stop_line_node.first =
+          boost::geometry::return_envelope<universe_utils::Box2d>(stop_line_node.second.stop_line);
+        rtree_nodes.push_back(stop_line_node);
+      }
+    }
+  }
+  ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()};
+}
+
 VelocityPlanningResult OutOfLaneModule::plan(
   const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & ego_trajectory_points,
   const std::shared_ptr<const PlannerData> planner_data)
@@ -169,6 +211,10 @@ VelocityPlanningResult OutOfLaneModule::plan(
     autoware::motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position);
   ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x;
   ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel();
+  stopwatch.tic("preprocessing");
+  prepare_stop_lines_rtree(ego_data, *planner_data, 100.0);
+  const auto preprocessing_us = stopwatch.toc("preprocessing");
+
   stopwatch.tic("calculate_trajectory_footprints");
   const auto current_ego_footprint =
     out_of_lane::calculate_current_ego_footprint(ego_data, params_, true);
@@ -289,21 +335,23 @@ VelocityPlanningResult OutOfLaneModule::plan(
   RCLCPP_DEBUG(
     logger_,
     "Total time = %2.2fus\n"
+    "\tpreprocessing = %2.0fus\n"
     "\tcalculate_lanelets = %2.0fus\n"
     "\tcalculate_trajectory_footprints = %2.0fus\n"
     "\tcalculate_overlapping_ranges = %2.0fus\n"
     "\tfilter_pred_objects = %2.0fus\n"
     "\tcalculate_decisions = %2.0fus\n"
     "\tcalc_slowdown_points = %2.0fus\n"
     "\tinsert_slowdown_points = %2.0fus\n",
-    total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
+    preprocessing_us, total_time_us, calculate_lanelets_us, calculate_trajectory_footprints_us,
     calculate_overlapping_ranges_us, filter_predicted_objects_us, calculate_decisions_us,
     calc_slowdown_points_us, insert_slowdown_points_us);
   debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array(debug_data_));
   virtual_wall_marker_creator.add_virtual_walls(
     out_of_lane::debug::create_virtual_walls(debug_data_, params_));
   virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now()));
   std::map<std::string, double> processing_times;
+  processing_times["preprocessing"] = preprocessing_us / 1000;
   processing_times["calculate_lanelets"] = calculate_lanelets_us / 1000;
   processing_times["calculate_trajectory_footprints"] = calculate_trajectory_footprints_us / 1000;
   processing_times["calculate_overlapping_ranges"] = calculate_overlapping_ranges_us / 1000;

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp

@@ -16,12 +16,17 @@
 #define TYPES_HPP_
 
 #include <autoware/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
+#include <boost/geometry/geometries/multi_polygon.hpp>
+#include <boost/geometry/index/rtree.hpp>
+
+#include <lanelet2_core/Forward.h>
 #include <lanelet2_core/LaneletMap.h>
 
 #include <algorithm>
@@ -172,6 +177,16 @@ struct OtherLane
   }
 };
 
+namespace bgi = boost::geometry::index;
+struct StopLine
+{
+  universe_utils::LineString2d stop_line;
+  lanelet::ConstLanelets lanelets;
+};
+using StopLineNode = std::pair<universe_utils::Box2d, StopLine>;
+using StopLinesRtree = bgi::rtree<StopLineNode, bgi::rstar<16>>;
+using OutAreaRtree = bgi::rtree<std::pair<universe_utils::Box2d, size_t>, bgi::rstar<16>>;
+
 /// @brief data related to the ego vehicle
 struct EgoData
 {
@@ -180,16 +195,17 @@ struct EgoData
   double velocity{};   // [m/s]
   double max_decel{};  // [m/s²]
   geometry_msgs::msg::Pose pose{};
+  StopLinesRtree stop_lines_rtree;
 };
 
 /// @brief data needed to make decisions
 struct DecisionInputs
 {
-  OverlapRanges ranges{};
-  EgoData ego_data{};
-  autoware_perception_msgs::msg::PredictedObjects objects{};
-  std::shared_ptr<const route_handler::RouteHandler> route_handler{};
-  lanelet::ConstLanelets lanelets{};
+  OverlapRanges ranges;
+  EgoData ego_data;
+  autoware_perception_msgs::msg::PredictedObjects objects;
+  std::shared_ptr<const route_handler::RouteHandler> route_handler;
+  lanelet::ConstLanelets lanelets;
 };
 
 /// @brief debug data

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp

@@ -25,7 +25,7 @@ TEST(TestCollisionDistance, CutPredictedPathBeyondLine)
 {
   using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line;
   autoware_perception_msgs::msg::PredictedPath predicted_path;
-  lanelet::BasicLineString2d stop_line;
+  autoware::universe_utils::LineString2d stop_line;
   double object_front_overhang = 0.0;
   const auto eps = 1e-9;