Merge branch 'chore-and-fix/platform-unit-listener-itf' into 'master'

Platform Unit interface modifications: using GlobalRobotPoseLocalizationListener instead

See merge request !53

source/ArmarXSimulation/components/KinematicSelfLocalization/KinematicSelfLocalization.cpp

@@ -21,6 +21,10 @@
  */
 
 #include "KinematicSelfLocalization.h"
+#include <Eigen/Geometry>
+#include <SimoxUtility/math/convert/rpy_to_mat3f.h>
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include <RobotAPI/interface/core/GeometryBase.h>
 
 #include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
@@ -67,12 +71,18 @@ void KinematicSelfLocalization::onConnectComponent()
     ARMARX_INFO << "Creating robot agent...done";
 
     // Simulating platform movement to initial pose
-    PlatformPose pose;
-    pose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
-    pose.x = getProperty<float>("InitialPlatformPoseX").getValue();
-    pose.y = getProperty<float>("InitialPlatformPoseY").getValue();
-    pose.rotationAroundZ = getProperty<float>("InitialPlatformPoseAngle").getValue();
-    reportPlatformPose(pose);
+    Eigen::Isometry3f global_T_robot_initial = Eigen::Isometry3f::Identity();
+    global_T_robot_initial.translation().x() = getProperty<float>("InitialPlatformPoseX").getValue();
+    global_T_robot_initial.translation().y() = getProperty<float>("InitialPlatformPoseY").getValue();
+    global_T_robot_initial.linear() = simox::math::rpy_to_mat3f(0, 0, getProperty<float>("InitialPlatformPoseAngle").getValue());
+
+    TransformStamped transformStamped;
+    transformStamped.transform = global_T_robot_initial.matrix();
+    transformStamped.header.agent = agentName;
+    transformStamped.header.frame = GlobalFrame;
+    transformStamped.header.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+
+    reportGlobalRobotPose(transformStamped);
 
     // periodic task setup
     cycleTime = 30;
@@ -129,29 +139,12 @@ void KinematicSelfLocalization::reportRobotPose()
     }
 }
 
-void armarx::KinematicSelfLocalization::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current&)
+void KinematicSelfLocalization::reportGlobalRobotPose(const ::armarx::TransformStamped& transformStamped, const ::Ice::Current&) 
 {
     std::unique_lock lock(dataMutex);
-    Eigen::Matrix3f ori;
-    ori.setIdentity();
-    ori = Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ());
-    Eigen::Vector3f pos;
-    pos[0] = currentPose.x;
-    pos[1] = currentPose.y;
-    pos[2] = robotPoseZ;
-
-    currentRobotPose = new FramedPose(ori, pos, armarx::GlobalFrame, "");
-}
-
-void armarx::KinematicSelfLocalization::reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
-}
 
-void armarx::KinematicSelfLocalization::reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
-}
+    Eigen::Isometry3f global_T_robot(transformStamped.transform);
+    global_T_robot.translation().z() = robotPoseZ; // TODO lecagy. Is this really necessary?
 
-
-void armarx::KinematicSelfLocalization::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
-{
+    currentRobotPose = new FramedPose(global_T_robot.matrix(), armarx::GlobalFrame, "");
 }

source/ArmarXSimulation/components/KinematicSelfLocalization/KinematicSelfLocalization.h

@@ -24,6 +24,7 @@
 
 #include <ArmarXCore/core/Component.h>
 #include <ArmarXCore/core/services/tasks/PeriodicTask.h>
+#include <RobotAPI/interface/core/RobotLocalization.h>
 #include <RobotAPI/interface/core/RobotState.h>
 
 // MemoryX
@@ -76,7 +77,7 @@ namespace armarx
      */
     class KinematicSelfLocalization :
         virtual public armarx::Component,
-        public PlatformUnitListener
+        public GlobalRobotPoseLocalizationListener
     {
     public:
         /**
@@ -134,11 +135,6 @@ namespace armarx
         float robotPoseZ;
 
     public:
-        // PlatformUnitListener interface
-        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& = Ice::emptyCurrent) override;
-        void reportNewTargetPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current& = Ice::emptyCurrent) override;
-        void reportPlatformVelocity(Ice::Float, Ice::Float, Ice::Float, const Ice::Current& = Ice::emptyCurrent) override;
-        void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&) override;
+        void reportGlobalRobotPose(const ::armarx::TransformStamped&, const ::Ice::Current& = ::Ice::emptyCurrent) override;
     };
 }
-

source/ArmarXSimulation/components/PlatformUnitDynamicSimulation/PlatformUnitDynamicSimulation.cpp

@@ -25,6 +25,8 @@
 
 #include "PlatformUnitDynamicSimulation.h"
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include "RobotAPI/libraries/core/FramedPose.h"
+#include "RobotAPI/libraries/core/Pose.h"
 #include <Eigen/Geometry>
 
 #include <cmath>
@@ -242,7 +244,7 @@ void PlatformUnitDynamicSimulation::moveTo(Ice::Float targetPlatformPositionX, I
     targetRotation = targetPlatformRotation;
     this->positionalAccuracy = positionalAccuracy * .001f;
     this->orientationalAccuracy = orientationalAccuracy;
-    listenerPrx->begin_reportNewTargetPose(targetPosition.x, targetPosition.y, targetRotation);
+    // listenerPrx->begin_reportNewTargetPose(targetPosition.x, targetPosition.y, targetRotation);
     ARMARX_INFO << "New Target: " << targetPosition.x << " " << targetPosition.y << " " << targetRotation << flush;
 }
 
@@ -334,12 +336,20 @@ void PlatformUnitDynamicSimulation::reportState(SimulatedRobotState const& state
 
     // Pose
     updateCurrentPose(state.pose);
-    PlatformPose currentPose;
-    currentPose.timestampInMicroSeconds = state.timestampInMicroSeconds;
-    currentPose.x = currentPosition.x;
-    currentPose.y = currentPosition.y;
-    currentPose.rotationAroundZ = currentRotation;
-    listenerPrx->begin_reportPlatformPose(currentPose);
+
+    ::armarx::TransformStamped transformStamped;
+    
+    FrameHeader header;
+    header.parentFrame = GlobalFrame;
+    header.frame = "root";
+    header.agent = robotName;
+    header.timestampInMicroSeconds = state.timestampInMicroSeconds;
+
+    TransformStamped globalRobotPose;
+    globalRobotPose.header = header;
+    globalRobotPose.transform = armarx::fromIce(state.pose);
+
+    globalPosePrx->reportGlobalRobotPose(globalRobotPose);
 
     // Velocity
     updateCurrentVelocity(state.linearVelocity, state.angularVelocity);