Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_pointcloud_preprocessor): distortion corrector node update azimuth and distance #8380

Merged
Changes from 1 commit
Commits
Show all changes
98 commits
Select commit Hold shift + click to select a range
b7ea57d
feat: add option for updating distance and azimuth value
vividf Aug 6, 2024
1bd37c4
chore: clean code
vividf Aug 6, 2024
336f709
chore: remove space
vividf Aug 6, 2024
c8378ac
chore: add documentation
vividf Aug 6, 2024
3081d66
chore: fix docs
vividf Aug 6, 2024
f6394b2
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Aug 6, 2024
94551b6
feat: conversion formula implementation for degree, still need to cha…
vividf Aug 9, 2024
9ac2689
chore: fix tests for AzimuthConversionExists function
vividf Aug 13, 2024
0990c02
feat: add fastatan to utils
vividf Aug 14, 2024
6796341
feat: remove seperate sin, cos and use sin_and_cos function
vividf Aug 14, 2024
afd0fda
chore: fix readme
vividf Aug 14, 2024
9809a4b
chore: fix some grammar errors
vividf Aug 14, 2024
64bfbc5
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Aug 14, 2024
9aeac77
chore: fix spell error
vividf Aug 14, 2024
8a37932
Merge branch 'feature/distortion_corrector_node_update_azimuth_and_di…
vividf Aug 14, 2024
b5dfa85
chore: set debug mode to false
vividf Aug 14, 2024
d55db4b
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Aug 19, 2024
d57f751
chore: set update_azimuth_and_distance default value to false
vividf Aug 19, 2024
8703a23
chore: update readme
vividf Aug 19, 2024
5390055
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Sep 4, 2024
f509d7d
chore: remove cout
vividf Sep 4, 2024
62434fa
chore: add opencv license
vividf Sep 4, 2024
a3c977b
chore: fix grammar error
vividf Sep 4, 2024
ee1b169
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Sep 9, 2024
7288042
style(pre-commit): autofix
pre-commit-ci[bot] Sep 9, 2024
7e1f510
chore: add runtime error when azimuth conversion failed
vividf Sep 9, 2024
b156fb5
chore: change default pointcloud
vividf Sep 9, 2024
0a1f4c9
chore: change function name
vividf Sep 9, 2024
f11e529
chore: move variables to structure
vividf Sep 9, 2024
f0609f5
chore: add random seed
vividf Sep 9, 2024
ec4172e
chore: rewrite get conversion function
vividf Sep 9, 2024
e797752
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Sep 10, 2024
be9d8c9
Merge branch 'feature/distortion_corrector_node_update_azimuth_and_di…
vividf Sep 10, 2024
427a0ce
chore: fix opencv fast atan2 function
vividf Sep 10, 2024
25253c3
chore: fix schema description
vividf Sep 10, 2024
ed3efee
Update sensing/autoware_pointcloud_preprocessor/test/test_distortion_…
vividf Sep 10, 2024
031bc2c
Update sensing/autoware_pointcloud_preprocessor/test/test_distortion_…
vividf Sep 10, 2024
a1f076c
chore: move code to function for readability
vividf Sep 10, 2024
71b1917
chore: simplify code
vividf Sep 10, 2024
80ad8e2
chore: fix sentence, angle conversion
vividf Sep 10, 2024
11b88bd
chore: add more invalid condition
vividf Sep 11, 2024
53f4162
chore: fix the string name to enum
vividf Sep 11, 2024
a65a2bf
chore: remove runtime error
vividf Sep 11, 2024
8a24a79
chore: use optional for AngleConversion structure
vividf Sep 11, 2024
f6c5dab
chore: fix bug and clean code
vividf Sep 11, 2024
4342dca
chore: refactor the logic of calculating conversion
vividf Sep 11, 2024
242ab45
chore: refactor function in unit test
vividf Sep 12, 2024
1d5ccdb
chore: RCLCPP_WARN_STREAM logging when failed to get angle conversion
vividf Sep 12, 2024
5e55d01
chore: improve normalize angle algorithm
vividf Sep 12, 2024
02beb86
chore: improve multiple_of_90_degrees logic
vividf Sep 12, 2024
29f4b96
chore: add opencv license
vividf Sep 12, 2024
b9a5c56
style(pre-commit): autofix
pre-commit-ci[bot] Sep 12, 2024
13d981f
chore: clean code
vividf Sep 12, 2024
96bafef
Merge branch 'feature/distortion_corrector_node_update_azimuth_and_di…
vividf Sep 12, 2024
02b1217
chore: fix sentence
vividf Sep 12, 2024
cdb88ae
style(pre-commit): autofix
pre-commit-ci[bot] Sep 12, 2024
4e3d70a
Merge branch 'feature/distortion_corrector_node_update_azimuth_and_di…
vividf Sep 12, 2024
a853293
chore: add 0 0 0 points in test case
vividf Sep 12, 2024
cd066c4
chore: fix spell error
vividf Sep 12, 2024
5560c1d
Update common/autoware_universe_utils/NOTICE
vividf Sep 13, 2024
4c521c0
Update sensing/autoware_pointcloud_preprocessor/src/distortion_correc…
vividf Sep 13, 2024
8e26988
Update sensing/autoware_pointcloud_preprocessor/src/distortion_correc…
vividf Sep 13, 2024
41f086a
chore: use constexpr for threshold
vividf Sep 13, 2024
8428321
Merge branch 'feature/distortion_corrector_node_update_azimuth_and_di…
vividf Sep 13, 2024
a3e5cd8
chore: fix the path of license
vividf Sep 13, 2024
550a646
chore: explanation for failures
vividf Sep 13, 2024
03354c8
chore: use throttle
vividf Sep 13, 2024
320e961
chore: fix empty pointcloud function
vividf Sep 13, 2024
f6be637
refactor: change camel to snake case
vividf Sep 17, 2024
8d4bcd7
Update sensing/autoware_pointcloud_preprocessor/include/autoware/poin…
vividf Sep 19, 2024
4d58839
Update sensing/autoware_pointcloud_preprocessor/include/autoware/poin…
vividf Sep 19, 2024
1f144d3
style(pre-commit): autofix
pre-commit-ci[bot] Sep 19, 2024
f58307d
Update sensing/autoware_pointcloud_preprocessor/test/test_distortion_…
vividf Sep 19, 2024
896d843
refactor: refactor virtual function in base class
vividf Sep 19, 2024
3027ec1
chore: fix test naming error
vividf Sep 19, 2024
3c74303
chore: fix clang error
vividf Sep 19, 2024
2439956
chore: fix error
vividf Sep 20, 2024
f40d081
Merge branch 'autowarefoundation:main' into feature/distortion_correc…
vividf Sep 25, 2024
4e0b8bf
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Sep 30, 2024
cf27e42
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Sep 30, 2024
783219f
chore: fix clangd
vividf Sep 30, 2024
caafd52
Merge branch 'feature/distortion_corrector_node_update_azimuth_and_di…
vividf Sep 30, 2024
72f5bb2
chore: add runtime error if the setting is wrong
vividf Oct 3, 2024
9663eaa
chore: clean code
vividf Oct 3, 2024
a06ad31
Update sensing/autoware_pointcloud_preprocessor/src/distortion_correc…
vividf Oct 3, 2024
20ffc99
style(pre-commit): autofix
pre-commit-ci[bot] Oct 3, 2024
7f31139
chore: fix unit test for runtime error
vividf Oct 3, 2024
9d1bef7
Update sensing/autoware_pointcloud_preprocessor/docs/distortion-corre…
vividf Oct 8, 2024
bbbc501
chore: fix offset_rad_threshold
vividf Oct 8, 2024
2594e76
chore: change pointer to reference
vividf Oct 8, 2024
9d0aa59
chore: snake_case for unit test
vividf Oct 8, 2024
cfddc0d
chore: fix refactor process twist and imu
vividf Oct 8, 2024
b1ffa5e
chore: fix abs and return type of matrix to tf2
vividf Oct 8, 2024
57824ef
chore: fix grammar error
vividf Oct 8, 2024
9680027
chore: fix readme description
vividf Oct 9, 2024
4bc0df1
chore: remove runtime error
vividf Oct 9, 2024
384abe8
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Oct 24, 2024
a4a693e
Merge branch 'main' into feature/distortion_corrector_node_update_azi…
vividf Oct 28, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
chore: fix empty pointcloud function
Signed-off-by: vividf <yihsiang.fang@tier4.jp>
  • Loading branch information
