From d865b5f2fa62366bc8e3efbe8f1721297de2edcf Mon Sep 17 00:00:00 2001
From: Fabian Paus <fabian.paus@kit.edu>
Date: Wed, 8 Mar 2017 10:10:04 +0100
Subject: [PATCH] Hokuyo: Add plausability checks for distance values
---
.../drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp | 11 ++++++++---
1 file changed, 8 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp b/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
index b05638bbd..ac411a211 100644
--- a/source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp
+++ b/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?
--
GitLab