Merge branch 'kitgripper' of https://gitlab.com/ArmarX/RobotAPI into kitgripper

source/RobotAPI/components/units/PlatformUnitObserver.cpp

@@ -86,9 +86,9 @@ void PlatformUnitObserver::onConnectObserver()
 
 }
 
-void PlatformUnitObserver::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
+void PlatformUnitObserver::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
 {
-    nameValueMapToDataFields("platformPose", currentPlatformPositionX, currentPlatformPositionY, currentPlatformRotation);
+    nameValueMapToDataFields("platformPose", currentPose.x, currentPose.y, currentPose.rotationAroundZ);
     updateChannel("platformPose");
 }
 

source/RobotAPI/components/units/PlatformUnitObserver.h

@@ -78,7 +78,7 @@ namespace armarx
         void onConnectObserver() override;
 
         // slice interface implementation
-        void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&) override;

source/RobotAPI/components/units/PlatformUnitSimulation.cpp

@@ -58,7 +58,13 @@ void PlatformUnitSimulation::onInitPlatformUnit()
 
 void PlatformUnitSimulation::onStartPlatformUnit()
 {
-    listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
+    PlatformPose currentPose;
+    // FIXME: Take the timestamp from simulation
+    currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+    currentPose.x = currentPositionX;
+    currentPose.y = currentPositionY;
+    currentPose.rotationAroundZ = currentRotation;
+    listenerPrx->reportPlatformPose(currentPose);
     simulationTask->start();
 }
 
@@ -157,7 +163,13 @@ void PlatformUnitSimulation::simulationFunction()
                 break;
         }
     }
-    listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
+    PlatformPose currentPose;
+    // FIXME: Take the timestamp from simulation
+    currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+    currentPose.x = currentPositionX;
+    currentPose.y = currentPositionY;
+    currentPose.rotationAroundZ = currentRotation;
+    listenerPrx->reportPlatformPose(currentPose);
     listenerPrx->reportPlatformVelocity(vel[0], vel[1], vel[2]);
 }
 

source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp

@@ -50,8 +50,13 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
         {
             const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
             abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
-            const float globR = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
-            listenerPrx->reportPlatformPose(abs(0, 3), abs(1, 3), globR);
+
+            PlatformPose currentPose;
+            currentPose.timestampInMicroSeconds = sc.sensorValuesTimestamp.toMicroSeconds();
+            currentPose.x = abs(0, 3);
+            currentPose.y = abs(1, 3);
+            currentPose.rotationAroundZ = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
+            listenerPrx->reportPlatformPose(currentPose);
         }
         else
         {

source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp

@@ -197,11 +197,11 @@ void PlatformUnitWidget::stopControlTimer()
     rotaCtrl->setNibble({0, 0});
 }
 
-void PlatformUnitWidget::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
+void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
 {
     // moved to qt thread for thread safety
-    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPlatformPositionX), Q_ARG(float, currentPlatformPositionY), Q_ARG(float, currentPlatformRotation));
-    platformRotation = currentPlatformRotation;
+    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPose.x), Q_ARG(float, currentPose.y), Q_ARG(float, currentPose.rotationAroundZ));
+    platformRotation = currentPose.rotationAroundZ;
 }
 
 void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)

source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h

@@ -118,7 +118,7 @@ namespace armarx
         void onExitComponent() override;
 
         // slice interface implementation
-        void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
+        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = Ice::emptyCurrent) override;
         void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;

source/RobotAPI/interface/units/PlatformUnitInterface.ice

@@ -72,6 +72,15 @@ module armarx
 		**/
         void stopPlatform();
     };
+
+    struct PlatformPose
+    {
+        long timestampInMicroSeconds = 0;
+        float x = 0.0f;
+        float y = 0.0f;
+        float rotationAroundZ = 0.0f;
+    };
+
 	/**
 	* Implements an interface to an PlatformUnitListener.
 	**/
@@ -79,11 +88,9 @@ module armarx
     {
 		/**
 		* reportPlatformPose reports current platform pose.
-		* @param currentPlatformPositionX Global x-coordinate of current platform position.
-		* @param currentPlatformPositionY Global y-coordinate of current platform position.
-		* @param currentPlatformRotation Current global orientation of platform.
+        * @param currentPose Current global platform pose.
 		**/
-        void reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation);
+        void reportPlatformPose(PlatformPose currentPose);
         /**
 		* reportNewTargetPose reports target platform pose.
 		* @param newPlatformPositionX Global x-coordinate of target platform position.