From c42d6f531227cff0cb53fa9a45a086ee56b14c3d Mon Sep 17 00:00:00 2001
From: satoshi-ota <satoshi.ota928@gmail.com>
Date: Thu, 9 Jun 2022 17:04:47 +0900
Subject: [PATCH] perf(map_based_prediction): early return in crosswalk
 iteration

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
---
 .../map_based_prediction/map_based_prediction_node.hpp   | 5 +++++
 .../src/map_based_prediction_node.cpp                    | 9 +++++++++
 2 files changed, 14 insertions(+)

diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
index 52068ca72859f..e3aba33bd9c34 100644
--- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
+++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
@@ -22,6 +22,7 @@
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
@@ -87,6 +88,7 @@ using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_perception_msgs::msg::TrackedObject;
 using autoware_auto_perception_msgs::msg::TrackedObjectKinematics;
 using autoware_auto_perception_msgs::msg::TrackedObjects;
+using tier4_autoware_utils::StopWatch;
 
 class MapBasedPredictionNode : public rclcpp::Node
 {
@@ -136,6 +138,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   double reference_path_resolution_;
   double min_crosswalk_user_velocity_;
 
+  // Stop watch
+  StopWatch<std::chrono::milliseconds> stop_watch_;
+
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
   void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects);
diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp
index 6d51acce3c427..eea016d8135e2 100644
--- a/perception/map_based_prediction/src/map_based_prediction_node.cpp
+++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp
@@ -450,6 +450,15 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
         for (const auto & crosswalk : crosswalks_) {
           const auto entry_point = getCrosswalkEntryPoint(crosswalk);
 
+          const auto reachable_first = hasPotentialToReach(
+            object, entry_point.first, prediction_time_horizon_, min_crosswalk_user_velocity_);
+          const auto reachable_second = hasPotentialToReach(
+            object, entry_point.second, prediction_time_horizon_, min_crosswalk_user_velocity_);
+
+          if (!reachable_first && !reachable_second) {
+            continue;
+          }
+
           const auto reachable_crosswalk = isReachableEntryPoint(
             object, entry_point, lanelet_map_ptr_, prediction_time_horizon_,
             min_crosswalk_user_velocity_);