diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp index 3c9cde6b67a1106976beee9032795262755a4be3..6abaa676025df57bbfbbde4f61ad147ac54e97eb 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)