From fe16fd14b697e03d46c5ab18727257b9cf060fad Mon Sep 17 00:00:00 2001
From: armar4-user <armar4-user@kit.edu>
Date: Mon, 20 Mar 2017 12:57:31 +0100
Subject: [PATCH] ForceTorqueObserver: Fixed frames
---
.../components/units/ForceTorqueObserver.cpp | 13 +++++++------
1 file changed, 7 insertions(+), 6 deletions(-)
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 3da920390..452f4c350 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/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);
}
}
--
GitLab