vividf committed Sep 13, 2024
commit 320e9610d1dd43eeffc04bdfb3accd17009bd97a
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2024 TIER IV, Inc.

Check warning on line 1 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Code Duplication

introduced similar code in: TEST:DistortionCorrectorTest:TestTryComputeAngleConversionCartesianPointcloud,TEST:DistortionCorrectorTest:TestTryComputeAngleConversionOnHesaiPointcloud,TEST:DistortionCorrectorTest:TestTryComputeAngleConversionOnRandomPointcloud,TEST:DistortionCorrectorTest:TestTryComputeAngleConversionOnVelodynePointcloud and 2 more functions. Avoid duplicated, aka copy-pasted, code inside the module. More duplication lowers the code health.

Check notice on line 1 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Primitive Obsession

The ratio of primitive types in function arguments decreases from 70.00% to 54.05%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -171,65 +171,57 @@
return imu_msgs;
}

std::tuple<std::vector<Eigen::Vector3f>, std::vector<float>> generateDefaultPointcloud(
AngleCoordinateSystem vendor)
{
// Generate all combinations of signs { -, 0, + } x { -, 0, + } for x and y.
// Also include the case of (0, 0 ,0)
std::vector<Eigen::Vector3f> default_points = {{
Eigen::Vector3f(0.0f, 0.0f, 0.0f), // point 1
Eigen::Vector3f(0.0f, 0.0f, 0.0f), // point 2
Eigen::Vector3f(10.0f, 0.0f, 1.0f), // point 3
Eigen::Vector3f(5.0f, -5.0f, 2.0f), // point 4
Eigen::Vector3f(0.0f, -10.0f, 3.0f), // point 5
Eigen::Vector3f(-5.0f, -5.0f, 4.0f), // point 6
Eigen::Vector3f(-10.0f, 0.0f, 5.0f), // point 7
Eigen::Vector3f(-5.0f, 5.0f, -5.0f), // point 8
Eigen::Vector3f(0.0f, 10.0f, -4.0f), // point 9
Eigen::Vector3f(5.0f, 5.0f, -3.0f), // point 10
}};

std::vector<float> default_azimuths;
for (const auto & point : default_points) {
if (vendor == AngleCoordinateSystem::VELODYNE) {
// velodyne coordinates: x-axis is 0 degrees, y-axis is 270 degrees, angle increase in
// clockwise direction
float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
if (cartesian_deg < 0) cartesian_deg += 360;
float velodyne_deg = 360 - cartesian_deg;
if (velodyne_deg == 360) velodyne_deg = 0;
default_azimuths.push_back(velodyne_deg * autoware::universe_utils::pi / 180);
} else if (vendor == AngleCoordinateSystem::HESAI) {
// hesai coordinates: y-axis is 0 degrees, x-axis is 90 degrees, angle increase in clockwise
// direction
float cartesian_deg = std::atan2(point.y(), point.x()) * 180 / autoware::universe_utils::pi;
if (cartesian_deg < 0) cartesian_deg += 360;
float hesai_deg = 90 - cartesian_deg < 0 ? 90 - cartesian_deg + 360 : 90 - cartesian_deg;
if (hesai_deg == 360) hesai_deg = 0;
default_azimuths.push_back(hesai_deg * autoware::universe_utils::pi / 180);
} else if (vendor == AngleCoordinateSystem::CARTESIAN) {
// Cartesian coordinates: x-axis is 0 degrees, y-axis is 90 degrees, angle increase in
// counterclockwise direction
default_azimuths.push_back(std::atan2(point.y(), point.x()));
} else {
throw std::runtime_error("Invalid angle coordinate system");
}
}

return std::make_tuple(default_points, default_azimuths);
}

