diff --git a/data/RobotAPI/VariantInfo-RobotAPI.xml b/data/RobotAPI/VariantInfo-RobotAPI.xml
index 4d490440cbe8afbb145564d63c638b5235ecf7f2..974673ee5b5457412a18676d0bc06abaaadaf3ba 100644
--- a/data/RobotAPI/VariantInfo-RobotAPI.xml
+++ b/data/RobotAPI/VariantInfo-RobotAPI.xml
@@ -11,6 +11,8 @@
<Variant baseType="::armarx::FramedOrientationBase" dataType="::armarx::FramedOrientation" humanName="FramedOrientation" />
<Variant baseType="::armarx::LinkedPoseBase" dataType="::armarx::LinkedPose" humanName="LinkedPose" />
<Variant baseType="::armarx::LinkedDirectionBase" dataType="::armarx::LinkedDirection" humanName="LinkedDirection" />
+ <Variant baseType="::armarx::OrientedPointBase" dataType="::armarx::OrientedPoint" humanName="OrientedPoint" />
+ <Variant baseType="::armarx::FramedOrientedPointBase" dataType="::armarx::FramedOrientedPoint" humanName="FramedOrientedPoint" />
<Proxy include="RobotAPI/interface/units/KinematicUnitInterface.h"
humanName="Kinematic Unit"
typeName="KinematicUnitInterfacePrx"
@@ -36,7 +38,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/etc/cmake/ArmarXPackageVersion.cmake b/etc/cmake/ArmarXPackageVersion.cmake
index 2f095097a7054c082646d5a58a1f1e3b8807b1c0..3518efcb327dc50993a3eddb2372a2efcc0226a1 100644
--- a/etc/cmake/ArmarXPackageVersion.cmake
+++ b/etc/cmake/ArmarXPackageVersion.cmake
@@ -1,7 +1,7 @@
# armarx version file
set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0")
set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "8")
-set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "1")
+set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "2")
set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}")
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 8f0305101f8aeef91051e05600af0828d80a8ba2..edf26cd26e856103945f568019cf8798335cd5d4 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -76,13 +76,13 @@ void ForceTorqueObserver::onInitObserver()
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
- usingProxy("RobotStateComponent");
+ usingProxy(getProperty<std::string>("RobotStateComponentName").getValue());
offeringTopic("DebugDrawerUpdates");
}
void ForceTorqueObserver::onConnectObserver()
{
-
+ robot = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
if (getProperty<bool>("VisualizeForce").getValue())
{
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 3134ceee1ccb121976a5f5a9a429a960f6d04f86..e291a52e49d65f872febe613e777f97fc3b8c771 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -46,6 +46,8 @@ namespace armarx
defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 30, "Frequency with which the force is visualized");
defineOptionalProperty<float>("ForceVisualizerFactor", 0.01f, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
defineOptionalProperty<float>("MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
+ defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
+
}
};
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..695faec4bfb6012452035bbf522b7b9be3cf0e1e 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);
}
@@ -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);
}
diff --git a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
index 759b4cb477dbeeb79773b0cea1ed8ec93dec9e06..df065c634b00903e407290caee31218b76d64486 100644
--- a/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
+++ b/source/RobotAPI/gui-plugins/SensorActorWidgetsPlugin/ArmarXPlotter/ArmarXPlotter.cpp
@@ -234,7 +234,7 @@ namespace armarx
{
logstream << "Timestamp";
- for (auto& channel : selectedChannels)
+ for (auto & channel : selectedChannels)
{
logstream << "," << channel.toStdString();
}
@@ -740,11 +740,15 @@ namespace armarx
if (logstream.is_open() && dataMaptoAppend.size() > 0)
{
- logstream << (time - logStartTime).toMilliSecondsDouble();
+ logstream << (time - logStartTime).toMilliSecondsDouble() << ",";
- for (const auto& elem : dataMaptoAppend)
+ for (auto& channel : selectedChannels)
{
- logstream << "," /*<< elem.first << ","*/ << elem.second->getOutputValueOnly();
+ logstream << dataMaptoAppend.at(channel.toStdString())->Variant::getOutputValueOnly();
+ if (!selectedChannels.endsWith(channel))
+ {
+ logstream << ",";
+ }
}
logstream << std::endl;
diff --git a/source/RobotAPI/libraries/core/OrientedPoint.h b/source/RobotAPI/libraries/core/OrientedPoint.h
index facef186ba63bc04a06b081a415fe74a15ed20a3..d5964e4992baceff86e8bdf690b2082194b457f4 100644
--- a/source/RobotAPI/libraries/core/OrientedPoint.h
+++ b/source/RobotAPI/libraries/core/OrientedPoint.h
@@ -80,6 +80,8 @@ namespace armarx
virtual void serialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current()) const;
virtual void deserialize(const armarx::ObjectSerializerBasePtr& serializer, const ::Ice::Current& = ::Ice::Current());
};
+
+ typedef IceInternal::Handle<OrientedPoint> OrientedPointPtr;
}
#endif // armarx_core_OrientedPoint