KinematicSelfLocalization: using GlobalRobotPoseLocalizationListener instead...

KinematicSelfLocalization: using GlobalRobotPoseLocalizationListener instead of PlatformUnitListener

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;
     };
 }
-