Check warning on line 220 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

generateDefaultPointcloud has a cyclomatic complexity of 11, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 220 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead

generateDefaultPointcloud has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

sensor_msgs::msg::PointCloud2 generateEmptyPointCloudMsg(rclcpp::Time stamp)
{
sensor_msgs::msg::PointCloud2 empty_pointcloud_msg;
empty_pointcloud_msg.header.stamp = stamp;
empty_pointcloud_msg.header.frame_id = "lidar_top";
empty_pointcloud_msg.height = 1;
empty_pointcloud_msg.is_dense = true;
empty_pointcloud_msg.is_bigendian = false;
empty_pointcloud_msg.width = 0;
empty_pointcloud_msg.row_step = 0;

auto empty_pointcloud_msg = generatePointCloudMsg(true, stamp, {}, {});
return empty_pointcloud_msg;
}

mojomex marked this conversation as resolved.
Show resolved Hide resolved
Expand Down Expand Up @@ -762,36 +754,36 @@
TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureLinearMotion)
{
rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
auto [default_points, default_azimuths] =
generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
auto test2d_pointcloud =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
auto test3d_pointcloud =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);

// Generate and process a single twist message with constant linear velocity
auto twist_msg = generateTwistMsg(1.0, 0.0, timestamp);

distortion_corrector_2d_->processTwistMessage(twist_msg);
distortion_corrector_2d_->initialize();
distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, test2d_pointcloud);

distortion_corrector_3d_->processTwistMessage(twist_msg);
distortion_corrector_3d_->initialize();
distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);

// Generate expected point cloud for testing
auto expected_pointcloud =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);

// Calculate expected point cloud values based on constant linear motion
double velocity = 1.0; // 1 m/s linear velocity
sensor_msgs::PointCloud2Iterator<float> iter_x(expected_pointcloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(expected_pointcloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(expected_pointcloud, "z");
sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(expected_pointcloud, "time_stamp");

Check warning on line 786 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method

TEST:DistortionCorrectorTest:TestUndistortPointCloudWithPureLinearMotion has 73 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

std::vector<Eigen::Vector3f> expected_points;
for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) {
Expand Down Expand Up @@ -853,71 +845,71 @@
TEST_F(DistortionCorrectorTest, TestUndistortPointCloudWithPureRotationalMotion)
{
rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
auto [default_points, default_azimuths] =
generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
auto test2d_pointcloud =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);
auto test3d_pointcloud =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);

// Generate and process a single twist message with constant angular velocity
auto twist_msg = generateTwistMsg(0.0, 0.1, timestamp);

distortion_corrector_2d_->processTwistMessage(twist_msg);
distortion_corrector_2d_->initialize();
distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
distortion_corrector_2d_->undistortPointCloud(false, std::nullopt, test2d_pointcloud);

distortion_corrector_3d_->processTwistMessage(twist_msg);
distortion_corrector_3d_->initialize();
distortion_corrector_3d_->setPointCloudTransform("base_link", "base_link");
distortion_corrector_3d_->undistortPointCloud(false, std::nullopt, test3d_pointcloud);

// Generate expected point cloud for testing
auto expected_pointcloud =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);

