Skip to content

Commit df7eb3a

Browse files
authored
chore(autoware_pointcloud_preprocessor): fix variable naming in distortion corrector (#10185)
chore: fix naming Signed-off-by: vividf <yihsiang.fang@tier4.jp>
1 parent b3caa5c commit df7eb3a

File tree

2 files changed

+10
-10
lines changed

2 files changed

+10
-10
lines changed

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -347,35 +347,35 @@ void DistortionCorrector<T>::undistort_pointcloud(
347347
bool is_twist_valid = true;
348348
bool is_imu_valid = true;
349349

350-
const double global_point_stamp =
350+
const double current_point_stamp =
351351
pointcloud.header.stamp.sec + 1e-9 * (pointcloud.header.stamp.nanosec + *it_time_stamp);
352352

353353
// 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) {
355355
++it_twist;
356356
twist_stamp = rclcpp::Time(it_twist->header.stamp).seconds();
357357
}
358-
if (std::abs(global_point_stamp - twist_stamp) > 0.1) {
358+
if (std::abs(current_point_stamp - twist_stamp) > 0.1) {
359359
is_twist_time_stamp_too_late = true;
360360
is_twist_valid = false;
361361
}
362362

363363
// Get closest IMU information
364364
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) {
366366
++it_imu;
367367
imu_stamp = rclcpp::Time(it_imu->header.stamp).seconds();
368368
}
369369

370-
if (std::abs(global_point_stamp - imu_stamp) > 0.1) {
370+
if (std::abs(current_point_stamp - imu_stamp) > 0.1) {
371371
is_imu_time_stamp_too_late = true;
372372
is_imu_valid = false;
373373
}
374374
} else {
375375
is_imu_valid = false;
376376
}
377377

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);
379379

380380
// Undistort a single point based on the strategy
381381
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(
403403
++it_distance;
404404
}
405405

406-
prev_time_stamp_sec = global_point_stamp;
406+
prev_time_stamp_sec = current_point_stamp;
407407
}
408408

409409
warn_if_timestamp_is_too_late(is_twist_time_stamp_too_late, is_imu_time_stamp_too_late);

sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -284,11 +284,11 @@ class DistortionCorrectorTest : public ::testing::Test
284284
const rclcpp::Time & pointcloud_timestamp, size_t number_of_points)
285285
{
286286
std::vector<std::uint32_t> timestamps;
287-
rclcpp::Time global_point_stamp = pointcloud_timestamp;
287+
rclcpp::Time current_point_stamp = pointcloud_timestamp;
288288
for (size_t i = 0; i < number_of_points; ++i) {
289-
std::uint32_t relative_timestamp = (global_point_stamp - pointcloud_timestamp).nanoseconds();
289+
std::uint32_t relative_timestamp = (current_point_stamp - pointcloud_timestamp).nanoseconds();
290290
timestamps.push_back(relative_timestamp);
291-
global_point_stamp = add_milliseconds(global_point_stamp, points_interval_ms);
291+
current_point_stamp = add_milliseconds(current_point_stamp, points_interval_ms);
292292
}
293293

294294
return timestamps;

0 commit comments

Comments
 (0)