Driver is not currently compatible with ROS2 SLAM Toolbox -- 70% of scans are discarded

[DrewSBAI]

[updating this issue from a separate repo]

Describe the bug
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.
Regardless of whether the LD19 hardware provides a variable number of range values in a single scan (which it does), this LD19 driver needs a param that forces the driver to always publish the same number of readings in every scan. It also needs to not repeat the ranges at 0 degrees and 360 degrees (which are the same thing). The output range should be [0,360) not [0,360].

To Reproduce

• Run this repo's driver on ROS2 Humble

• 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)

• the LD19's laserscan topic will provide a solid 10Hz output (USB or serial UART doesn't affect this problem)

• 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.

• 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

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.
SteveMacenski/slam_toolbox#293 (comment)
SteveMacenski/slam_toolbox#430 (comment)
SteveMacenski/slam_toolbox#141 (comment)
SteveMacenski/slam_toolbox#426 (comment)
SteveMacenski/slam_toolbox#328 (comment)
SteveMacenski/slam_toolbox#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

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

[yorobot256]

Some of you may be interested in a solution for this error.

I updated thanks to chatgpt this function to allways return a fixed number of points from LD06 lidar.

bool LiPkg::GetLaserScanData(Points2D& out) {
if (IsFrameReady()) {
ResetFrameReady();
{
std::lock_guardstd::mutex lg(mutex_lock2_);
out = laser_scan_data_;
}

// Truncate or fill with NaN values
if (out.size() < 456) {
const double nan_value = std::numeric_limits::quiet_NaN();
size_t current_size = out.size();
out.resize(456, PointData(nan_value, nan_value, nan_value, 0));
// Set timestamps for the NaN values to 0
for (size_t i = current_size; i < 456; ++i) {
out[i].stamp = 0;
}
} else if (out.size() > 456) {
out.resize(456); // Truncate to 456 points
}

return true;
} else {
return false;
}
}

Now I am able to generate a map with slam_toolbox.
I am still working on finetuning the robot to improve the quality of the map. But LD06 seems to be usable with slam_toolbox

[DrewSBAI]

@yorobot256 @sskorol
I agree that this will work and stop SLAM Toolbox from complaining about too few or too many readings in the scans. However, I'm not sure we should hard code the range to 456 for all LD19 units in the wild. I would imagine manufacturing variations in the LD19's drive motor and PWM control cause these units to return a variable number of readings per scan (which is the root cause of this entire issue in the first place). For instance, if you start up the LD19 ten times in a row on ten different LD19, the length of the first reading is going to be variable (I think I saw anywhere from 450 to 465)?
Another way to implement this would be to (on init), listen for the first reading in the 450-475 range and then lock each subsequent scan to that length. For instance, on init, you might get scan lengths of 0, 0, 120, 459. You'd drop the first 3 scans and then lock all future scans to 459.

[sskorol]

@DrewSBAI this number would be different even for a single device if you re-run it 10 times. For me, slam-toolbox mostly every single run prints a new number in ~447-453 range.
And expected value on slam-toolbox side is different as well, so it seems like it locks the first reading as well. So this init you've mentioned should probably be smart enough to do not send these first scans before locking some value.

[DrewSBAI]

Yes, SLAM toolbox definitely locks on the first scan length since it expects a fixed number of readings, period.
And, yes, I've found that my LD19's will, after a few scans, settle on a range of +/- 1 or 2 readings for the duration of the session. So, for example the returns from the LD19 might be of length 0 readings, 0 readings, 200 readings, 459, 460, 460, 460, 459, 460, 459, 459, 461, 460, 459.

[erer2001]

@DrewSBAI Have you found any alternative to SLAM toolbox that doesn't have this issue?

[DrewSBAI]

No, but SLAM Toolbox is, in my opinion, the go-to standard for SLAM in ROS2 projects, so I've focused on just getting it to work. Per above, you can easily make it work with the LD19 by fixing the length of the scan readings.

[bperseghetti]

For those only following this issue instead of #4:

@ErliPan You can now set binning as a param and it will upscale or downscale to what you want. Also allows for setting area mask so you can mask out returns for transient events (like an elevated magnetometer cable in FOV) works great in Nav2 and reduces CPU load a ton.
Here is the PR: #14

[rbretmounet]

@bperseghetti What is the typical number of bins you should be using to fix the issue?

[bperseghetti]

@bperseghetti What is the typical number of bins you should be using to fix the issue?

@rbretmounet, I guess it depends in part on your specific lidar unit and your compute, however, slam_toolbox only uses a subset if I remember correctly. We for instance are running a low power resource constrained ARM64 5W SoC and we get really good localization and mapping by binning it all the way down to 72 (5 degrees per bin). I would suggest some solid divisor or multiple of 360. IE 720 (if running the LD27L), 360, 180, 120, 90, 72, we noticed degradation issues when going down to 60 in localization and map creation.

[rbretmounet]

@bperseghetti What is the typical number of bins you should be using to fix the issue?

@rbretmounet, I guess it depends in part on your specific lidar unit and your compute, however, slam_toolbox only uses a subset if I remember correctly. We for instance are running a low power resource constrained ARM64 5W SoC and we get really good localization and mapping by binning it all the way down to 72 (5 degrees per bin). I would suggest some solid divisor or multiple of 360. IE 720 (if running the LD27L), 360, 180, 120, 90, 72, we noticed degradation issues when going down to 60 in localization and map creation.

Thanks for the reply @bperseghetti I am using an FHL-LD19P Lidar