further interface changes

source/RobotAPI/components/units/PlatformUnitSimulation.cpp

@@ -24,8 +24,12 @@
 
 
 #include "PlatformUnitSimulation.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
 
 #include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Translation.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
 #include <VirtualRobot/MathTools.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 #include <cmath>
@@ -56,14 +60,19 @@ namespace armarx
 
     }
 
+    Eigen::Matrix4f PlatformUnitSimulation::currentPlatformPose() const
+    {
+        // FIXME(fabian.reister): check if order is correct
+        return (Eigen::Translation3f(currentPositionX, currentPositionY, 0.F) * Eigen::AngleAxisf(currentRotation, Eigen::Vector3f::UnitZ())).matrix();
+    }
+
     void PlatformUnitSimulation::onStartPlatformUnit()
     {
-        PlatformPose currentPose;
+        PoseStampedPtr currentPose(new PoseStamped);
         // FIXME: Take the timestamp from simulation
-        currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
-        currentPose.x = currentPositionX;
-        currentPose.y = currentPositionY;
-        currentPose.rotationAroundZ = currentRotation;
+        currentPose->timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+        currentPose->pose = currentPlatformPose();
+
         listenerPrx->reportPlatformPose(currentPose);
         simulationTask->start();
     }
@@ -85,6 +94,12 @@ namespace armarx
         }
     }
 
+    void PlatformUnitSimulation::fillCommonPlatformFields(FramedAndStamped & framedAndStamped)
+    {
+        framedAndStamped.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+        framedAndStamped.frame = GlobalFrame;
+        framedAndStamped.agent = RobotFrame;
+    }
 
     void PlatformUnitSimulation::simulationFunction()
     {
@@ -164,22 +179,14 @@ namespace armarx
             }
         }
 
-        auto fillCommonPlatformFields = [&](FramedAndStamped & framedAndStamped)
-        {
-            platformPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
-            platformPose.parentFrame = GlobalFrame;
-            platformPose.childFrame = robotFrame;
-        };
-
-        PoseStamped platformPose;
-        fillCommonPlatformFields(platformPose);
-        // FIXME(fabian.reister): check if order is correct
-        platformPose.pose = (Eigen::Translation3f(currentPositionX, currentPositionY, 0.F) * Eigen::AngleAxisf(currentRotation, Eigen::Vector3f::UnitZ())).matrix();
 
-        VelocityStamped platformVelocity;
-        fillCommonPlatformFields(platformVelocity);
-        platformVelocity.velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
+        PoseStampedPtr platformPose(new PoseStamped);
+        fillCommonPlatformFields(*platformPose);
+        platformPose->pose = currentPlatformPose();
 
+        VelocityStampedPtr platformVelocity(new VelocityStamped);
+        fillCommonPlatformFields(*platformVelocity);
+        platformVelocity->velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
 
         listenerPrx->reportPlatformPose(platformPose);
         listenerPrx->reportPlatformVelocity(platformVelocity);
@@ -197,6 +204,10 @@ namespace armarx
             this->positionalAccuracy = positionalAccuracy;
             this->orientationalAccuracy = orientationalAccuracy;
         }
+
+        PoseStamped targetPose;
+
+
         listenerPrx->reportNewTargetPose(targetPositionX, targetPositionY, targetRotation);
     }
 

source/RobotAPI/components/units/PlatformUnitSimulation.h

@@ -134,6 +134,10 @@ namespace armarx
         PeriodicTask<PlatformUnitSimulation>::pointer_type simulationTask;
 
 
+    private:
+        Eigen::Matrix4f currentPlatformPose() const;
+        void fillCommonPlatformFields(FramedAndStamped & framedAndStamped)
+
     };
 }
 

source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp

@@ -23,6 +23,8 @@
  */
 #include "NJointHolonomicPlatformGlobalPositionController.h"
 
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
 namespace armarx
 {
     NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
@@ -124,15 +126,17 @@ namespace armarx
     {
         // ..todo: check if norm is too large
 
-        // TODO(fabian.reister): add check: frame and agent have to "Global" and "robot"
+        // TODO(fabian.reister): add check: frame and agent have to be "Global" and "robot"
+
+        const float yaw = simox::math::mat4f_to_rpy(currentPose.pose).z();
 
-        const float yaw = simox::math::mat4f_2_rpy(currentPose.pose);
+        const Eigen::Affine3f pose(currentPose.pose);
 
-        getWriterControlStruct().globalPosition << currentPose.translation().x(), currentPose.translation().y();
+        getWriterControlStruct().globalPosition << pose.translation().x(), pose.translation().y();
         getWriterControlStruct().globalOrientation = yaw;
 
         getWriterControlStruct().startPosition = currentPosition;
-        getWriterControlStruct().startOrientation = yaw;
+        getWriterControlStruct().startOrientation = currentOrientation;
 
         getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
         writeControlStruct();

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

@@ -148,23 +148,19 @@ Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
 }
 
 
-void armarx::PlatformSubUnit::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+void armarx::PlatformSubUnit::reportPlatformPose(const PoseStampedPtr& platformPose, const Ice::Current& c)
 {
-    globalPosCtrl->setGlobalPos(currentPose);
+    globalPosCtrl->setGlobalPos(*platformPose);
 }
 
-void armarx::PlatformSubUnit::reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c)
+void armarx::PlatformSubUnit::reportNewTargetPose(const PoseStampedPtr& /*targetPlatformPose*/, const Ice::Current& c)
 {
-
 }
 
-void armarx::PlatformSubUnit::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+void armarx::PlatformSubUnit::reportPlatformVelocity(const VelocityStampedPtr&, const Ice::Current& c)
 {
-
 }
 
-
-
-void armarx::PlatformSubUnit::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+void armarx::PlatformSubUnit::reportPlatformOdometryPose(const PoseStampedPtr&, const Ice::Current&)
 {
 }

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

@@ -58,12 +58,10 @@ namespace armarx
         virtual void setGlobalPose(PoseBasePtr globalPose,  const Ice::Current& = Ice::emptyCurrent) /*override*/;
         virtual PoseBasePtr getGlobalPose(const Ice::Current& = Ice::emptyCurrent) /*override*/;
 
-
-        void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = ::Ice::Current()) override ;
-        void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
-        void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current()) override;
-
-        void reportPlatformOdometryPose(::Ice::Float, ::Ice::Float, ::Ice::Float, const ::Ice::Current& = ::Ice::Current()) override;
+        void reportPlatformPose(const PoseStampedPtr& currentPose, const Ice::Current& c) override ;
+        void reportNewTargetPose(const PoseStampedPtr& targetPlatformPose, const Ice::Current& c) override;
+        void reportPlatformVelocity(const VelocityStampedPtr& platformVelocity, const Ice::Current& c) override;
+        void reportPlatformOdometryPose(const PoseStampedPtr& odometryPose, const ::Ice::Current&) override;
 
         // PlatformUnit interface
         void onInitPlatformUnit()  override

source/RobotAPI/libraries/core/FramedPose.h

@@ -59,6 +59,7 @@ namespace armarx
     std::string const GlobalFrame = "Global";
     std::string const OdometryFrame = "Odom";
     std::string const MapFrame = "Map";
+    std::string const RobotFrame = "robot"; // FIXME(fabian.reister): this has to be variable.