optimized force torque observer performance

source/RobotAPI/components/units/ForceTorqueObserver.cpp

@@ -125,7 +125,7 @@ void ForceTorqueObserver::visualizerFunction()
         float forceFactor = getProperty<float>("ForceVisualizerFactor").getValue();
         RemoteRobot::synchronizeLocalClone(localRobot, robot);
         auto channels = getAvailableChannels(false);
-        for (auto& channel : channels)
+        for (auto & channel : channels)
         {
             if (localRobot->hasRobotNode(channel.first))
             {
@@ -164,38 +164,61 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
 }
 
 
-void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
+void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
 {
     FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
 
 
 
-    if (!existsDataField(id->channelName, id->datafieldName))
+
+    try
     {
-        if (!existsChannel(id->channelName))
-        {
-            offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
-        }
-        offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+        setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
     }
-    else
+    catch (...)
     {
-        setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
+        ARMARX_INFO << "Creating force/torque fields";
+        if (!existsDataField(id->channelName, id->datafieldName))
+        {
+            if (!existsChannel(id->channelName))
+            {
+                offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+            }
+            offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+        }
     }
 
 
     // pod = plain old data
     std::string pod_channelName = id->channelName + POD_SUFFIX;
 
-    if (!existsChannel(pod_channelName))
+
+
+    try
+    {
+        StringVariantBaseMap values;
+        values[id->datafieldName + "_x"    ] = new Variant(vec->x);
+        values[id->datafieldName + "_y"    ] = new Variant(vec->y);
+        values[id->datafieldName + "_z"    ] = new Variant(vec->z);
+        values[id->datafieldName + "_frame"] = new Variant(vec->frame);
+        setDataFieldsFlatCopy(pod_channelName, values);
+    }
+    catch (...)
     {
-        offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+        ARMARX_INFO << "Creating force/torque pod fields";
+        if (!existsChannel(pod_channelName))
+        {
+            offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+
+        }
+        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);
     }
 
-    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);
+
+
 }
 
 void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&)
@@ -223,7 +246,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
         updateChannel(id->channelName);
         updateChannel(id->channelName + POD_SUFFIX);
     }
-    catch (std::exception& e)
+    catch (std::exception&)
     {
         ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
         handleExceptions();

source/RobotAPI/components/units/ForceTorqueObserver.h

@@ -97,7 +97,7 @@ namespace armarx
         DebugDrawerInterfacePrx debugDrawer;
         PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
 
-        void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
+        void offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id);
 
         // ForceTorqueUnitObserverInterface interface
     public: