new types: PoseStamped and VelocityStamped

see https://gitlab.com/h2t/organization/software-organization/-/issues/33

source/RobotAPI/components/RobotState/RobotStateComponent.cpp

@@ -38,6 +38,8 @@
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
+
 
 using namespace Eigen;
 using namespace Ice;
@@ -342,21 +344,19 @@ namespace armarx
 
     void
     RobotStateComponent::reportGlobalRobotPose(
-        const std::string& robotName,
-        const Eigen::Matrix4f& pose,
-        const long timestamp,
+        const TransformStamped& globalRobotPose,
         const Ice::Current&)
     {
         if (_synchronized)
         {
             std::string localRobotName = _synchronized->getName();
-            ARMARX_DEBUG << "Comparing " << localRobotName << " and " << robotName << ".";
-            if (localRobotName == robotName)
+            ARMARX_DEBUG << "Comparing " << localRobotName << " and " << globalRobotPose.header.agent << ".";
+            if (localRobotName == globalRobotPose.header.agent)
             {
-                const IceUtil::Time time = IceUtil::Time::microSeconds(timestamp);
+                const IceUtil::Time time = IceUtil::Time::microSeconds(globalRobotPose.header.timestampInMicroSeconds);
 
-                insertPose(time, pose);
-                _synchronized->setGlobalPose(pose);
+                insertPose(time, globalRobotPose.transform);
+                _synchronized->setGlobalPose(globalRobotPose.transform);
 
                 if (_sharedRobotServant)
                 {
@@ -371,49 +371,6 @@ namespace armarx
     }
 
 
-    void RobotStateComponent::reportPlatformPose(const PlatformPose& currentPose, const Current&)
-    {
-        const float z = 0;
-        const Eigen::Vector3f position(currentPose.x, currentPose.y, z);
-        const Eigen::Matrix3f orientation =
-            Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix();
-        const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
-
-        IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
-        insertPose(time, globalPose);
-
-        if (_sharedRobotServant)
-        {
-            _sharedRobotServant->setTimestamp(time);
-        }
-    }
-
-
-    void RobotStateComponent::reportNewTargetPose(
-        Float newPlatformPositionX, Float newPlatformPositionY, Float newPlatformRotation,
-        const Current&)
-    {
-        // Unused.
-        (void) newPlatformPositionX, (void) newPlatformPositionY, (void) newPlatformRotation;
-    }
-
-
-    void RobotStateComponent::reportPlatformVelocity(
-        Float currentPlatformVelocityX, Float currentPlatformVelocityY, Float currentPlatformVelocityRotation,
-        const Current&)
-    {
-        // Unused.
-        (void) currentPlatformVelocityX, (void) currentPlatformVelocityY, (void) currentPlatformVelocityRotation;
-    }
-
-
-    void RobotStateComponent::reportPlatformOdometryPose(Float x, Float y, Float angle, const Current&)
-    {
-        // Unused.
-        (void) x, (void) y, (void) angle;
-    }
-
-
     std::vector<std::string> RobotStateComponent::getArmarXPackages(const Current&) const
     {
         std::vector<std::string> result;