associate only one time

Signed-off-by: Kento Yabuuchi <kento.yabuuchi.2@tier4.jp>
KYabuuchi · Apr 17, 2024 · 984d028 · 984d028
1 parent b60bc35
commit 984d028
Showing 1 changed file with 26 additions and 31 deletions.
diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp
@@ -246,43 +246,38 @@ bool DistortionCorrectorComponent::undistortPointCloud(
   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();
-    }
+  while (twist_it != std::end(twist_queue_) - 1 && first_point_time_stamp_sec > 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)};
+  float v{static_cast<float>(twist_it->twist.linear.x)};
+  float w{static_cast<float>(twist_it->twist.angular.z)};
+
+  if (std::abs(first_point_time_stamp_sec - twist_stamp) > 0.2) {
+    twist_time_stamp_is_too_late = true;
+    v = 0.0f;
+    w = 0.0f;
+  }
 
-    if (std::abs(*it_time_stamp - twist_stamp) > 0.1) {
-      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();
+
+    while (imu_it != std::end(angular_velocity_queue_) - 1 &&
+           first_point_time_stamp_sec > imu_stamp) {
+      ++imu_it;
+      imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
     }
 
-    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) {
-        imu_time_stamp_is_too_late = true;
-      } else {
-        w = static_cast<float>(imu_it->vector.z);
-      }
+    if (std::abs(first_point_time_stamp_sec - imu_stamp) > 0.2) {
+      imu_time_stamp_is_too_late = true;
+    } else {
+      w = static_cast<float>(imu_it->vector.z);
     }
+  }
 
+  for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
     const auto time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
     point.setValue(*it_x, *it_y, *it_z);