finished IMU SubUnit

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

@@ -22,6 +22,12 @@
 #include "InertialMeasurementSubUnit.h"
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
+void armarx::InertialMeasurementSubUnit::onStartIMU()
+{
+
+}
+
+
 void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl& sc, const JointAndNJointControllers&)
 {
     if (!getProxy())
@@ -35,17 +41,21 @@ void armarx::InertialMeasurementSubUnit::update(const armarx::SensorAndControl&
         return;
     }
     TimestampVariantPtr t = new TimestampVariant(sc.sensorValuesTimestamp);
+    InertialMeasurementUnitListenerPrx batchPrx = IMUTopicPrx->ice_batchOneway();
     for (auto nam2idx : devs)
     {
         const auto devidx = nam2idx.second;
         const auto& dev = nam2idx.first;
         const SensorValueBase* sv = sc.sensors.at(devidx).get();
         ARMARX_CHECK_EXPRESSION(sv->isA<SensorValueIMU>());
-        //        const SensorValueIMU* s = sv->asA<SensorValueIMU>();
+        const SensorValueIMU* s = sv->asA<SensorValueIMU>();
         IMUData data;
-        //                data.acceleration = s->linearAcceleration;
-        //                data.gyroscopeRotation = s->angularVelocity;
-        IMUTopicPrx->reportSensorValues(dev, "name", data, t);
-        ///TODO fix unclear imu stuff
+        data.acceleration = Ice::FloatSeq(s->linearAcceleration.data(), s->linearAcceleration.data() + s->linearAcceleration.rows() * s->linearAcceleration.cols());
+        data.gyroscopeRotation = Ice::FloatSeq(s->angularVelocity.data(), s->angularVelocity.data() + s->angularVelocity.rows() * s->angularVelocity.cols());;
+        data.orientationQuaternion = {s->orientation.w(), s->orientation.x(), s->orientation.y(), s->orientation.z()};
+        auto frame = dev;
+        IMUTopicPrx->reportSensorValues(dev, frame, data, t);
     }
+    batchPrx->ice_flushBatchRequests();
 }
+

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

@@ -44,7 +44,7 @@ namespace armarx
         // InertialMeasurementUnit interface
     protected:
         virtual void onInitIMU()  override {}
-        virtual void onStartIMU() override {}
+        virtual void onStartIMU() override;
         virtual void onExitIMU()  override {}
     public:
         std::map<std::string, std::size_t> devs;