From 5c4685bd1aa92b1eba67ca7e164b024267d76f58 Mon Sep 17 00:00:00 2001
From: Markus Grotz <markus.grotz@kit.edu>
Date: Fri, 23 Jan 2015 17:59:30 +0100
Subject: [PATCH] added simple visualization for IMU device

---
 .../units/InertialMeasurementUnitObserver.cpp | 30 +++++++++++++++----
 .../units/InertialMeasurementUnitObserver.h   |  3 ++
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
index f49985393..70c386f89 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.cpp
@@ -19,23 +19,26 @@ void InertialMeasurementUnitObserver::onInitObserver()
 {
     usingTopic(getProperty<std::string>("IMUTopicName").getValue());
 
-
     offerConditionCheck("updated", new ConditionCheckUpdated());
     offerConditionCheck("larger", new ConditionCheckLarger());
     offerConditionCheck("equals", new ConditionCheckEquals());
     offerConditionCheck("smaller", new ConditionCheckSmaller());
+
+    offeringTopic(getProperty<std::string>("DebugDrawerTopic").getValue());
 }
 
 
 
 void InertialMeasurementUnitObserver::onConnectObserver()
 {
+    debugDrawerPrx = getTopic<DebugDrawerInterfacePrx>(getProperty<std::string>("DebugDrawerTopic").getValue());
 }
 
 
 void InertialMeasurementUnitObserver::onExitObserver()
 {
-
+    debugDrawerPrx->removePoseVisu("IMU", "orientation");
+    debugDrawerPrx->removeLineVisu("IMU", "acceleration");
 }
 
 void InertialMeasurementUnitObserver::reportSensorValues(const std::string &device, const std::string &name, const IMUData &values, const TimestampBasePtr &timestamp, const Ice::Current &c)
@@ -44,10 +47,10 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string &devi
 
     TimestampVariantPtr timestampPtr = TimestampVariantPtr::dynamicCast(timestamp);
 
-    Vector3BasePtr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
-    Vector3BasePtr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
-    Vector3BasePtr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
-    QuaternionBasePtr orientationQuaternion =  new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
+    Vector3Ptr acceleration = new Vector3(values.acceleration.at(0), values.acceleration.at(1), values.acceleration.at(2));
+    Vector3Ptr gyroscopeRotation = new Vector3(values.gyroscopeRotation.at(0), values.gyroscopeRotation.at(1), values.gyroscopeRotation.at(2));
+    Vector3Ptr magneticRotation = new Vector3(values.magneticRotation.at(0), values.magneticRotation.at(1), values.magneticRotation.at(2));
+    QuaternionPtr orientationQuaternion =  new Quaternion(values.orientationQuaternion.at(0), values.orientationQuaternion.at(1), values.orientationQuaternion.at(2), values.orientationQuaternion.at(3));
 
     if(!existsChannel(device))
     {
@@ -70,6 +73,21 @@ void InertialMeasurementUnitObserver::reportSensorValues(const std::string &devi
         setDataField(device, "timestamp", timestampPtr);
     }
     updateChannel(device);
+
+    Eigen::Vector3f zero;
+    zero.setZero();
+
+    DrawColor color;
+    color.r = 255;
+    color.g = 255;
+
+    Eigen::Vector3f ac = acceleration->toEigen();
+    ac *= 10;
+
+    debugDrawerPrx->setLineVisu("IMU", "acceleration", new Vector3(), new Vector3(ac), 2.0f, color);
+
+    PosePtr posePtr = new Pose(orientationQuaternion->toEigen(), zero);
+    debugDrawerPrx->setPoseVisu("IMU", "orientation", posePtr);
 }
 
 
diff --git a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
index 6ce41f2bf..2c31902f3 100644
--- a/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
+++ b/source/RobotAPI/components/units/InertialMeasurementUnitObserver.h
@@ -26,6 +26,7 @@
 
 #include <RobotAPI/interface/units/InertialMeasurementUnit.h>
 #include <Core/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 
 namespace armarx
@@ -39,6 +40,7 @@ namespace armarx
             ComponentPropertyDefinitions(prefix)
         {
             defineOptionalProperty<std::string>("IMUTopicName", "IMUValues", "Name of the IMU Topic.");
+            defineOptionalProperty<std::string>("DebugDrawerTopic", "DebugDrawerUpdates", "Name of the DebugDrawerTopic");
         }
     };
 
@@ -65,6 +67,7 @@ namespace armarx
 
       private:
          Mutex dataMutex;
+         DebugDrawerInterfacePrx debugDrawerPrx;
 
 
     };
-- 
GitLab