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

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -242,78 +242,88 @@
   // 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;
       twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
     }
 
     float v{static_cast<float>(twist_it->twist.linear.x)};
     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);
       }
     }
 
     const auto time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
     point.setValue(*it_x, *it_y, *it_z);
 
     if (need_transform) {
       point = tf2_base_link_to_sensor_inv * point;
     }
 
     theta += w * time_offset;
     baselink_quat.setValue(
       0, 0, tier4_autoware_utils::sin(theta * 0.5f),
       tier4_autoware_utils::cos(theta * 0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta);
     const float dis = v * time_offset;
     x += dis * tier4_autoware_utils::cos(theta);
     y += dis * tier4_autoware_utils::sin(theta);
 
     baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0));
     baselink_tf_odom.setRotation(baselink_quat);
 
     undistorted_point = baselink_tf_odom * point;
 
     if (need_transform) {
       undistorted_point = tf2_base_link_to_sensor * undistorted_point;
     }
 
     *it_x = static_cast<float>(undistorted_point.getX());
     *it_y = static_cast<float>(undistorted_point.getY());
     *it_z = static_cast<float>(undistorted_point.getZ());
 
     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;
 }