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

[vividf]

Description

This PR solves the issue #3896.
Note that this PR is tested with the nebula PR tier4/nebula#180

• The node updates the per-point azimuth and distance values based on the undistorted XYZ coordinates when the input point cloud is in the sensor frame (not in the base_link) and the update_azimuth_and_distance parameter is set to true. The azimuth values are calculated using the cv::fastAtan2 function.
• Please note that updating the azimuth and distance fields increases the execution time by approximately 20%. Additionally, due to the cv::fastAtan2 algorithm's accuracy of about 0.3 degrees, there is a possibility of changing beam order for high azimuth resolution LiDAR.
• LiDARs from different vendors have different azimuth coordinates. Currently, the coordinate systems listed below have been tested, and the node will update the azimuth based on the input coordinate system.
  • velodyne: (x: 0 degrees, y: 270 degrees)
  • hesai: (x: 90 degrees, y: 0 degrees)
  • others: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees)

before distortion correction (Velodyne):
x: -7.27784, y: 1.06172, z: xxxxx, azimuth: 3.28645, distance: 7.356
after distortion correction:
x: -7.2696, y: 0.767085, z: xxxxx, azimuth: 3.24671, distance: 7.311

More information in https://github.com/vividf/autoware.universe/blob/feature/distortion_corrector_node_update_azimuth_and_distance/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md.

Additionally, this PR also removes the separate sin and cos by using the sin_and_cos function which can speed up the processing time (from this #7120)

Related links

Previous closed PR: #5560

Parent Issue:

• Link

How was this PR tested?

• Tested with Unit test.

colcon test --packages-select autoware_pointcloud_preprocessor --event-handlers console_cohesion+

• Tested with sample rosbag.
  TIER4_INTERNAL_LINK

1. change the input/pointcloud to /sensing/lidar/top/pointcloud_raw_ex in the launch file
2. change the parameter update_azimuth_and_distance to true in distortion_corrector_node.param.yaml
3. ros2 launch autoware_pointcloud_preprocessor distortion_corrector_node.launch.xml and ros2 topic hz /sensing/lidar/top/rectified/pointcloud_ex
4. play the provided ros bag

Time comparison
Input topic: /sensing/lidar/top/pointcloud_raw_ex (number of points is larger than the /sensing/lidar/top/mirror_cropped/pointcloud_ex)
bag: sample rosbag
frame: 121

• Before this PR

	Minimum	Maximum	Average
Time	7.763	15.318	10.82

• After this PR

1. with new sin_and_cos optimization
2. without update azimuth and distance

	Minimum	Maximum	Average
Time	7.038	14.375	10.28

1. with new sin_and_cos optimization
2. with update azimuth and distance

	Minimum	Maximum	Average
Time	9.078	19.907	12.33

Notes for reviewers

None.

Interface changes

None.

Effects on system behavior

None.

[github-actions]

Thank you for contributing to the Autoware project!

🚧 If your pull request is in progress, switch it to draft mode.

Please ensure:

• You've checked our contribution guidelines.
• Your PR follows our pull request guidelines.
• All required CI checks pass before marking the PR ready for review.

[mojomex]

The code looks very good now, thanks!!
I tried running it with the sample rosbag from the logging sim tutorial but am constantly getting the following error, along with a pointcloud that jumps or hangs for a frame sometimes:

[distortion_corrector_node-1] [1727355131.361596872 WARN] [distortion_corrector_node]: Twist time_stamp is too late. Could not interpolate.

Could you tell me what rosbag to use or what to do to set it up correctly?

Here's my commands:

# All in separate terminals
ros2 launch autoware_pointcloud_preprocessor distortion_corrector_node.launch.xml
ros2 bag play -l ~/autoware_map/sample-rosbag/ -s sqlite3 --remap /sensing/lidar/top/velodyne_packets:=/velodyne_packets
ros2 launch nebula_ros nebula_launch.py sensor_model:=VLS128 launch_hw:=false
ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 base_link velodyne

As for processing time, I got similar numbers to the ones you stated. My maximum was around 21ms. I guess this module would be perfect to parallelize/gpu-ize in the future.

[vividf]

@mojomex
Could you try with the ros2 launch autoware_launch logging_simulator.launch.xml map_path:=$HOME/autoware_map/sample-map-rosbag vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit.

From your commands, it seems that you didn't run the vehicle velocity converter and the topic sensing/vehicle_velocity_converter/twist_with_covariance is missed.

I could give you the rosbag that I recorded before that includes that topic on Monday :)

[mojomex]

@veqcc CPPcheck failed on a file that has not been touched by this PR. How to best handle such a case?

[veqcc]

CPPcheck failed on a file that has not been touched by this PR. How to best handle such a case?

Sorry for the inconvenience.
This problem is fixed in #8978, so please merge it into your branch!! (Or just Update branch to the latest main)

[mojomex]

It works well for me, thanks! Below are videos to confirm (top: input, bottom: output).
In the first half, I color by distance, in the second half by azimuth.

dist_azi.mp4

Because I'm a noob, I first ran distortion corrector on a pointcloud not in sensor frame, in chich case it failed silently.
Please add a warning/error when a user attempts to do that 🙇

[vividf]

@mojomex
Thanks for the check! I added a runtime error to notify the user if they set things wrong :)
72f5bb2

[mojomex]

LGTM!

[mojomex]

@vividf Sorry, applying suggestions through GitHub seems to have wrecked DCO: it's failing.
Could you rebase as suggested in that failing check? 🙇