perf(pointcloud_preprocessor): prevent excessive log and speed up a b…

…it (autowarefoundation#6843)

* provide only one warning

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* associate only one time

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

* Revert "associate only one time"

This reverts commit 984d028.

* remove redundant for loop and rclcpp::Time instantiation

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

---------

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -242,6 +242,16 @@ bool DistortionCorrectorComponent::undistortPointCloud(
   // For performance, avoid transform computation if unnecessary
   bool need_transform = points.header.frame_id != base_link_frame_;
 
+  // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
+  double imu_stamp{0.0};
+  if (use_imu_ && !angular_velocity_queue_.empty()) {
+    imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
+  }
+
+  // If there is a point that cannot be associated, record it to issue a warning
+  bool twist_time_stamp_is_too_late = false;
+  bool imu_time_stamp_is_too_late = false;
+
   for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
     while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) {
       ++twist_it;
@@ -252,32 +262,19 @@ bool DistortionCorrectorComponent::undistortPointCloud(
     float w{static_cast<float>(twist_it->twist.angular.z)};
 
     if (std::abs(*it_time_stamp - twist_stamp) > 0.1) {
-      RCLCPP_WARN_STREAM_THROTTLE(
-        get_logger(), *get_clock(), 10000 /* ms */,
-        "twist time_stamp is too late. Could not interpolate.");
+      twist_time_stamp_is_too_late = true;
       v = 0.0f;
       w = 0.0f;
     }
 
     if (use_imu_ && !angular_velocity_queue_.empty()) {
-      // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
-      double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
-
-      for (;
-           (imu_it != std::end(angular_velocity_queue_) - 1 &&
-            *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
-           ++imu_it) {
-      }
-
       while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) {
         ++imu_it;
         imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
       }
 
       if (std::abs(*it_time_stamp - imu_stamp) > 0.1) {
-        RCLCPP_WARN_STREAM_THROTTLE(
-          get_logger(), *get_clock(), 10000 /* ms */,
-          "imu time_stamp is too late. Could not interpolate.");
+        imu_time_stamp_is_too_late = true;
       } else {
         w = static_cast<float>(imu_it->vector.z);
       }
@@ -314,6 +311,19 @@ bool DistortionCorrectorComponent::undistortPointCloud(
 
     prev_time_stamp_sec = *it_time_stamp;
   }
+
+  if (twist_time_stamp_is_too_late) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      get_logger(), *get_clock(), 10000 /* ms */,
+      "twist time_stamp is too late. Could not interpolate.");
+  }
+
+  if (imu_time_stamp_is_too_late) {
+    RCLCPP_WARN_STREAM_THROTTLE(
+      get_logger(), *get_clock(), 10000 /* ms */,
+      "imu time_stamp is too late. Could not interpolate.");
+  }
+
   return true;
 }