Skip to content

Commit

Permalink
chore(autoware_pointcloud_preprocessor): fix variable naming in disto…
Browse files Browse the repository at this point in the history
…rtion corrector (#10185)

chore: fix naming

Signed-off-by: vividf <yihsiang.fang@tier4.jp>
  • Loading branch information
vividf authored Mar 6, 2025
1 parent b3caa5c commit df7eb3a
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 10 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -347,35 +347,35 @@ void DistortionCorrector<T>::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;
}
} else {
is_imu_valid = false;
}

auto time_offset = static_cast<float>(global_point_stamp - prev_time_stamp_sec);
auto time_offset = static_cast<float>(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);
Expand Down Expand Up @@ -403,7 +403,7 @@ void DistortionCorrector<T>::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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -284,11 +284,11 @@ class DistortionCorrectorTest : public ::testing::Test
const rclcpp::Time & pointcloud_timestamp, size_t number_of_points)
{
std::vector<std::uint32_t> 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;
Expand Down

0 comments on commit df7eb3a

Please sign in to comment.