From 049ea8ba8fbbe7a96a66ea129ce0410ebdfbce2a Mon Sep 17 00:00:00 2001
From: Lukas Kaul <lukas.s.kaul@gmail.com>
Date: Thu, 25 Feb 2016 18:09:23 +0100
Subject: [PATCH] Fixed IMU Observer
---
data/RobotAPI/VariantInfo-RobotAPI.xml | 2 +-
.../units/InertialMeasurementUnitObserver.cpp | 34 +++++++++----------
.../units/InertialMeasurementUnitObserver.h | 2 ++
source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp | 2 +-
4 files changed, 21 insertions(+), 19 deletions(-)
diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 4d490440c..4bfc6fae1 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/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"
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index 8feff3d67..1d290b00b 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/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()
{
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 26bee065b..df09d690d 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/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);
};
}
diff --git a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp b/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
index 964e38661..d8178eb8d 100644
--- a/source/RobotAPI/drivers/XsensIMU/XsensIMU.cpp
+++ b/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);
}
--
GitLab