Fixed RobotStateObserver, replaces now TCPControlUnitObserver

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)

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 );
         }
     };
 }

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

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;
 
 
     };

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);
     };
 };
 

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 &currentPose = 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 &currentPose = 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 &currentPose = 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;
+            }
+        }
+    }
 }

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