ForceTorqueObserver: Fixed frames

source/RobotAPI/components/units/ForceTorqueObserver.cpp

@@ -305,7 +305,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
 {
     ScopedLock lock(dataMutex);
 
-    auto offerForceAndTorqueValue = [ = ](std::string const & sensorNodeName, std::string const & frame, std::string channelName)
+    auto offerForceAndTorqueValue = [ = ](std::string const & sensorNodeName, std::string const & frame, std::string channelName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques)
     {
         try
         {
@@ -358,9 +358,9 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
 
     FramedDirectionPtr realTorques = FramedDirectionPtr::dynamicCast(torques);
     realTorques->changeFrame(localRobot, forces->frame);
-    offerForceAndTorqueValue(sensorNodeName, forces->frame, "");
 
     FramedDirectionPtr realForces = FramedDirectionPtr::dynamicCast(forces);
+    offerForceAndTorqueValue(sensorNodeName, forces->frame, "", realForces, realTorques);
 
     auto sensorMappingRange = sensorRobotNodeMapping.equal_range(sensorNodeName);
     for (auto iter = sensorMappingRange.first; iter != sensorMappingRange.second; ++iter)
@@ -368,11 +368,12 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
         auto& sensorName = iter->first;
         auto& robotNodeName = iter->second.first;
         auto& channelName = iter->second.second;
+        FramedDirectionPtr forcesCopy = FramedDirectionPtr::dynamicCast(realForces->clone());
+        FramedDirectionPtr torquesCopy = FramedDirectionPtr::dynamicCast(realTorques->clone());
+        forcesCopy->changeFrame(localRobot, robotNodeName);
+        torquesCopy->changeFrame(localRobot, robotNodeName);
 
-        realTorques->changeFrame(localRobot, robotNodeName);
-        realForces->changeFrame(localRobot, robotNodeName);
-
-        offerForceAndTorqueValue(sensorName, robotNodeName, channelName);
+        offerForceAndTorqueValue(sensorName, robotNodeName, channelName, forcesCopy, torquesCopy);
     }
 }