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.

[knzo25]

Just small change requests

[vividf]

This PR should be merged together with

• chore(aip_launcher_parameter): add distortion corrector param tier4/aip_launcher#318
• chore(common_sensor_launch): add distortion corrector parameter sample_sensor_kit_launch#105
• aw_sim: feat(common_awsim_sensor_launch): distortion corrector add new param tier4/awsim_sensor_kit_launch#20
• aw_sim_lab: feat(common_awsim_labs_sensor_launch): distortion corrector add new param awsim_labs_sensor_kit_launch#7

[vividf]

@knzo25 kindly ping

[knzo25]

LGTM 🎉

[vividf]

@TakaHoribe @soblin @takayuki5168
Seems that this PR need your approval since it include not only pointcloud preprocessor package :)
Thanks