@@ -347,35 +347,35 @@ void DistortionCorrector<T>::undistort_pointcloud(
347
347
bool is_twist_valid = true ;
348
348
bool is_imu_valid = true ;
349
349
350
- const double global_point_stamp =
350
+ const double current_point_stamp =
351
351
pointcloud.header .stamp .sec + 1e-9 * (pointcloud.header .stamp .nanosec + *it_time_stamp);
352
352
353
353
// Get closest twist information
354
- while (it_twist != std::end (twist_queue_) - 1 && global_point_stamp > twist_stamp) {
354
+ while (it_twist != std::end (twist_queue_) - 1 && current_point_stamp > twist_stamp) {
355
355
++it_twist;
356
356
twist_stamp = rclcpp::Time (it_twist->header .stamp ).seconds ();
357
357
}
358
- if (std::abs (global_point_stamp - twist_stamp) > 0.1 ) {
358
+ if (std::abs (current_point_stamp - twist_stamp) > 0.1 ) {
359
359
is_twist_time_stamp_too_late = true ;
360
360
is_twist_valid = false ;
361
361
}
362
362
363
363
// Get closest IMU information
364
364
if (use_imu && !angular_velocity_queue_.empty ()) {
365
- while (it_imu != std::end (angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) {
365
+ while (it_imu != std::end (angular_velocity_queue_) - 1 && current_point_stamp > imu_stamp) {
366
366
++it_imu;
367
367
imu_stamp = rclcpp::Time (it_imu->header .stamp ).seconds ();
368
368
}
369
369
370
- if (std::abs (global_point_stamp - imu_stamp) > 0.1 ) {
370
+ if (std::abs (current_point_stamp - imu_stamp) > 0.1 ) {
371
371
is_imu_time_stamp_too_late = true ;
372
372
is_imu_valid = false ;
373
373
}
374
374
} else {
375
375
is_imu_valid = false ;
376
376
}
377
377
378
- auto time_offset = static_cast <float >(global_point_stamp - prev_time_stamp_sec);
378
+ auto time_offset = static_cast <float >(current_point_stamp - prev_time_stamp_sec);
379
379
380
380
// Undistort a single point based on the strategy
381
381
undistort_point (it_x, it_y, it_z, it_twist, it_imu, time_offset, is_twist_valid, is_imu_valid);
@@ -403,7 +403,7 @@ void DistortionCorrector<T>::undistort_pointcloud(
403
403
++it_distance;
404
404
}
405
405
406
- prev_time_stamp_sec = global_point_stamp ;
406
+ prev_time_stamp_sec = current_point_stamp ;
407
407
}
408
408
409
409
warn_if_timestamp_is_too_late (is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);
0 commit comments