enable PlatformUnit::moveTo()

- moveTo() now uses the global position provided by the SelfLocalization
component to control the platform with an instance of
NJointHolonomicPlatformGlobalPositionController

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

@@ -133,17 +133,17 @@ namespace armarx
     }
 
 
-    void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(float x, float y, float yaw, const IceUtil::Time& time = ::IceUtil::Time::now())
+    void NJointHolonomicPlatformGlobalPositionController::setGlobalPos(const PlatformPose& currentPose)
     {
         // ..todo:: lock :)
 
-        getWriterControlStruct().globalPosition << x, y;
-        getWriterControlStruct().globalOrientation = yaw;
+        getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
+        getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ;
 
         getWriterControlStruct().startPosition = currentPosition;
         getWriterControlStruct().startOrientation = currentOrientation;
 
-        getWriterControlStruct().lastUpdate = time;
+        getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
         writeControlStruct();
     }
 

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

@@ -102,7 +102,7 @@ namespace armarx
 
         void setTarget(float x, float y, float yaw, float translationAccuracy, float rotationAccuracy);
 
-        void setGlobalPos(float x, float y, float yaw, const IceUtil::Time&);
+        void setGlobalPos(const PlatformPose& currentPose);
 
 
     protected:

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

@@ -45,6 +45,7 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
         relY = s->relativePositionY;
         relR = s->relativePositionRotation;
 
+
         // @@@ CHECK BELOW:
         if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
         {
@@ -57,6 +58,7 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
             currentPose.y = abs(1, 3);
             currentPose.rotationAroundZ = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
             listenerPrx->reportPlatformPose(currentPose);
+            globalPosCtrl->setGlobalPos(currentPose);
         }
         else
         {
@@ -90,7 +92,11 @@ void armarx::PlatformSubUnit::move(Ice::Float vx, Ice::Float vy, Ice::Float vr,
 
 void armarx::PlatformSubUnit::moveTo(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
 {
-    ARMARX_WARNING << "NYI";
+    globalPosCtrl->setTarget(rx, ry, rr, lac, rac);
+    if (!globalPosCtrl->isControllerActive())
+    {
+        globalPosCtrl->activateController();
+    };
 }
 
 void armarx::PlatformSubUnit::moveRelative(Ice::Float rx, Ice::Float ry, Ice::Float rr, Ice::Float lac, Ice::Float rac, const Ice::Current&)
@@ -138,3 +144,25 @@ Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
     rp(1, 3) = relY;
     return rp;
 }
+
+
+void armarx::PlatformSubUnit::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+{
+    globalPosCtrl->setGlobalPos(currentPose);
+}
+
+void armarx::PlatformSubUnit::reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c)
+{
+
+}
+
+void armarx::PlatformSubUnit::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+{
+
+}
+
+
+
+void armarx::PlatformSubUnit::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+{
+}

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

@@ -44,7 +44,8 @@ namespace armarx
     TYPEDEF_PTRS_HANDLE(PlatformSubUnit);
     class PlatformSubUnit:
         virtual public RobotUnitSubUnit,
-        virtual public PlatformUnit
+        virtual public PlatformUnit,
+        virtual public PlatformSubUnitInterface
     {
     public:
         void update(const SensorAndControl& sc, const JointAndNJointControllers& c) override;
@@ -59,8 +60,18 @@ 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;
+
         // PlatformUnit interface
-        void onInitPlatformUnit()  override {}
+        void onInitPlatformUnit()  override
+        {
+            usingTopic("PlatformState");
+        }
         void onStartPlatformUnit() override {}
         void onExitPlatformUnit()  override {}
         void stopPlatform(const Ice::Current& c = Ice::emptyCurrent) override;

source/RobotAPI/interface/units/PlatformUnitInterface.ice

@@ -109,5 +109,10 @@ module armarx
         void reportPlatformOdometryPose(float x, float y, float angle);
     };
 
+    interface PlatformSubUnitInterface extends PlatformUnitInterface, PlatformUnitListener
+    {
+
+    };
+
 };