diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.cpp b/source/RobotAPI/components/units/PlatformUnitObserver.cpp
index 9194408d35ea5596d18cf49a5dbe0f0c0aa85ab7..73661c4ceaacef34826d3a6e6ad22697b7ed628f 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.cpp
+++ b/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");
}
diff --git a/source/RobotAPI/components/units/PlatformUnitObserver.h b/source/RobotAPI/components/units/PlatformUnitObserver.h
index 412b9c57f942a48789c833773c01c5f9d6927331..a79ee07926621bf333aa9b4775fab3e1ae76d307 100644
--- a/source/RobotAPI/components/units/PlatformUnitObserver.h
+++ b/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;
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 75fcbca37ad130dd9e19c9455ad10dbe6d166581..b526cff934941abee0918d5797838bc60380df17 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/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]);
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 48258990833084dbd0fee2a25240a5b71f9cb61a..1201da179c7ce4770ae7afffd0230c180b466b6a 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/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
{
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
index f23423f6cfaa89711123bfbfa612269105d8e123..8d6e12d6e38c95a3e694a8de101dc27180ee9efc 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.cpp
+++ b/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)
diff --git a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
index 775e254616cf8dfd62f6abdb14bf7da3d361c00d..6b1fd53f0b76ec7af2bc6f6bd7ce5ad191789fae 100644
--- a/source/RobotAPI/gui-plugins/PlatformUnitPlugin/PlatformUnitGuiPlugin.h
+++ b/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;
diff --git a/source/RobotAPI/interface/units/PlatformUnitInterface.ice b/source/RobotAPI/interface/units/PlatformUnitInterface.ice
index df76354fa64b37ddbbb6b966141192aab9d9ed15..bec45c18e866a544a7a419f912c3169773187200 100644
--- a/source/RobotAPI/interface/units/PlatformUnitInterface.ice
+++ b/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.