diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index a010810784300..7845396b0d0cb 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -347,27 +347,27 @@ void DistortionCorrector::undistort_pointcloud( bool is_twist_valid = true; bool is_imu_valid = true; - const double global_point_stamp = + const double current_point_stamp = pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp); // Get closest twist information - while (it_twist != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) { + while (it_twist != std::end(twist_queue_) - 1 && current_point_stamp > twist_stamp) { ++it_twist; twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds(); } - if (std::abs(global_point_stamp - twist_stamp) > 0.1) { + if (std::abs(current_point_stamp - twist_stamp) > 0.1) { is_twist_time_stamp_too_late = true; is_twist_valid = false; } // Get closest IMU information if (use_imu && !angular_velocity_queue_.empty()) { - while (it_imu != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) { + while (it_imu != std::end(angular_velocity_queue_) - 1 && current_point_stamp > imu_stamp) { ++it_imu; imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds(); } - if (std::abs(global_point_stamp - imu_stamp) > 0.1) { + if (std::abs(current_point_stamp - imu_stamp) > 0.1) { is_imu_time_stamp_too_late = true; is_imu_valid = false; } @@ -375,7 +375,7 @@ void DistortionCorrector::undistort_pointcloud( is_imu_valid = false; } - auto time_offset = static_cast(global_point_stamp - prev_time_stamp_sec); + auto time_offset = static_cast(current_point_stamp - prev_time_stamp_sec); // Undistort a single point based on the strategy 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::undistort_pointcloud( ++it_distance; } - prev_time_stamp_sec = global_point_stamp; + prev_time_stamp_sec = current_point_stamp; } warn_if_timestamp_is_too_late(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late); diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp index 3ce80843d64ac..ac9f6a2a1ddc6 100644 --- a/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp @@ -284,11 +284,11 @@ class DistortionCorrectorTest : public ::testing::Test const rclcpp::Time & pointcloud_timestamp, size_t number_of_points) { std::vector timestamps; - rclcpp::Time global_point_stamp = pointcloud_timestamp; + rclcpp::Time current_point_stamp = pointcloud_timestamp; for (size_t i = 0; i < number_of_points; ++i) { - std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds(); + std::uint32_t relative_timestamp = (current_point_stamp - pointcloud_timestamp).nanoseconds(); timestamps.push_back(relative_timestamp); - global_point_stamp = add_milliseconds(global_point_stamp, points_interval_ms); + current_point_stamp = add_milliseconds(current_point_stamp, points_interval_ms); } return timestamps;