added convenience function to forceTorque Observer to retrieve datafieldrefs

source/RobotAPI/components/units/ForceTorqueObserver.cpp

@@ -7,6 +7,7 @@
 #include <Core/observers/checks/ConditionCheckSmaller.h>
 #include <RobotAPI/libraries/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
 #include <RobotAPI/libraries/robotstate/remote/observerfilters/OffsetFilter.h>
+#include <Core/observers/variant/DatafieldRef.h>
 
 #include <RobotAPI/libraries/robotstate/remote/RobotStateObjectFactories.h>
 
@@ -63,46 +64,45 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
 }
 
 
-void ForceTorqueObserver::offerValue(std::string channelName, const std::string &type, FramedVector3BasePtr value)
+void ForceTorqueObserver::offerValue(std::string nodeName, const std::string &type, FramedVector3BasePtr value)
 {
-    if(!existsChannel(channelName))
+    if(!existsChannel(nodeName))
     {
-        offerChannel(channelName, "Torque vectors on specific parts of the robot.");
+        offerChannel(nodeName, "Torque vectors on specific parts of the robot.");
     }
 
-
-
     FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(value);
-    std::string identifier =  type;
-    if(channelName != value->frame)
-    {
-        identifier += "_" + value->frame;
-    }
 
-    if(!existsDataField(channelName, identifier))
+    DataFieldIdentifierPtr id;
+    if(type ==FORCES_STRING)
+        id = getForceDatafieldId(nodeName, vec->frame);
+    else if(type == TORQUES_STRING)
+        id = getTorqueDatafieldId(nodeName, vec->frame);
+    else
+        throw LocalException() << "Unknown sensor type: " << type;
+
+    if(!existsDataField(id->channelName, id->dataFieldName))
     {
-        offerDataFieldWithDefault(channelName, identifier, Variant(value), "3D vector for " + type + " of " + channelName);
+        offerDataFieldWithDefault(id->channelName, id->dataFieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
     }
     else
     {
-        setDataField(channelName, identifier, Variant(value));
+        setDataField(id->channelName, id->dataFieldName, Variant(value));
     }
-    updateChannel(channelName);
-
+    updateChannel(id->channelName);
 
-    channelName += "_pod";
-    if(!existsChannel(channelName) )
+    // pod = plain old data
+    std::string pod_channelName = id->channelName + "_pod";
+    if(!existsChannel(pod_channelName) )
     {
-        offerChannel(channelName, identifier + " on " + channelName);
+        offerChannel(pod_channelName, id->dataFieldName + " on " + nodeName + " as plain old data (pod)");
     }
-    offerOrUpdateDataField(channelName, identifier + "_x", Variant(vec->x), type + " on X axis");
-    offerOrUpdateDataField(channelName, identifier + "_y", Variant(vec->y), type + " on Y axis");
-    offerOrUpdateDataField(channelName, identifier + "_z", Variant(vec->z), type + " on Z axis");
-    offerOrUpdateDataField(channelName, identifier + "_frame", Variant(vec->frame), "Frame of " + value->frame);
+    offerOrUpdateDataField(pod_channelName, id->dataFieldName + "_x", Variant(vec->x), type + " on X axis");
+    offerOrUpdateDataField(pod_channelName, id->dataFieldName + "_y", Variant(vec->y), type + " on Y axis");
+    offerOrUpdateDataField(pod_channelName, id->dataFieldName + "_z", Variant(vec->z), type + " on Z axis");
+    offerOrUpdateDataField(pod_channelName, id->dataFieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
 
-
-
-    updateChannel(channelName);
+    updateChannel(pod_channelName);
 
 }
 
@@ -110,24 +110,15 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string &sensorNo
 {
     ScopedLock lock(dataMutex);
 
-    std::string channelName;
     if(forces)
     {
-        if(forces->frame == sensorNodeName)
-            channelName = forces->frame;
-        else
-            channelName = sensorNodeName + "_" + forces->frame;
-        std::string type = "forces";
-        offerValue(channelName, type, forces);
+        std::string type = FORCES_STRING;
+        offerValue(sensorNodeName, type, forces);
     }
     if(torques)
     {
-        if(torques->frame == sensorNodeName)
-            channelName = torques->frame;
-        else
-            channelName = sensorNodeName + "_" + torques->frame;
-        std::string type = "torques";
-        offerValue(channelName, type, torques);
+        std::string type = TORQUES_STRING;
+        offerValue(sensorNodeName, type, torques);
     }
 }
 
@@ -136,3 +127,44 @@ DatafieldRefBasePtr armarx::ForceTorqueObserver::createNulledDatafield(const Dat
 {
     return createFilteredDatafield(new filters::OffsetFilter(), forceTorqueDatafieldRef);
 }
+
+DatafieldRefBasePtr ForceTorqueObserver::getForceDatafield(const std::string &nodeName, const Ice::Current &)
+{
+    auto id = getForceDatafieldId(nodeName, nodeName);
+    return new DatafieldRef(this, id->channelName, id->dataFieldName);
+
+}
+
+DatafieldRefBasePtr ForceTorqueObserver::getTorqueDatafield(const std::string &nodeName, const Ice::Current &)
+{
+    auto id = getTorqueDatafieldId(nodeName, nodeName);
+    return new DatafieldRef(this, id->channelName, id->dataFieldName);
+}
+
+DataFieldIdentifierPtr ForceTorqueObserver::getForceDatafieldId(const std::string &nodeName, const std::string &frame)
+{
+    std::string channelName;
+    if(frame == nodeName)
+        channelName = nodeName;
+    else
+        channelName = nodeName + "_" + frame;
+    std::string datafieldName = FORCES_STRING;
+    if(!existsChannel(channelName))
+        throw UserException("No sensor for node '" + nodeName + "'available: channel " + channelName);
+    if(!existsDataField(channelName, datafieldName))
+        throw UserException("No sensor for node '" + nodeName + "'available: datafield " + datafieldName);
+    DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
+    return id;
+}
+
+DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::string &nodeName, const std::string &frame)
+{
+    std::string channelName;
+    if(frame == nodeName)
+        channelName = nodeName;
+    else
+        channelName = nodeName + "_" + frame;
+    std::string datafieldName = TORQUES_STRING;
+    DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
+    return id;
+}

source/RobotAPI/components/units/ForceTorqueObserver.h

@@ -43,16 +43,25 @@ namespace armarx
          * @see PropertyUser::createPropertyDefinitions()
          */
         virtual PropertyDefinitionsPtr createPropertyDefinitions();
+
+        const std::string FORCES_STRING = "forces";
+        const std::string TORQUES_STRING = "torques";
     private:
         armarx::Mutex dataMutex;
         std::string topicName;
 
 
-        void offerValue(std::string channelName, const std::string &type, FramedVector3BasePtr value);
+        void offerValue(std::string nodeName, const std::string &type, FramedVector3BasePtr value);
 
         // ForceTorqueUnitObserverInterface interface
     public:
         DatafieldRefBasePtr createNulledDatafield(const DatafieldRefBasePtr &forceTorqueDatafieldRef, const Ice::Current &);
+
+        DatafieldRefBasePtr getForceDatafield(const std::string &nodeName, const Ice::Current &);
+        DatafieldRefBasePtr getTorqueDatafield(const std::string &nodeName, const Ice::Current &);
+
+        DataFieldIdentifierPtr getForceDatafieldId(const std::string &nodeName, const std::string &frame);
+        DataFieldIdentifierPtr getTorqueDatafieldId(const std::string &nodeName, const std::string &frame);
     };
 }
 

source/RobotAPI/interface/units/ForceTorqueUnit.ice

@@ -53,6 +53,9 @@ module armarx
 
     interface ForceTorqueUnitObserverInterface extends ObserverInterface, ForceTorqueUnitListener
     {
+        DatafieldRefBase getForceDatafield(string nodeName) throws UserException;
+        DatafieldRefBase getTorqueDatafield(string nodeName) throws UserException;
+
         DatafieldRefBase createNulledDatafield(DatafieldRefBase forceTorqueDatafieldRef);
     };
 };