Merge branch 'IMUFix'

data/RobotAPI/VariantInfo-RobotAPI.xml

@@ -36,7 +36,7 @@
         <Proxy include="RobotAPI/interface/units/InertialMeasurementUnit.h"
             humanName="Inertial Measurement Unit Observer"
             typeName="InertialMeasurementUnitObserverInterfacePrx"
-            memberName="inertialMeasurementObserverUnit"
+            memberName="inertialMeasurementUnitObserver"
             getterName="getIMUObserver"
             propertyName="IMUObserverName"
             propertyIsOptional="true"

source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp

@@ -55,25 +55,16 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string& devi
     if (!existsChannel(device))
     {
         offerChannel(device, "IMU data");
-        \
-        //todo remove
-        offerDataFieldWithDefault(device, "name", Variant(name), "Name of the IMU sensor");
-        offerDataFieldWithDefault(device, "acceleration", acceleration,  "acceleration values");
-        offerDataFieldWithDefault(device, "gyroscopeRotation", gyroscopeRotation,  "gyroscope rotation values");
-        offerDataFieldWithDefault(device, "magneticRotation", magneticRotation,  "magnetic rotation values");
-        offerDataFieldWithDefault(device, "orientationQuaternion", orientationQuaternion,  "orientation quaternion values");
-        offerDataFieldWithDefault(device, "timestamp", timestampPtr, "Timestamp");
-    }
-    else
-    {
-        setDataField(device, "name", Variant(name));
-        setDataField(device, "acceleration", acceleration);
-        setDataField(device, "gyroscopeRotation", gyroscopeRotation);
-        setDataField(device, "magneticRotation", magneticRotation);
-        setDataField(device, "orientationQuaternion", orientationQuaternion);
-        setDataField(device, "timestamp", timestampPtr);
     }
 
+    offerOrUpdateDataField(device, "name", Variant(name), "Name of the IMU sensor");
+    offerValue(device, "acceleration", acceleration);
+    offerValue(device, "gyroscopeRotation", gyroscopeRotation);
+    offerValue(device, "magneticRotation", magneticRotation);
+    offerValue(device, "acceleration", acceleration);
+    offerOrUpdateDataField(device, "orientationQuaternion", orientationQuaternion,  "orientation quaternion values");
+    offerOrUpdateDataField(device, "timestamp", timestampPtr, "Timestamp");
+
     updateChannel(device);
 
     Eigen::Vector3f zero;
@@ -92,6 +83,15 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string& devi
     debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
 }
 
+void InertialMeasurementUnitObserver::offerValue(std::string device, std::string fieldName, Vector3Ptr vec)
+{
+    offerOrUpdateDataField(device, fieldName, vec, fieldName + " values");
+    offerOrUpdateDataField(device, fieldName + "_x", vec->x, fieldName + "_x value");
+    offerOrUpdateDataField(device, fieldName + "_y", vec->y, fieldName + "_y value");
+    offerOrUpdateDataField(device, fieldName + "_z", vec->z, fieldName + "_z value");
+
+}
+
 
 PropertyDefinitionsPtr InertialMeasurementUnitObserver::createPropertyDefinitions()
 {

source/RobotAPI/components/units/InertialMeasurementUnitObserver.h

@@ -26,6 +26,7 @@
 #include <RobotAPI/interface/units/InertialMeasurementUnit.h>
 #include <ArmarXCore/observers/Observer.h>
 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
+#include <RobotAPI/libraries/core/Pose.h>
 
 
 namespace armarx
@@ -83,6 +84,7 @@ namespace armarx
         DebugDrawerInterfacePrx debugDrawerPrx;
 
 
+        void offerValue(std::string device, std::string fieldName, Vector3Ptr vec);
     };
 }
 

source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp

@@ -65,7 +65,7 @@ void XsensIMU::frameAcquisitionTaskLoop()
             data.orientationQuaternion.push_back(m_OrientationQuaternion[2]);
             data.orientationQuaternion.push_back(m_OrientationQuaternion[3]);
 
-            IMUTopicPrx->reportSensorValues("device", "name", data, now);
+            IMUTopicPrx->reportSensorValues("XsensIMU", "XsensIMU", data, now);
 
         }
 
@@ -99,7 +99,7 @@ void XsensIMU::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice
     data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[2]);
     data.orientationQuaternion.push_back(CurrentState.m_PhysicalData.m_OrientationQuaternion[3]);
 
-    IMUTopicPrx->reportSensorValues("device", pIMUDevice->GetDeviceId(), data, now);
+    IMUTopicPrx->reportSensorValues("XsensIMU", pIMUDevice->GetDeviceId(), data, now);
 
 }