From d25170fd95d75ce84e44e1d8d5d5f1a721d4f769 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <technolojin@gmail.com>
Date: Fri, 16 Aug 2024 09:29:36 +0900
Subject: [PATCH] perf(autoware_map_based_prediction): removed duplicate
 findNearest calculations (#8490) (#1470)

Signed-off-by: Y.Hisaki <yhisaki31@gmail.com>
Co-authored-by: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com>
---
 .../src/map_based_prediction_node.cpp         | 36 ++++++++++---------
 1 file changed, 20 insertions(+), 16 deletions(-)

diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
index 8c595c75eb481..409e5bc4bc7d7 100644
--- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp
@@ -32,6 +32,7 @@
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/Lanelet.h>
+#include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_core/geometry/Point.h>
 #include <lanelet2_routing/RoutingGraph.h>
 #include <tf2/utils.h>
@@ -360,20 +361,11 @@ CrosswalkEdgePoints getCrosswalkEdgePoints(const lanelet::ConstLanelet & crosswa
 }
 
 bool withinRoadLanelet(
-  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const TrackedObject & object,
+  const std::vector<std::pair<double, lanelet::Lanelet>> & surrounding_lanelets_with_dist,
   const bool use_yaw_information = false)
 {
-  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
-  lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
-  // nearest lanelet
-  constexpr double search_radius = 10.0;  // [m]
-  const auto surrounding_lanelets =
-    lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, search_radius);
-
-  for (const auto & lanelet_with_dist : surrounding_lanelets) {
-    const auto & dist = lanelet_with_dist.first;
-    const auto & lanelet = lanelet_with_dist.second;
-
+  for (const auto & [dist, lanelet] : surrounding_lanelets_with_dist) {
     if (lanelet.hasAttribute(lanelet::AttributeName::Subtype)) {
       lanelet::Attribute attr = lanelet.attribute(lanelet::AttributeName::Subtype);
       if (
@@ -397,6 +389,20 @@ bool withinRoadLanelet(
   return false;
 }
 
+bool withinRoadLanelet(
+  const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const bool use_yaw_information = false)
+{
+  const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position;
+  lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y);
+  // nearest lanelet
+  constexpr double search_radius = 10.0;  // [m]
+  const auto surrounding_lanelets_with_dist =
+    lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, search_point, search_radius);
+
+  return withinRoadLanelet(object, surrounding_lanelets_with_dist, use_yaw_information);
+}
+
 boost::optional<CrosswalkEdgePoints> isReachableCrosswalkEdgePoints(
   const TrackedObject & object, const lanelet::ConstLanelets & surrounding_lanelets,
   const lanelet::ConstLanelets & surrounding_crosswalks, const CrosswalkEdgePoints & edge_points,
@@ -1412,9 +1418,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
   const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear;
   const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y);
   const auto velocity = std::max(min_crosswalk_user_velocity_, estimated_velocity);
-  // TODO(Mamoru Sobue): 3rd argument of findNearest is the number of lanelets, not radius, so past
-  // implementation has been wrong.
-  const auto surrounding_lanelets_with_dist = lanelet::geometry::findNearest(
+  const auto surrounding_lanelets_with_dist = lanelet::geometry::findWithin2d(
     lanelet_map_ptr_->laneletLayer, lanelet::BasicPoint2d{obj_pos.x, obj_pos.y},
     prediction_time_horizon_.pedestrian * velocity);
   lanelet::ConstLanelets surrounding_lanelets;
@@ -1459,7 +1463,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser(
 
     // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest
     // crosswalk and generate path to the crosswalk edge
-  } else if (withinRoadLanelet(object, lanelet_map_ptr_)) {
+  } else if (withinRoadLanelet(object, surrounding_lanelets_with_dist)) {
     lanelet::ConstLanelet closest_crosswalk{};
     const auto & obj_pose = object.kinematics.pose_with_covariance.pose;
     const auto found_closest_crosswalk =