Driver Fix for LD19 scan length

[Chambana]

Describe the bug A clear and concise description of what the bug is.
The LD19 driver apparently calculates the number of readings in a scan in a non-conformal way and is out of spec with what the ROS2 SLAM Toolbox expects. The maintainer of SLAM toolbox has asked maintainers of LIDAR drivers to fix the calculation on their end. With the current LD19 driver, the SLAM Toolbox will discard around 70% of scans from the LD19 (see graphic below) and is largely unusable.

To Reproduce Steps to reproduce the behavior:

1. Run this repo's driver on ROS2 Humble
2. Run the SLAM toolbox on ROS2 Humble (you'll need a a robot model publisher and source of odom-base_link TF to start the toolbox)
3. the LD19's laserscan topic will provide a solid 10Hz output (USB or serial UART doesn't affect this problem)
4. the SLAM toolbox discards 60-70% of all scans because the SLAM toolbox is expecting a certain number of readings per scan (which appears to be set at SLAM Toolbox initialization) and the scan length provided by the LD19 driver is consistently off by 1 reading.
5. The issue is masked to the user because the scan topic looks great and comes in at 10Hz. Also, the SLAM map does get created (because it's still receiving some scans) but it's hugely inefficient due to the toolbox dropping 70% of scans.

Expected behavior A clear and concise description of what you expected to happen. These warnings in SLAM Toolbox should be pretty rare and only occur when the LIDAR scan speed doesn't hit its timing, but they're currently alerting on 70% of all scans [async_slam_toolbox_node-1] LaserRangeScan contains 452 range readings, expected 451 [async_slam_toolbox_node-1] LaserRangeScan contains 452 range readings, expected 451 [async_slam_toolbox_node-1] LaserRangeScan contains 452 range readings, expected 451 [async_slam_toolbox_node-1] LaserRangeScan contains 452 range readings, expected 451 [async_slam_toolbox_node-1] LaserRangeScan contains 452 range readings, expected 451 [async_slam_toolbox_node-1] LaserRangeScan contains 450 range readings, expected 451 [async_slam_toolbox_node-1] LaserRangeScan contains 452 range readings, expected 451

Screenshots If applicable, add screenshots to help explain your problem. SLAM toolbox discarding 70% of scans

Scans coming in at 10Hz Screencast from 08-18-2023 12:00:52 PM.webm

Desktop (please complete the following information):

• OS: Ubuntu 22.04
• ROS2 version: Humble
• Device type: x86-64 (SLAM toolbox), ARM (LD19 driver)

Additional context Add any other context about the problem here. https://github.com/SteveMacenski/slam_toolbox/issues/293#issuecomment-696296457 https://github.com/SteveMacenski/slam_toolbox/issues/430#issuecomment-905100814 https://github.com/SteveMacenski/slam_toolbox/issues/141#issuecomment-564670621 https://github.com/SteveMacenski/slam_toolbox/issues/426#issuecomment-882782222 https://github.com/SteveMacenski/slam_toolbox/issues/328#issuecomment-779367098 https://github.com/SteveMacenski/slam_toolbox/pull/145

Easiest fix is to probably alter the LD19 driver angle min / max so that it conforms to the SLAM Toolbox's format and triggers the GetIs360Laser() check here: https://github.com/SteveMacenski/slam_toolbox/blob/912703c43b7a640303b1b5fc62f05d53fae9cf57/lib/karto_sdk/include/karto_sdk/Karto.h#L4302C11-L4302C11

[Myzhar]

This is a sneaky problem. The LDLidar driver calculates the number of beams for each published topic and it's always correct, instead the code of Karto in slam toolbox seems that it does not update the reference value for each laser scan and so it outputs an error each time the value is different from what expected.

I modified the function LiPkg::ToLaserscan to match the same function used in Karto but the problem is still present.

For example:

[ldlidar_node-5] Beam count = 447
[ldlidar_node-5]  - angle_min: 0
[ldlidar_node-5]  - angle_max: 6.28318
[ldlidar_node-5]  - angle_increment: 0.014079
[async_slam_toolbox_node-2] LaserRangeScan contains 447 range readings, expected 450

447 is the correct value, not 450. This means that 450 is a not updated value calculated on the parameters of the received scan message

[Myzhar]

Fixed with the latest update