forces are now visualized

source/RobotAPI/components/units/ForceTorqueObserver.cpp

@@ -31,6 +31,7 @@
 #include <RobotAPI/libraries/core/checks/ConditionCheckMagnitudeChecks.h>
 #include <RobotAPI/libraries/core/observerfilters/OffsetFilter.h>
 #include <ArmarXCore/observers/variant/DatafieldRef.h>
+#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 #include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
 
@@ -75,12 +76,48 @@ void ForceTorqueObserver::onInitObserver()
     offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
     offerConditionCheck("magnitudesmaller", new ConditionCheckMagnitudeSmaller());
 
+    usingProxy("RobotStateComponent");
+    usingTopic("DebugDrawerUpdates");
 }
 
 void ForceTorqueObserver::onConnectObserver()
 {
+    assignProxy(robot, "RobotStateComponent");
+    debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
+    visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 50, false, "visualizerFunction");
+    localRobot = RemoteRobot::createLocalClone(robot);
+    visualizerTask->start();
 
+}
 
+void ForceTorqueObserver::visualizerFunction()
+{
+    RemoteRobot::synchronizeLocalClone(localRobot, robot);
+    auto channels = getAvailableChannels(false);
+    for(auto& channel : channels)
+    {
+        if(localRobot->hasRobotNode(channel.first))
+        {
+            DatafieldRefPtr field = DatafieldRefPtr::dynamicCast(getForceDatafield(channel.first, Ice::Current()));
+            FramedDirectionPtr value = field->getDataField()->get<FramedDirection>();
+            auto force = value->toGlobal(localRobot);
+//            ARMARX_INFO << deactivateSpam(5,channel.first) << "new force" << channel.first << " : " << force->toEigen();
+
+            float forceMag = force->toEigen().norm()/100;
+            forceMag = std::min(1.0f, forceMag);
+            debugDrawer->setArrowVisu("Forces",
+                                      "Force" + channel.first,
+                                      new Vector3(localRobot->getRobotNode(channel.first)->getGlobalPose()),
+                                      force,
+                                      DrawColor{forceMag, 1.0f-forceMag, 0.0f, 0.5f},
+                                      50,
+                                      5);
+        }
+        else
+        {
+//            ARMARX_INFO << deactivateSpam(5,channel.first) << "No node name " << channel.first;
+        }
+    }
 }
 
 
@@ -106,7 +143,7 @@ void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& ty
     }
     else
     {
-        setDataField(id->channelName, id->datafieldName, Variant(value));
+        setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
     }
 
 
@@ -232,3 +269,9 @@ DataFieldIdentifierPtr ForceTorqueObserver::getTorqueDatafieldId(const std::stri
     DataFieldIdentifierPtr id = new DataFieldIdentifier(getName(), channelName, datafieldName);
     return id;
 }
+
+void ForceTorqueObserver::onExitObserver()
+{
+    if(visualizerTask)
+        visualizerTask->stop();
+}

source/RobotAPI/components/units/ForceTorqueObserver.h

@@ -26,6 +26,7 @@
 #include <RobotAPI/interface/units/ForceTorqueUnit.h>
 #include <RobotAPI/libraries/core/FramedPose.h>
 #include <ArmarXCore/observers/Observer.h>
+#include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
 
 namespace armarx
 {
@@ -68,6 +69,9 @@ namespace armarx
         }
         void onInitObserver();
         void onConnectObserver();
+        void onExitObserver();
+
+        void visualizerFunction();
 
         virtual void reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&);
 
@@ -84,7 +88,10 @@ namespace armarx
     private:
         armarx::Mutex dataMutex;
         std::string topicName;
-
+        RobotStateComponentInterfacePrx robot;
+        VirtualRobot::RobotPtr localRobot;
+        DebugDrawerInterfacePrx debugDrawer;
+        PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
 
         void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
 
@@ -97,6 +104,7 @@ namespace armarx
 
         DataFieldIdentifierPtr getForceDatafieldId(const std::string& nodeName, const std::string& frame);
         DataFieldIdentifierPtr getTorqueDatafieldId(const std::string& nodeName, const std::string& frame);
+
     };
 }