From 6133d110c3cd6c0b1c6408b181589b3c44f64435 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Nov 2022 15:53:46 +0100
Subject: [PATCH] platformunitsimulation: publishing on new topic
---
.../units/PlatformUnitSimulation.cpp | 18 +++++++++++-------
1 file changed, 11 insertions(+), 7 deletions(-)
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 3c9cde6b6..6abaa6760 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -103,8 +103,6 @@ namespace armarx
globalPosePrx->reportGlobalRobotPose(currentPose);
- // legacy
- listenerPrx->reportPlatformPose(toPlatformPose(currentPose));
simulationTask->start();
}
@@ -229,8 +227,17 @@ namespace armarx
odometryPrx->reportOdometryPose(platformPose);
odometryPrx->reportOdometryVelocity(platformVelocity);
- // legacy
- listenerPrx->reportPlatformPose(toPlatformPose(platformPose));
+ {
+ TransformStamped currentPose;
+ currentPose.header.parentFrame = GlobalFrame;
+ currentPose.header.frame = robotRootFrame;
+ currentPose.header.agent = agentName;
+ currentPose.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose.transform = currentPlatformPose();
+
+ globalPosePrx->reportGlobalRobotPose(currentPose);
+ }
+
// legacy
const auto& velo = platformVelocity.twist;
@@ -259,9 +266,6 @@ namespace armarx
TransformStamped targetPose;
targetPose.header = header;
targetPose.transform = VirtualRobot::MathTools::posrpy2eigen4f(targetPositionX, targetPositionY, 0, 0, 0, targetRotation);
-
- const auto tp = toPlatformPose(targetPose);
- listenerPrx->reportNewTargetPose(tp.x, tp.y, tp.rotationAroundZ);
}
void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
--
GitLab