platform unit gui: using reportGlobalRobotPose topic

removed old topics and from now on, using better suited reportGlobalRobotPose topic instead

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

@@ -26,6 +26,8 @@
 #include <RobotAPI/gui-plugins/PlatformUnitPlugin/ui_PlatformUnitConfigDialog.h>
 #include <ArmarXCore/core/system/ArmarXDataPath.h>
 
+#include <Eigen/Geometry>
+
 // Qt headers
 #include <Qt>
 #include <QtGlobal>
@@ -35,6 +37,8 @@
 #include <QHBoxLayout>
 
 //std
+#include <RobotAPI/interface/core/RobotLocalization.h>
+#include <SimoxUtility/math/convert/mat3f_to_rpy.h>
 #include <memory>
 #include <cmath>
 
@@ -86,8 +90,8 @@ PlatformUnitWidget::PlatformUnitWidget() :
 void PlatformUnitWidget::onInitComponent()
 {
     usingProxy(platformUnitProxyName);
-    usingTopic(platformName + "State");
-    ARMARX_INFO << "Listening on Topic: " << platformName + "State";
+    usingTopic(globalRobotPoseLocalizationListenerName);
+    // ARMARX_INFO << "Listening on Topic: " << platformName + "State";
 
 }
 
@@ -177,7 +181,6 @@ void PlatformUnitWidget::setNewPlatformPoseLabels(float x, float y, float alpha)
     ui.labelCurrentPositionX->setText(QString::number(x));
     ui.labelCurrentPositionY->setText(QString::number(y));
     ui.labelCurrentRotation->setText(QString::number(alpha));
-
 }
 
 
@@ -203,26 +206,16 @@ void PlatformUnitWidget::stopControlTimer()
 }
 
 
-void PlatformUnitWidget::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+void PlatformUnitWidget::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&)
 {
-    // moved to qt thread for thread safety
-    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, currentPose.x), Q_ARG(float, currentPose.y), Q_ARG(float, currentPose.rotationAroundZ));
-    platformRotation = currentPose.rotationAroundZ;
-}
+    const Eigen::Isometry3f global_T_robot(transformStamped.transform);
+    const float x = global_T_robot.translation().x();
+    const float y = global_T_robot.translation().y();
+    const float yaw = simox::math::mat3f_to_rpy(global_T_robot.linear()).z();
 
-
-void PlatformUnitWidget::reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c)
-{
     // moved to qt thread for thread safety
-    QMetaObject::invokeMethod(this, "setNewTargetPoseLabels",
-                              Q_ARG(float, newPlatformPositionX),
-                              Q_ARG(float, newPlatformPositionY),
-                              Q_ARG(float, newPlatformRotation));
-}
-
-void PlatformUnitWidget::reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
-{
-
+    QMetaObject::invokeMethod(this, "setNewPlatformPoseLabels", Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, yaw));
+    platformRotation = yaw;
 }
 
 
@@ -396,9 +389,3 @@ void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
     }
     QWidget::keyReleaseEvent(event);
 }
-
-
-void armarx::PlatformUnitWidget::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
-    // ignore for now
-}