temporary fix: scan min and max angles hard coded; returning distance in mm

source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp

@@ -47,6 +47,8 @@
 *
 */
 
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include <VirtualRobot/MathTools.h>
 #ifdef _MSC_VER
 #pragma warning(disable: 4996)
 #pragma warning(disable: 4267)
@@ -78,10 +80,10 @@
 #include <boost/lexical_cast.hpp>
 #include <vector>
 
-#ifndef rad2deg
-#define rad2deg(x) ((x) / M_PI * 180.0)
-#endif
-#define deg2rad_const (0.017453292519943295769236907684886f)
+// #ifndef rad2deg
+// #define rad2deg(x) ((x) / M_PI * 180.0)
+// #endif
+// #define deg2rad_const (0.017453292519943295769236907684886f)
 
 
 //std::vector<unsigned char> exampleData(65536);
@@ -406,7 +408,7 @@ namespace armarx
                     {
                         unsigned short iRange;
                         sscanf(fields[offset + i], "%hx", &iRange);
-                        float range = iRange / 1000.0;
+                        float range = iRange; // / 1000.0;
                         distVal.push_back(range);
                     }
                     else
@@ -429,11 +431,23 @@ namespace armarx
             ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
             return sick_scan::ExitError;
         }
+
+        // FIXME scan info is not set atm.
+        scanInfo.minAngle = VirtualRobot::MathTools::deg2rad(-45);
+        scanInfo.maxAngle = VirtualRobot::MathTools::deg2rad(225);
+        scanInfo.stepSize = VirtualRobot::MathTools::deg2rad(0.33);
+
+        ARMARX_VERBOSE << "Min/max angle: " << VirtualRobot::MathTools::rad2deg(scanInfo.minAngle) << ", " << VirtualRobot::MathTools::rad2deg(scanInfo.maxAngle) << "";
+
         scanData.reserve(distVal.size());
         for (int i = 0; i < (int) distVal.size(); i++)
         {
             LaserScanStep step;
-            step.angle = i * scanInfo.stepSize;
+            step.angle = i * scanInfo.stepSize + scanInfo.minAngle;
+            // step.angle = scanInfo.maxAngle - i * scanInfo.stepSize;
+            ARMARX_CHECK_LESS_EQUAL(step.angle, scanInfo.maxAngle);
+            ARMARX_CHECK_GREATER_EQUAL(step.angle, scanInfo.minAngle);
+
             step.distance = distVal[i];
             //step.intensity = intensityVal[i];
             scanData.push_back(step);