report platform odometry

source/RobotAPI/components/units/PlatformUnitObserver.cpp

@@ -62,6 +62,7 @@ void PlatformUnitObserver::onConnectObserver()
     offerChannel("platformPose", "Current Position of " + platformNodeName);
     offerChannel("targetPose", "Target Position of " + platformNodeName);
     offerChannel("platformVelocity", "Current velocity of " + platformNodeName);
+    offerChannel("platformOdometryPose", "Current Odometry Position of " + platformNodeName);
 
     // register all data fields
     offerDataField("platformPose", "positionX", VariantType::Float, "Current X position of " + platformNodeName + " in mm");
@@ -76,6 +77,10 @@ void PlatformUnitObserver::onConnectObserver()
     offerDataField("platformVelocity", "velocityY", VariantType::Float, "Current Y velocity of " + platformNodeName + " in mm/s");
     offerDataField("platformVelocity", "velocityRotation", VariantType::Float, "Current Rotation velocity of " + platformNodeName + " in radian/s");
 
+    offerDataField("platformOdometryPose", "positionX", VariantType::Float, "Current Odometry X position of " + platformNodeName + " in mm");
+    offerDataField("platformOdometryPose", "positionY", VariantType::Float, "Current Odometry Y position of " + platformNodeName + " in mm");
+    offerDataField("platformOdometryPose", "rotation", VariantType::Float, "Current Odometry Rotation of " + platformNodeName + " in radian");
+
 }
 
 void PlatformUnitObserver::reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c)
@@ -113,3 +118,10 @@ PropertyDefinitionsPtr PlatformUnitObserver::createPropertyDefinitions()
     return PropertyDefinitionsPtr(new PlatformUnitObserverPropertyDefinitions(
                                       getConfigIdentifier()));
 }
+
+
+void armarx::PlatformUnitObserver::reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&)
+{
+    nameValueMapToDataFields("platformOdometryPose", x, y, angle);
+    updateChannel("platformOdometryPose");
+}

source/RobotAPI/components/units/PlatformUnitObserver.h

@@ -82,6 +82,7 @@ namespace armarx
         void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = ::Ice::Current());
         void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current());
         void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current());
+        void reportPlatformOdometryPose(Ice::Float x, Ice::Float y, Ice::Float angle, const Ice::Current&);
 
         virtual std::string getDefaultName() const
         {
@@ -98,6 +99,7 @@ namespace armarx
 
     private:
         std::string platformNodeName;
+
     };
 
     /**

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

@@ -45,18 +45,20 @@ void armarx::PlatformSubUnit::update(const armarx::SensorAndControl& sc, const J
         relY = s->relativePositionY;
         relR = s->relativePositionRotation;
 
+        // @@@ CHECK BELOW:
         if (s->isA<SensorValueHolonomicPlatformAbsolutePosition>())
         {
             const SensorValueHolonomicPlatformAbsolutePosition* sabs = s->asA<SensorValueHolonomicPlatformAbsolutePosition>();
             abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(sabs->absolutePositionX, sabs->absolutePositionY, 0, 0, 0, sabs->absolutePositionRotation);
+            const float globR = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
+            listenerPrx->reportPlatformPose(abs(0, 3), abs(1, 3), globR);
         }
         else
         {
             abs = positionCorrection * VirtualRobot::MathTools::posrpy2eigen4f(relX, relY, 0, 0, 0, relR);
         }
 
-        const float globR = VirtualRobot::MathTools::eigen4f2rpy(abs)(2);
-        listenerPrx->reportPlatformPose(abs(0, 3), abs(1, 3), globR);
+        listenerPrx->reportPlatformOdometryPose(relX, relY, relR);
     }
     //vel
     {

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

@@ -393,3 +393,9 @@ void KeyboardPlatformHookWidget::keyReleaseEvent(QKeyEvent* event)
 
 Q_EXPORT_PLUGIN2(robotapi_gui_PlatformUnitGuiPlugin, PlatformUnitGuiPlugin)
 
+
+
+void armarx::PlatformUnitWidget::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+{
+    // ignore for now
+}

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

@@ -117,6 +117,7 @@ namespace armarx
         void reportPlatformPose(::Ice::Float currentPlatformPositionX, ::Ice::Float currentPlatformPositionY, ::Ice::Float currentPlatformRotation, const Ice::Current& c = ::Ice::Current());
         void reportNewTargetPose(::Ice::Float newPlatformPositionX, ::Ice::Float newPlatformPositionY, ::Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current());
         void reportPlatformVelocity(::Ice::Float currentPlatformVelocityX, ::Ice::Float currentPlatformVelocityY, ::Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current());
+        void reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&);
 
         // inherited of ArmarXWidget
         static QString GetWidgetName()
@@ -221,6 +222,7 @@ namespace armarx
         // ArmarXWidgetController interface
     public:
         QPointer<QWidget> getWidget();
+
     };
     typedef boost::shared_ptr<PlatformUnitWidget> PlatformUnitGuiPluginPtr;
 }

source/RobotAPI/interface/units/PlatformUnitInterface.ice

@@ -99,6 +99,8 @@ module armarx
 		* @param currentPlatformVelocityRotation Current orientational velocity defined in platform root. 
 		**/
         void reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation);
+
+        void reportPlatformOdometryPose(float x, float y, float angle);
     };
 
 };