diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml index 4d490440cbe8afbb145564d63c638b5235ecf7f2..4bfc6fae104cebea105451a29bb45374dcd7c856 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 8feff3d67002613b2b5269b3671182a5aed60bc1..1d290b00b66da954bd48037b5837a7c0ecd5323d 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 26bee065be5154cdc9bfb6ecd4f67dee3040efa2..df09d690da2490eb85a7cb5c701fa7f6cf6403a4 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 964e38661512f8ccd5448d993d759312855aafe4..d8178eb8d32e5d56db304fab1c762296a9942f8f 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); }