Merge branch 'master' of https://gitlab.com/ArmarX/RobotAPI into aron/dev

source/RobotAPI/components/units/PlatformUnit.cpp

@@ -38,7 +38,7 @@ namespace armarx
 
         def->topic(odometryPrx);
         def->topic(globalPosePrx);
-        def->topic(listenerPrx, listenerChannelName, "");
+        // def->topic(listenerPrx, listenerChannelName, "");
 
         def->component(robotStateComponent);
 
@@ -59,6 +59,8 @@ namespace armarx
 
     void PlatformUnit::onConnectComponent()
     {
+                listenerPrx = getTopic<PlatformUnitListenerPrx>(listenerChannelName);
+
         this->onStartPlatformUnit();
     }
 

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.cpp

@@ -376,6 +376,23 @@ namespace armarx::RobotUnitModule
                 sd->owner = this;
                 rtThreadTimingsSensorDevice = std::dynamic_pointer_cast<RTThreadTimingsSensorDevice>(sd);
             }
+            else if(sd->getDeviceName() == GlobalRobotLocalizationSensorDevice::DeviceName())
+            {
+                ARMARX_DEBUG << "Device is the "  << sd->getDeviceName();
+                if (!std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd))
+                {
+                    throw InvalidArgumentException
+                    {
+                        "You tried to add a SensorDevice with the name " + sd->getDeviceName() +
+                        " which does not derive from GlobalRobotLocalizationSensorDevice. (Don't do this)"
+                    };
+                }
+                //this checks if we already added such a device (do this before setting timingSensorDevice)
+                ARMARX_DEBUG << "Adding the SensorDevice "  << sd->getDeviceName() << " " << &sd ;
+                sensorDevices.add(sd->getDeviceName(), sd);
+                sd->owner = this;
+                globalRobotLocalizationSensorDevice = sd; //std::dynamic_pointer_cast<GlobalRobotLocalizationSensorDevice>(sd);
+            }
             else
             {
                 ARMARX_DEBUG << "Adding the SensorDevice "  << sd->getDeviceName() << " " << &sd ;

source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleDevices.h

@@ -25,6 +25,7 @@
 #include <mutex>
 
 #include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
+#include "RobotAPI/components/units/RobotUnit/Devices/GlobalRobotPoseSensorDevice.h"
 #include "RobotUnitModuleBase.h"
 
 #include "../Devices/ControlDevice.h"
@@ -239,6 +240,8 @@ namespace armarx::RobotUnitModule
         template<class PtrT>
         void updateVirtualRobotFromSensorValues(const VirtualRobot::RobotPtr& robot, const std::vector<VirtualRobot::RobotNodePtr>& nodes, const std::vector<PtrT>& sensors) const
         {
+            ARMARX_CHECK_NOT_NULL(robot);
+
             for (const SimoxRobotSensorValueMapping& m : simoxRobotSensorValueMapping)
             {
                 const SensorValueBase* sv = sensors.at(m.idxSens);
@@ -247,6 +250,15 @@ namespace armarx::RobotUnitModule
                 ARMARX_CHECK_NOT_NULL(svPos);
                 nodes.at(m.idxRobot)->setJointValueNoUpdate(svPos->position);
             }
+
+            // update global pose 
+            if(globalRobotLocalizationSensorDevice != nullptr)
+            {
+                const auto sensorValue = globalRobotLocalizationSensorDevice->getSensorValue()->asA<GlobalRobotLocalizationSensorDevice::SensorValueType>();
+                const auto global_T_root = sensorValue->global_T_root;
+                robot->setGlobalPose(global_T_root, false);
+            }
+
             robot->applyJointValues();
         }
 
@@ -422,6 +434,9 @@ namespace armarx::RobotUnitModule
 
         /// @brief a pointer to the RTThreadTimingsSensorDevice used to meassure timings in the rt loop
         RTThreadTimingsSensorDevicePtr rtThreadTimingsSensorDevice;
+
+        armarx::ConstSensorDevicePtr globalRobotLocalizationSensorDevice;
+
         /// @brief The device name of rtThreadTimingsSensorDevice
         static const std::string rtThreadTimingsSensorDeviceName;
         // //////////////////////////////////////////////////////////////////////////////////////// //

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

@@ -134,7 +134,7 @@ namespace armarx
 
             // legacy interface
             const auto& vel = odomVelocity.twist;
-            // listenerPrx->reportPlatformVelocity(vel.linear.x(), vel.linear.y(), vel.angular.z());
+            listenerPrx->reportPlatformVelocity(vel.linear.x(), vel.linear.y(), vel.angular.z());
         }
     }