Hokuyo: Added angle offset

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.cpp

@@ -35,6 +35,7 @@ void HokuyoLaserUnit::onInitComponent()
     topicName = getProperty<std::string>("LaserScannerTopicName").getValue();
     offeringTopic(topicName);
     updatePeriod = getProperty<int>("UpdatePeriod").getValue();
+    angleOffset = getProperty<float>("AngleOffset").getValue();
 
     std::string deviceStrings = getProperty<std::string>("Devices").getValue();
     std::vector<std::string> splitDeviceStrings;
@@ -147,7 +148,7 @@ void HokuyoLaserUnit::updateScanData()
             for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
             {
                 LaserScanStep step;
-                step.angle = float(0.25 * M_PI / 180.0 * stepIndex);
+                step.angle = angleOffset + float(0.25 * M_PI / 180.0 * stepIndex);
                 step.distance = lengthDataSize * stepIndex / 100.0f + 200.0f;
                 scan.push_back(step);
             }
@@ -179,8 +180,7 @@ void HokuyoLaserUnit::updateScanData()
             for (int stepIndex = 0; stepIndex < lengthDataSize; ++stepIndex)
             {
                 LaserScanStep step;
-                // TODO: We need an angle offset for the sensor
-                step.angle = (float)urg_step2rad(&device.urg, stepIndex); // Convert steps to rad
+                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);
             }

source/RobotAPI/drivers/HokuyoLaserUnit/HokuyoLaserUnit.h

@@ -46,6 +46,7 @@ namespace armarx
         {
             defineOptionalProperty<std::string>("LaserScannerTopicName", "LaserScans", "Name of the laser scan topic.");
             defineOptionalProperty<int>("UpdatePeriod", 25, "Update period for laser scans");
+            defineOptionalProperty<float>("AngleOffset", -2.3561944902, "Offset is applied the raw angles before reporting them");
             defineOptionalProperty<std::string>("Devices", "", "List of devices in form of 'IP1,port1;IP2,port2;...'");
         }
     };
@@ -117,6 +118,7 @@ namespace armarx
         std::string topicName;
         LaserScannerUnitListenerPrx topic;
         int updatePeriod = 25;
+        float angleOffset = 0.0f;
         std::vector<HokuyoLaserScanDevice> devices;
         PeriodicTask<HokuyoLaserUnit>::pointer_type task;