// Calculate expected point cloud values based on constant rotational motion
double angular_velocity = 0.1; // 0.1 rad/s rotational velocity
sensor_msgs::PointCloud2Iterator<float> iter_x(expected_pointcloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(expected_pointcloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(expected_pointcloud, "z");
sensor_msgs::PointCloud2Iterator<std::uint32_t> iter_t(expected_pointcloud, "time_stamp");

std::vector<Eigen::Vector3f> expected_points;
for (; iter_t != iter_t.end(); ++iter_t, ++iter_x, ++iter_y, ++iter_z) {
double time_offset = static_cast<double>(*iter_t) / 1e9;
float angle = angular_velocity * time_offset;

// Set the quaternion for the current angle
tf2::Quaternion quaternion;
quaternion.setValue(
0, 0, autoware::universe_utils::sin(angle * 0.5f),
autoware::universe_utils::cos(angle * 0.5f));

tf2::Vector3 point(*iter_x, *iter_y, *iter_z);
tf2::Vector3 rotated_point = tf2::quatRotate(quaternion, point);
expected_points.emplace_back(
static_cast<float>(rotated_point.x()), static_cast<float>(rotated_point.y()),
static_cast<float>(rotated_point.z()));
}

// Verify each point in the undistorted 2D point cloud
sensor_msgs::PointCloud2Iterator<float> test2d_iter_x(test2d_pointcloud, "x");
sensor_msgs::PointCloud2Iterator<float> test2d_iter_y(test2d_pointcloud, "y");
sensor_msgs::PointCloud2Iterator<float> test2d_iter_z(test2d_pointcloud, "z");

size_t i = 0;
std::ostringstream oss;
oss << "Expected pointcloud:\n";

for (; test2d_iter_x != test2d_iter_x.end();
++test2d_iter_x, ++test2d_iter_y, ++test2d_iter_z, ++i) {
oss << "Point " << i << ": (" << *test2d_iter_x << ", " << *test2d_iter_y << ", "
<< *test2d_iter_z << ")\n";
EXPECT_FLOAT_EQ(*test2d_iter_x, expected_points[i].x());
EXPECT_FLOAT_EQ(*test2d_iter_y, expected_points[i].y());
EXPECT_FLOAT_EQ(*test2d_iter_z, expected_points[i].z());

Check warning on line 912 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

TEST:DistortionCorrectorTest:TestUndistortPointCloudWithPureRotationalMotion increases from 77 to 81 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
}

if (debug_) {
Expand Down Expand Up @@ -954,118 +946,118 @@
}
}

TEST_F(DistortionCorrectorTest, TestUndistortPointCloudNotUpdatingAzimuthAndDistance)
{
// Test the case when the cloud will not update the azimuth and distance values
// 1. when pointcloud is in base_link (pointcloudTransformNeeded() is false)

// Generate the point cloud message in base_link
rclcpp::Time timestamp(timestamp_seconds_, timestamp_nanoseconds_, RCL_ROS_TIME);
auto [default_points, default_azimuths] =
generateDefaultPointcloud(AngleCoordinateSystem::CARTESIAN);
auto pointcloud_base_link =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);

// Generate and process multiple twist messages
generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);

// Generate and process multiple IMU messages
generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);

distortion_corrector_2d_->initialize();
distortion_corrector_2d_->setPointCloudTransform("base_link", "base_link");
auto angle_conversion_opt =
distortion_corrector_2d_->tryComputeAngleConversion(pointcloud_base_link);

distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_base_link);

auto original_pointcloud_base_link =
generatePointCloudMsg(false, timestamp, default_points, default_azimuths);

sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_base_link(
pointcloud_base_link, "azimuth");
sensor_msgs::PointCloud2ConstIterator<float> test_iter_distance_base_link(
pointcloud_base_link, "distance");

sensor_msgs::PointCloud2ConstIterator<float> original_iter_azimuth_base_link(
original_pointcloud_base_link, "azimuth");
sensor_msgs::PointCloud2ConstIterator<float> original_iter_distance_base_link(
original_pointcloud_base_link, "distance");

size_t i = 0;
std::ostringstream oss;

