From b181faf3de678ee11a142280aabeb6630a808678 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 18 Feb 2015 17:46:18 +0100
Subject: [PATCH] Fixed RobotStateObserver, replaces now TCPControlUnitObserver
---
source/RobotAPI/applications/CMakeLists.txt | 1 -
.../RobotStateComponentApp.h | 6 +-
.../robotstate/RobotStateComponent.cpp | 40 ++-
.../robotstate/RobotStateComponent.h | 4 +
.../core/RobotStateObserverInterface.ice | 4 +-
.../core/remoterobot/RobotStateObserver.cpp | 303 +++++++++++++-----
.../core/remoterobot/RobotStateObserver.h | 37 ++-
7 files changed, 278 insertions(+), 117 deletions(-)
diff --git a/source/RobotAPI/applications/CMakeLists.txt b/source/RobotAPI/applications/CMakeLists.txt
index 3599c8008..18109e31b 100644
--- a/source/RobotAPI/applications/CMakeLists.txt
+++ b/source/RobotAPI/applications/CMakeLists.txt
@@ -15,7 +15,6 @@ add_subdirectory(KinematicUnitSimulation)
add_subdirectory(PlatformUnitSimulation)
add_subdirectory(PlatformUnitObserver)
add_subdirectory(RobotStateComponent)
-add_subdirectory(RobotStateObserver)
add_subdirectory(HandUnitObserver)
add_subdirectory(HandUnitSimulation)
add_subdirectory(ForceTorqueUnitSimulation)
diff --git a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
index de16b57ee..5ec854e50 100644
--- a/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
+++ b/source/RobotAPI/applications/RobotStateComponent/RobotStateComponentApp.h
@@ -39,7 +39,11 @@ namespace armarx
*/
void setup(const ManagedIceObjectRegistryInterfacePtr& registry, Ice::PropertiesPtr properties)
{
- registry->addObject( Component::create<RobotStateComponent>(properties) );
+ auto state = Component::create<RobotStateComponent>(properties);
+ auto observer = Component::create<RobotStateObserver>(properties);
+ state->setRobotStateObserver(observer);
+ registry->addObject( state );
+ registry->addObject( observer );
}
};
}
diff --git a/source/RobotAPI/components/robotstate/RobotStateComponent.cpp b/source/RobotAPI/components/robotstate/RobotStateComponent.cpp
index aa5ecd439..c94837542 100644
--- a/source/RobotAPI/components/robotstate/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/robotstate/RobotStateComponent.cpp
@@ -29,6 +29,8 @@
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <Core/core/system/ArmarXDataPath.h>
+#include <Core/core/ArmarXManager.h>
+#include <Core/core/ArmarXObjectScheduler.h>
using namespace std;
using namespace VirtualRobot;
@@ -118,6 +120,7 @@ namespace armarx
this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_sharedRobotServant));
ARMARX_INFO << "Started RobotStateComponent" << flush;
+ robotStateObs->setRobot(_synchronized);
}
void RobotStateComponent::onDisconnectComponent()
@@ -149,21 +152,27 @@ namespace armarx
// IceUtil::Time start = IceUtil::Time::now();
if(aValueChanged)
{
- ReadLockPtr lock = _synchronized->getReadLock();
-
- std::vector<float> jv;
- std::vector< RobotNodePtr > nodes;
- BOOST_FOREACH(NameValueMap::value_type namedAngle, jointAngles )
{
- RobotNodePtr node = this->_synchronized->getRobotNode(namedAngle.first);
- nodes.push_back(node);
- jv.push_back(namedAngle.second);
+ WriteLockPtr lock = _synchronized->getWriteLock();
+
+ std::vector<float> jv;
+ std::vector< RobotNodePtr > nodes;
+ BOOST_FOREACH(NameValueMap::value_type namedAngle, jointAngles )
+ {
+ RobotNodePtr node = this->_synchronized->getRobotNode(namedAngle.first);
+ nodes.push_back(node);
+ jv.push_back(namedAngle.second);
+ }
+ _synchronized->setJointValues(nodes, jv);
}
- _synchronized->setJointValues(nodes, jv);
+ if(robotStateObs)
+ robotStateObs->updatePoses();
}
if(_sharedRobotServant)
_sharedRobotServant->setTimestamp(IceUtil::Time::now());
// ARMARX_VERBOSE << deactivateSpam(2) << "my duration: " << (IceUtil::Time::now() - start).toMicroSecondsDouble() << " ms";
+
+
}
std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
@@ -172,7 +181,11 @@ namespace armarx
}
void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged,const Current& c){}
- void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged,const Current& c){}
+ void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged,const Current& c)
+ {
+ if(robotStateObs)
+ robotStateObs->updateNodeVelocities(jointVelocities);
+ }
void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged,const Current& c){}
void RobotStateComponent::reportJointAccelerations(const NameValueMap &jointAccelerations, bool aValueChanged, const Current &c)
@@ -210,7 +223,12 @@ namespace armarx
PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
+ }
+
+ void RobotStateComponent::setRobotStateObserver(RobotStateObserverPtr observer)
+ {
+ robotStateObs = observer;
}
string RobotStateComponent::getRobotName(const Current &) const
diff --git a/source/RobotAPI/components/robotstate/RobotStateComponent.h b/source/RobotAPI/components/robotstate/RobotStateComponent.h
index 0d1eaaef3..905a0c739 100644
--- a/source/RobotAPI/components/robotstate/RobotStateComponent.h
+++ b/source/RobotAPI/components/robotstate/RobotStateComponent.h
@@ -34,6 +34,8 @@
#include <RobotAPI/libraries/core/RobotAPIObjectFactories.h>
+#include <RobotAPI/libraries/core/remoterobot/RobotStateObserver.h>
+
namespace armarx
{
/**
@@ -106,6 +108,7 @@ namespace armarx
{
return "RobotStateComponent";
}
+ void setRobotStateObserver(RobotStateObserverPtr observer);
protected:
/**
* Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -127,6 +130,7 @@ namespace armarx
SharedRobotServantPtr _sharedRobotServant;
VirtualRobot::RobotPtr _synchronized;
std::string robotFile;
+ RobotStateObserverPtr robotStateObs;
};
diff --git a/source/RobotAPI/interface/core/RobotStateObserverInterface.ice b/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
index 4c0db7742..551ddeec6 100644
--- a/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
+++ b/source/RobotAPI/interface/core/RobotStateObserverInterface.ice
@@ -31,8 +31,10 @@
module armarx
{
- interface RobotStateObserverInterface extends ObserverInterface, KinematicUnitListener
+ interface RobotStateObserverInterface extends ObserverInterface
{
+ ["cpp:const"]
+ idempotent DatafieldRefBase getPoseDatafield(string nodeName);
};
};
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
index 4635b94ab..a5c40b50f 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.cpp
@@ -33,150 +33,281 @@
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/VirtualRobot.h>
+#include <Core/observers/variant/DatafieldRef.h>
+
+#include <boost/algorithm/string/trim.hpp>
+
+#define TCP_POSE_CHANNEL "TCPPose"
+#define TCP_TRANS_VELOCITIES_CHANNEL "TCPVelocities"
using namespace armarx;
+using namespace VirtualRobot;
// ********************************************************************
// observer framework hooks
// ********************************************************************
-void RobotStateObserver::onInitObserver()
+RobotStateObserver::RobotStateObserver()
{
- // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
- // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
- std::string robotFile = getProperty<std::string>("RobotFileName").getValue();
- robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
-
- this->server = getProxy<RobotStateComponentInterfacePrx>("RobotStateComponent");
- this->robot.reset(new RemoteRobot(server->getSynchronizedRobot()));
- if(robotNodeSetName == "")
- {
- throw UserException("RobotNodeSet not defined");
- }
+}
+void RobotStateObserver::onInitObserver()
+{
// register all checks
offerConditionCheck("equals", new ConditionCheckEquals());
offerConditionCheck("inrange", new ConditionCheckInRange());
offerConditionCheck("larger", new ConditionCheckLarger());
offerConditionCheck("smaller", new ConditionCheckSmaller());
-
- usingTopic(robotNodeSetName + "State");
-
}
void RobotStateObserver::onConnectObserver()
{
- VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = this->robot->getRobotNodeSet(robotNodeSetName);
- std::vector< VirtualRobot::RobotNodePtr > robotNodes;
- robotNodes = robotNodeSetPtr->getAllRobotNodes();
+
+ offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
+ offerChannel(TCP_TRANS_VELOCITIES_CHANNEL, "TCP velocities of the robot.");
+}
+
+// ********************************************************************
+// private methods
+// ********************************************************************
- // register all channels
- offerChannel("positions","Link positions of the " + robotNodeSetName + " kinematic chain");
- offerChannel("orientations","Link orientations of the" + robotNodeSetName + "kinematic chain");
- // register all data fields
- for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
+void RobotStateObserver::updatePoses()
+{
+ if(getState() < eManagedIceObjectStarting)
+ return;
+ if(!robot)
+ return;
+ ScopedRecursiveLock lock(dataMutex);
+ ReadLockPtr lock2 = robot->getReadLock();
+ FramedPoseBaseMap tcpPoses;
+ std::string rootFrame = robot->getRootNode()->getName();
+ //IceUtil::Time start = IceUtil::Time::now();
+ for(unsigned int i=0; i < nodesToReport.size(); i++)
{
- std::string jointName = (*it)->getName();
- std::cout << "* " << jointName << std::endl;
- offerDataField("positions", jointName, VariantType::FramedPosition, "" + jointName + "");
- offerDataField("orientations", jointName, VariantType::FramedOrientation, "" + jointName + "");
+ VirtualRobot::RobotNodePtr& node = nodesToReport.at(i);
+ const std::string &tcpName = node->getName();
+ const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robot->getName());
- // create lookup map
- namesToNodes.insert(make_pair(jointName, *it));
}
-}
+ udpatePoseDatafields(tcpPoses);
-// ********************************************************************
-// KinematicUnitListener interface implementation
-// ********************************************************************
-void RobotStateObserver::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
-{
}
-void RobotStateObserver::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
+void RobotStateObserver::udpatePoseDatafields(const FramedPoseBaseMap &poseMap)
{
- if(!aValueChanged)
- return;
- // update current model
- updateRobotModel(jointAngles);
+// ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
+ FramedPoseBaseMap::const_iterator it = poseMap.begin();
+ for(; it != poseMap.end(); it++)
+ {
- // update datafields
- updatePoses();
+ FramedPosePtr vec = FramedPosePtr::dynamicCast(it->second);
+ const std::string &tcpName = it->first;
+ if(!existsDataField(TCP_POSE_CHANNEL, tcpName))
+ offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ else
+ setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
+ updateChannel(TCP_POSE_CHANNEL);
+ if(!existsChannel(tcpName))
+ {
+ offerChannel(tcpName, "pose components of " + tcpName);
+ offerDataFieldWithDefault(tcpName,"x", Variant(vec->position->x), "X axis");
+ offerDataFieldWithDefault(tcpName,"y", Variant(vec->position->y), "Y axis");
+ offerDataFieldWithDefault(tcpName,"z", Variant(vec->position->z), "Z axis");
+ offerDataFieldWithDefault(tcpName,"qx", Variant(vec->orientation->qx), "Quaternion part x");
+ offerDataFieldWithDefault(tcpName,"qy", Variant(vec->orientation->qy), "Quaternion part y");
+ offerDataFieldWithDefault(tcpName,"qz", Variant(vec->orientation->qz), "Quaternion part z");
+ offerDataFieldWithDefault(tcpName,"qw", Variant(vec->orientation->qw), "Quaternion part w");
+ offerDataFieldWithDefault(tcpName,"frame", Variant(vec->frame), "Reference Frame");
+ }
+ else
+ {
+ StringVariantBaseMap newValues;
+ newValues["x"] = new Variant(vec->position->x);
+ newValues["y"] = new Variant(vec->position->y);
+ newValues["z"] = new Variant(vec->position->z);
+ newValues["qx"] = new Variant(vec->orientation->qx);
+ newValues["qy"] = new Variant(vec->orientation->qy);
+ newValues["qz"] = new Variant(vec->orientation->qz);
+ newValues["qw"] = new Variant(vec->orientation->qw);
+ newValues["frame"] = new Variant(vec->frame);
+ setDataFieldsFlatCopy(tcpName, newValues);
+ }
+ updateChannel(tcpName);
- // update channels
- updateChannel("positions");
- updateChannel("orientations");
+ }
}
-void RobotStateObserver::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
+DatafieldRefBasePtr RobotStateObserver::getPoseDatafield(const std::string &nodeName, const Ice::Current &) const
{
+ return getDataFieldRef(new DataFieldIdentifier(getName(), TCP_POSE_CHANNEL, nodeName));
}
-void RobotStateObserver::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
+void RobotStateObserver::updateNodeVelocities(const NameValueMap &jointVel)
{
-}
-void RobotStateObserver::reportJointAccelerations(const NameValueMap &jointAccelerations, bool aValueChanged, const Ice::Current &c)
-{
+ if(getState() < eManagedIceObjectStarting)
+ return;
+ ScopedRecursiveLock lock(dataMutex);
+ if(!robot)
+ return;
+ ReadLockPtr lock2 = robot->getReadLock();
+ if(!velocityReportRobot)
+ velocityReportRobot = robot->clone(robot->getName());
+// IceUtil::Time start = IceUtil::Time::now();
+// ARMARX_INFO << jointVel; FramedPoseBaseMap tcpPoses;
+ FramedVector3Map tcpTranslationVelocities;
+ FramedVector3Map tcpOrientationVelocities;
+ std::string rootFrame = robot->getRootNode()->getName();
+ NameValueMap tempJointAngles = robot->getConfig()->getRobotNodeJointValueMap();
+ FramedPoseBaseMap tcpPoses;
+ for(unsigned int i=0; i < nodesToReport.size(); i++)
+ {
+ RobotNodePtr node = nodesToReport.at(i);
+ const std::string &tcpName = node->getName();
+ const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
+ tcpPoses[tcpName] = new FramedPose(currentPose, rootFrame, robot->getName());
-}
+ }
-void RobotStateObserver::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
-{
-}
+ double tDelta = 0.001;
+ for(NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
+ {
+ NameValueMap::const_iterator itSrc = jointVel.find(it->first);
+ if(itSrc != jointVel.end())
+ it->second += itSrc->second * tDelta;
+ }
-void RobotStateObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
-{
-}
+ velocityReportRobot->setJointValues(tempJointAngles);
+// ARMARX_INFO << deactivateSpam(1) << "duration: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
+// start = IceUtil::Time::now();
+ Eigen::Matrix4f mat;
+ Eigen::Vector3f rpy;
+ for(unsigned int i=0; i < nodesToReport.size(); i++)
+ {
+ RobotNodePtr node = velocityReportRobot->getRobotNode(nodesToReport.at(i)->getName());
+ const std::string &tcpName = node->getName();
+ const Eigen::Matrix4f ¤tPose = node->getPoseInRootFrame();
-void RobotStateObserver::reportJointStatuses(const NameStatusMap &jointStatuses, bool aValueChanged, const Ice::Current &c)
-{
-}
-// ********************************************************************
-// private methods
-// ********************************************************************
-void RobotStateObserver::updateRobotModel(const NameValueMap& nameValueMap)
-{
- // update all nodes contained in the valueMap
- NameValueMap::const_iterator iter = nameValueMap.begin();
+ FramedPosePtr lastPose = FramedPosePtr::dynamicCast(tcpPoses[tcpName]);
- while(iter != nameValueMap.end())
- {
- VirtualRobot::RobotNodePtr robotNode = namesToNodes[iter->first];
- robotNode->setJointValue(iter->second);
+ tcpTranslationVelocities[tcpName] = new FramedVector3((currentPose.block(0,3,3,1) - lastPose->toEigen().block(0,3,3,1))/tDelta, rootFrame, robot->getName());
- iter++;
- }
+ mat = currentPose * lastPose->toEigen().inverse();
+
+ VirtualRobot::MathTools::eigen4f2rpy(mat, rpy);
+
+ tcpOrientationVelocities[tcpName] = new FramedVector3(rpy/tDelta, rootFrame, robot->getName());
+
+
+ }
+// ARMARX_INFO << deactivateSpam(1) << "duration2: " << (IceUtil::Time::now()-start).toMicroSecondsDouble();
+// ARMARX_INFO << deactivateSpam(5) << "TCP Pose Vel Calc took: " << ( IceUtil::Time::now() - start).toMilliSecondsDouble();
+ updateVelocityDatafields(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void RobotStateObserver::updatePoses()
+void RobotStateObserver::updateVelocityDatafields(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities)
{
- // go through all nodes
- VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = this->robot->getRobotNodeSet(robotNodeSetName);
+ FramedVector3Map::const_iterator it = tcpTranslationVelocities.begin();
+ for(; it != tcpTranslationVelocities.end(); it++)
+ {
- std::vector< VirtualRobot::RobotNodePtr > robotNodes;
- robotNodes = robotNodeSetPtr->getAllRobotNodes();
+ FramedVector3Ptr vec = FramedVector3Ptr::dynamicCast(it->second);
+ FramedVector3Ptr vecOri;
+ FramedVector3Map::const_iterator itOri = tcpOrientationVelocities.find(it->first);
+ if(itOri != tcpOrientationVelocities.end())
+ vecOri = FramedVector3Ptr::dynamicCast(itOri->second);
+ const std::string &tcpName = it->first;
+
+ ARMARX_CHECK_EXPRESSION(vec->frame == vecOri->frame);
+
+ if(!existsDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName))
+ offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
+ else
+ setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
+ updateChannel(TCP_TRANS_VELOCITIES_CHANNEL);
+ const std::string channelName = tcpName+"Velocities";
+ if(!existsChannel(channelName))
+ {
+ offerChannel(channelName, "pose components of " + tcpName);
+ offerDataFieldWithDefault(channelName,"x", Variant(vec->x), "X axis");
+ offerDataFieldWithDefault(channelName,"y", Variant(vec->y), "Y axis");
+ offerDataFieldWithDefault(channelName,"z", Variant(vec->z), "Z axis");
+ offerDataFieldWithDefault(channelName,"roll", Variant(vecOri->x), "Roll");
+ offerDataFieldWithDefault(channelName,"pitch", Variant(vecOri->y), "Pitch");
+ offerDataFieldWithDefault(channelName,"yaw", Variant(vecOri->z), "Yaw");
+ offerDataFieldWithDefault(channelName,"frame", Variant(vecOri->frame), "Reference Frame");
+ }
+ else
+ {
+ StringVariantBaseMap newValues;
+ newValues["x"] = new Variant(vec->x);
+ newValues["y"] = new Variant(vec->y);
+ newValues["z"] = new Variant(vec->z);
+ newValues["roll"] = new Variant(vecOri->x);
+ newValues["pitch"] = new Variant(vecOri->y);
+ newValues["yaw"] = new Variant(vecOri->z);
+ newValues["frame"] = new Variant(vec->frame);
+ setDataFieldsFlatCopy(channelName, newValues);
+ }
+ updateChannel(channelName);
- // set all data fields
- // TODO: maybe global pose is not correct!!!
- for (std::vector<VirtualRobot::RobotNodePtr>::iterator it = robotNodes.begin(); it != robotNodes.end(); it++)
- {
- std::string jointName = (*it)->getName();
- setDataField("positions", jointName, FramedPositionPtr(new FramedPosition((*it)->getGlobalPose(), GlobalFrame, robot->getName())));
- setDataField("orientations", jointName, FramedOrientationPtr(new FramedOrientation((*it)->getGlobalPose(), GlobalFrame, robot->getName())));
}
}
+
PropertyDefinitionsPtr RobotStateObserver::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new RobotStateObserverPropertyDefinitions(
- getConfigIdentifier()));
+ getConfigIdentifier()));
+}
+
+void RobotStateObserver::setRobot(RobotPtr robot)
+{
+ ScopedRecursiveLock lock(dataMutex);
+ this->robot = robot;
+
+ std::vector< VirtualRobot::RobotNodeSetPtr > robotNodes;
+ robotNodes = robot->getRobotNodeSets();
+
+ std::string nodesetsString = getProperty<std::string>("TCPsToReport").getValue();
+ if(!nodesetsString.empty())
+ {
+ if(nodesetsString=="*")
+ {
+ auto nodesets = robot->getRobotNodeSets();
+ for(RobotNodeSetPtr& set : nodesets)
+ {
+ if(set->getTCP())
+ nodesToReport.push_back(set->getTCP());
+ }
+ }
+ else
+ {
+ std::vector<std::string> nodesetNames;
+ boost::split(nodesetNames,
+ nodesetsString,
+ boost::is_any_of(","),
+ boost::token_compress_on);
+ for(auto name : nodesetNames)
+ {
+ boost::trim(name);
+ auto node = robot->getRobotNode(name);
+
+ if(node)
+ nodesToReport.push_back(node);
+ else
+ ARMARX_ERROR << "Could not find node set with name: " << name;
+ }
+ }
+ }
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
index 3e75142fb..fbd074b6d 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RobotStateObserver.h
@@ -56,12 +56,12 @@ namespace armarx
RobotStateObserverPropertyDefinitions(std::string prefix):
ComponentPropertyDefinitions(prefix)
{
- defineRequiredProperty<std::string>("RobotNodeSetName","Set of nodes that is controlled by the KinematicUnit");
- defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
- defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+ defineOptionalProperty<std::string>("TCPsToReport", "", "comma seperated list of nodesets' endeffectors, which poses and velocities that should be reported. * for all, empty for none");
}
};
+ typedef ::std::map< ::std::string, ::armarx::FramedPoseBasePtr> FramedPoseBaseMap;
+
/**
* ArmarX RobotStateObserver.
*
@@ -76,34 +76,37 @@ namespace armarx
virtual public RobotStateObserverInterface
{
public:
+ RobotStateObserver();
// framework hooks
void onInitObserver();
void onConnectObserver();
- // slice interface implementation
- void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
- void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
- void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c = Ice::Current());
-
virtual PropertyDefinitionsPtr createPropertyDefinitions();
+ void setRobot(VirtualRobot::RobotPtr robot);
+
virtual std::string getDefaultName() const { return "RobotStateObserver"; }
+ void updatePoses();
+ void updateNodeVelocities(const NameValueMap &jointVel);
protected:
- void updateRobotModel(const NameValueMap& nameValueMap);
- void updatePoses();
+ void updateVelocityDatafields(const FramedVector3Map &tcpTranslationVelocities, const FramedVector3Map &tcpOrientationVelocities);
+
+ void udpatePoseDatafields(const FramedPoseBaseMap &poseMap);
private:
std::string robotNodeSetName;
RobotStateComponentInterfacePrx server;
- VirtualRobot::RobotPtr robot;
- std::map<std::string,VirtualRobot::RobotNodePtr> namesToNodes;
+ VirtualRobot::RobotPtr robot, velocityReportRobot;
+ std::vector<VirtualRobot::RobotNodePtr > nodesToReport;
+ RecursiveMutex dataMutex;
+
+ // RobotStateObserverInterface interface
+ public:
+ DatafieldRefBasePtr getPoseDatafield(const std::string &nodeName, const Ice::Current &) const;
};
+
+ typedef IceInternal::Handle<RobotStateObserver> RobotStateObserverPtr;
}
#endif
--
GitLab