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

ignore measurements outside lidar range #544

Open
wants to merge 1 commit into
base: ros2
Choose a base branch
from

Conversation

981213
Copy link
Contributor

@981213 981213 commented Oct 25, 2022

According to the comment in sensor_msgs/LaserScan, values < range_min or > range_max should be discarded.
Without this, scan matching will fail for lidars publishing 0 for invalid measurements.


Basic Info

Info Please fill out this column
Primary OS tested on Ubuntu 22.04 / ROS2 Humble
Robotic platform tested on My DIY robot with LeiShen Lidar

Description of contribution in a few bullet points

  • Don't add a point to UnfilteredPoingReadings when the range reading is outside lidar range.


m_UnfilteredPointReadings.push_back(point);
if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(),
pLaserRangeFinder->GetMaximumRange()))
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

m_pRangeThreshold->SetValue(math::Clip(rangeThreshold, GetMinimumRange(), GetMaximumRange()));

The rangeThreshold is always less than GetMaximumRange so this is already true. You can revert these changes

lib/karto_sdk/include/karto_sdk/Karto.h Outdated Show resolved Hide resolved
@@ -5658,7 +5658,10 @@ class LocalizedRangeScan : public LaserRangeScan
kt_int32u beamNum = 0;
for (kt_int32u i = 0; i < pLaserRangeFinder->GetNumberOfRangeReadings(); i++, beamNum++) {
kt_double rangeReading = GetRangeReadings()[i];
if (!math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold)) {
if (rangeReading < pLaserRangeFinder->GetMinimumRange()) {
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not using that InRange line that was there before math::InRange(rangeReading, pLaserRangeFinder->GetMinimumRange(), rangeThreshold)? This seems to remove the far-check

@981213 981213 marked this pull request as draft April 4, 2023 05:31
According to the comment in sensor_msgs/LaserScan, values
< range_min or > range_max should be discarded.
Without this, scan matching will fail for lidars publishing
0 for invalid measurements.
@981213
Copy link
Contributor Author

981213 commented Apr 4, 2023

I moved the filtration outside karto_sdk in the latest iteration. The scan matching is fixed in both the old and this version, but loop closure still doesn't seem right with my half-blocked lidar.

I placed a second lidar on top of the half-blocked one to do some comparison. Here's my robot setup:
Robot Setup
With async slam node, the top lidar and all the default configuration, it works perfectly:
Result with the top LSN10 lidar
But here's the result using the bottom one:
Screenshot_20230404_145322
I wrote the lidar driver myself: https://github.com/981213/ls01_ros2 and the blocked measurements are set to 0 with memset in my code.

It would be great if someone can give me some suggestions on debugging.
Here's the ros2 bag for the test above.
rosbag2_2023_04_04-14_16_58.zip

/scan is the half-blocked lidar and /scan2 is the one on top.
The missing lidar transform:

ros2 run tf2_ros static_transform_publisher 0.03 0.0 0.1415 0 0 0 base_footprint laser_link

@981213 981213 marked this pull request as ready for review April 4, 2023 07:20
@981213 981213 marked this pull request as draft April 5, 2023 06:12
@981213
Copy link
Contributor Author

981213 commented Apr 5, 2023

Fixed with the latest commit, but I'm not sure if it's the correct fix because I haven't actually understood how ScanMatcher works yet.

edit: dropped this. I should simply lower the threshold in config file instead.

@981213 981213 marked this pull request as ready for review April 5, 2023 11:20
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants