use rtree for the stop lines

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

Author: maxime-clem

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp

@@ -15,20 +15,25 @@
 #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/detail/intersects/interface.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 <iterator>
+#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 +63,46 @@ 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_node : query_results) {
+    const auto & stop_line = stop_line_node.second;
+    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());
+          });
+        for (const auto & ll : stop_line.lanelets) {
+        }
+        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 +149,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/out_of_lane_module.cpp

@@ -23,15 +23,29 @@
 #include "overlapping_range.hpp"
 #include "types.hpp"
 
+#include <autoware/universe_utils/ros/update_param.hpp>
 #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/route_handler/route_handler.hpp>
+#include <autoware/universe_utils/geometry/boost_geometry.hpp>
+#include <autoware/universe_utils/geometry/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/detail/disjoint/interface.hpp>
+#include <boost/geometry/algorithms/detail/envelope/interface.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
 
+#include <lanelet2_core/geometry/BoundingBox.h>
 #include <lanelet2_core/geometry/LaneletMap.h>
+#include <lanelet2_core/geometry/Polygon.h>
+#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
+#include <lanelet2_core/primitives/CompoundPolygon.h>
+#include <lanelet2_core/primitives/Lanelet.h>
+#include <lanelet2_core/primitives/Polygon.h>
 
 #include <map>
 #include <memory>
@@ -155,6 +169,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 +219,8 @@ 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();
+  prepare_stop_lines_rtree(ego_data, *planner_data, 100.0);
+
   stopwatch.tic("calculate_trajectory_footprints");
   const auto current_ego_footprint =
     out_of_lane::calculate_current_ego_footprint(ego_data, params_, true);

planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp

@@ -22,6 +22,11 @@
 #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 <autoware/universe_utils/geometry/boost_geometry.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;