Hokuyo: Add plausability checks for distance values

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp

@@ -180,9 +180,14 @@ void HokuyoLaserUnit::updateScanData()
             for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
             {
                 LaserScanStep step;
-                step.angle = angleOffset + (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
-                step.distance = (float)device.lengthData[stepIndex]; // Data is already in mm
-                scan.push_back(step);
+                long distance = device.lengthData[stepIndex];
+                // TODO: Extract the min and max valid value for distance into parameters
+                if (distance >= 21 && distance <= 30000)
+                {
+                    step.angle = angleOffset + (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
+                    step.distance = (float)distance; // Data is already in mm
+                    scan.push_back(step);
+                }
             }
 
             // TODO: Better names for the devices?