perf(map_based_prediction): early return in crosswalk iteration

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jun 27, 2022 · c42d6f5 · c42d6f5
1 parent 0e6bdcd
commit c42d6f5
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
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
@@ -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
@@ -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_);