oss << "Expected pointcloud:\n";
for (; test_iter_azimuth_base_link != test_iter_azimuth_base_link.end();
++test_iter_azimuth_base_link, ++test_iter_distance_base_link,
++original_iter_azimuth_base_link, ++original_iter_distance_base_link, ++i) {
oss << "Point " << i << " - Output azimuth and distance: (" << *test_iter_azimuth_base_link
<< ", " << *test_iter_distance_base_link << ")"
<< " vs Original azimuth and distance: (" << *original_iter_azimuth_base_link << ", "
<< *original_iter_distance_base_link << ")\n";

EXPECT_FLOAT_EQ(*test_iter_azimuth_base_link, *original_iter_azimuth_base_link);
EXPECT_FLOAT_EQ(*test_iter_distance_base_link, *original_iter_distance_base_link);
}

if (debug_) {
RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
}

// Test the case when the cloud will not update the azimuth and distance values
// 2. when the return value of tryComputeAngleConversion is std::nullopt (couldn't find the angle
// conversion)

// Generate the point cloud message in sensor frame
auto pointcloud_lidar_top =
generatePointCloudMsg(true, timestamp, default_points, default_azimuths);

// Generate and process multiple twist messages
generateAndProcessTwistMsgs(distortion_corrector_2d_, timestamp);

// Generate and process multiple IMU messages
generateAndProcessIMUMsgs(distortion_corrector_2d_, timestamp);

distortion_corrector_2d_->initialize();
distortion_corrector_2d_->setPointCloudTransform("base_link", "lidar_top");

angle_conversion_opt = std::nullopt;
distortion_corrector_2d_->undistortPointCloud(true, angle_conversion_opt, pointcloud_lidar_top);

auto original_pointcloud_lidar_top =
generatePointCloudMsg(true, timestamp, default_points, default_azimuths);

sensor_msgs::PointCloud2ConstIterator<float> test_iter_azimuth_lidar_top(
pointcloud_lidar_top, "azimuth");
sensor_msgs::PointCloud2ConstIterator<float> test_iter_distance_lidar_top(
pointcloud_lidar_top, "distance");

sensor_msgs::PointCloud2ConstIterator<float> original_iter_azimuth_lidar_top(
original_pointcloud_lidar_top, "azimuth");
sensor_msgs::PointCloud2ConstIterator<float> original_iter_distance_lidar_top(
original_pointcloud_lidar_top, "distance");

i = 0;
oss.str("");
oss.clear();

oss << "Expected pointcloud:\n";
for (; test_iter_azimuth_lidar_top != test_iter_azimuth_lidar_top.end();
++test_iter_azimuth_lidar_top, ++test_iter_distance_lidar_top,
++original_iter_azimuth_lidar_top, ++original_iter_distance_lidar_top, ++i) {
oss << "Point " << i << " - Output azimuth and distance: (" << *test_iter_azimuth_lidar_top
<< ", " << *test_iter_distance_lidar_top << ")"
<< " vs Original azimuth and distance: (" << *original_iter_azimuth_lidar_top << ", "
<< *original_iter_distance_lidar_top << ")\n";

EXPECT_FLOAT_EQ(*test_iter_azimuth_lidar_top, *original_iter_azimuth_lidar_top);
EXPECT_FLOAT_EQ(*test_iter_distance_lidar_top, *original_iter_distance_lidar_top);
}

if (debug_) {
RCLCPP_INFO(node_->get_logger(), "%s", oss.str().c_str());
}
}

Check warning on line 1060 in sensing/autoware_pointcloud_preprocessor/test/test_distortion_corrector_node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Large Method

TEST:DistortionCorrectorTest:TestUndistortPointCloudNotUpdatingAzimuthAndDistance has 76 lines, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

TEST_F(DistortionCorrectorTest, TestUndistortPointCloudUpdateAzimuthAndDistanceInVelodyne)
{
Expand Down
Loading