From 66d417e001d0ab9ccd9e9779e6797dc3f05670bb Mon Sep 17 00:00:00 2001
From: armar-user <armar-user@h2t.kit.edu>
Date: Fri, 4 Feb 2022 15:32:49 +0100
Subject: [PATCH] Use up-to-date implementation
---
.../components/RobotState/RobotStateComponent.cpp | 15 ++++++++++++++-
1 file changed, 14 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 374fae4c4..19d3e19df 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -214,7 +214,6 @@ namespace armarx
{
throw NotInitializedException("Shared Robot is NULL", "getSynchronizedRobot");
}
-
return this->_synchronizedPrx;
}
@@ -357,6 +356,7 @@ namespace armarx
insertPose(time, globalRobotPose.transform);
_synchronized->setGlobalPose(globalRobotPose.transform);
+ _sharedRobotServant->setGlobalPose(new Pose(globalRobotPose.transform));
if (_sharedRobotServant)
{
@@ -703,12 +703,25 @@ namespace armarx
const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+
+ TransformStamped stamped;
+ stamped.header.frame = armarx::GlobalFrame;
+ stamped.header.agent = _synchronized->getName();
+ stamped.header.timestampInMicroSeconds = time.toMicroSeconds();
+ stamped.header.parentFrame = "";
+ stamped.transform = globalPose;
+
+ this->reportGlobalRobotPose(stamped);
+
+ /*
+ * old:
insertPose(time, globalPose);
if (_sharedRobotServant)
{
_sharedRobotServant->setTimestamp(time);
}
+ */
}
--
GitLab