Skip to content

Commit

Permalink
Push NAN into point array for measurement with range < range_min
Browse files Browse the repository at this point in the history
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.
  • Loading branch information
981213 committed Nov 22, 2022
1 parent 73afa48 commit f9e64d3
Showing 1 changed file with 4 additions and 1 deletion.
5 changes: 4 additions & 1 deletion lib/karto_sdk/include/karto_sdk/Karto.h
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
m_UnfilteredPointReadings.push_back(Vector2<kt_double>(NAN, NAN));
continue;
} else if (rangeReading > rangeThreshold) {
kt_double angle = scanPose.GetHeading() + minimumAngle + beamNum * angularResolution;

Vector2<kt_double> point;
Expand Down

0 comments on commit f9e64d3

Please sign in to comment.