diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp index 2fa220da656ab2d002693284a68f4e4ab660f74e..653bf523d4624394dee8434a86efc07d4f69f824 100644 --- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp +++ b/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(); +} diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h index 8aa5dbd05cf2ebcd1b8ebe227c6fe70823811215..ca2f27cc632d4be8d85676e4aba158accc60b929 100644 --- a/source/RobotAPI/components/units/ForceTorqueObserver.h +++ b/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); + }; }