From 9d7ee0bc8da3bf3f7c2fb83206b2d8f6b84a0fee Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 24 Feb 2016 17:00:11 +0100
Subject: [PATCH] added timestamps to kinematic unit report functions
---
.../RobotState/RobotStateComponent.cpp | 31 +++++++------
.../RobotState/RobotStateComponent.h | 25 ++++++-----
.../units/KinematicUnitObserver.cpp | 44 ++++++++++---------
.../components/units/KinematicUnitObserver.h | 18 ++++----
.../units/KinematicUnitSimulation.cpp | 14 +++---
.../components/units/TCPControlUnit.cpp | 38 ++++++++++------
.../components/units/TCPControlUnit.h | 16 +++----
.../KinematicUnitGuiPlugin.cpp | 16 +++----
.../KinematicUnitGuiPlugin.h | 16 +++----
source/RobotAPI/interface/core/RobotState.ice | 4 +-
.../units/KinematicUnitInterface.ice | 16 +++----
11 files changed, 129 insertions(+), 109 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index b19b39b26..9966218f2 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -266,10 +266,13 @@ namespace armarx
return true;
}
- void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Current& c)
+ void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current& c)
{
// IceUtil::Time start = IceUtil::Time::now();
- auto timestamp = IceUtil::Time::now();
+ if (timestamp <= 0)
+ {
+ timestamp = IceUtil::Time::now().toMicroSeconds();
+ }
if (aValueChanged)
{
{
@@ -294,13 +297,13 @@ namespace armarx
if (_sharedRobotServant)
{
- _sharedRobotServant->setTimestamp(timestamp);
+ _sharedRobotServant->setTimestamp(IceUtil::Time::microSeconds(timestamp));
}
- history[timestamp.toMicroSeconds()] = {timestamp.toMicroSeconds(),
- new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
- jointAngles
- };
+ history[timestamp] = {timestamp,
+ new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
+ jointAngles
+ };
if (history.size() > historyLength)
{
@@ -335,23 +338,23 @@ namespace armarx
return result;
}
- void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Current& c) {}
- void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Current& c)
+ void RobotStateComponent::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Current& c)
{
if (robotStateObs)
{
robotStateObs->updateNodeVelocities(jointVelocities);
}
}
- void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
- void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Current& c)
+ void RobotStateComponent::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Current& c)
{
}
- void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Current& c) {}
- void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Current& c) {}
- void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
+ void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Current& c) {}
PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index b7f35b5f9..aa626e121 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -89,10 +89,14 @@ namespace armarx
virtual SharedRobotInterfacePrx getSynchronizedRobot(const Ice::Current&) const;
/**
- * \return Clone of the internal synchronized robot.
+ * \return Clone of the internal synchronized robot fixed to the configuration from the time of calling this function.
*/
virtual SharedRobotInterfacePrx getRobotSnapshot(const std::string& time, const Ice::Current&);
+ SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(Ice::Long time, const Ice::Current& current);
+ NameValueMap getJointConfigAtTimestamp(Ice::Long, const Ice::Current&) const;
+ RobotStateConfig getRobotStateAtTimestamp(Ice::Long timestamp, const Ice::Current&) const;
+
/**
* \return the robot xml filename as specified in the configuration
*/
@@ -126,9 +130,6 @@ namespace armarx
return "RobotStateComponent";
}
void setRobotStateObserver(RobotStateObserverPtr observer);
- SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(Ice::Long time, const Ice::Current& current);
- NameValueMap getJointConfigAtTimestamp(Ice::Long, const Ice::Current&) const;
- RobotStateConfig getRobotStateAtTimestamp(Ice::Long timestamp, const Ice::Current&) const;
bool interpolate(long time, RobotStateConfig& config) const;
protected:
@@ -149,14 +150,14 @@ namespace armarx
virtual ~RobotStateComponent();
// inherited from KinematicUnitInterface
- 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());
+ void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
private:
SharedRobotInterfacePrx _synchronizedPrx;
SharedRobotServantPtr _sharedRobotServant;
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index dc4c2776f..345e3efc6 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
@@ -126,12 +126,10 @@ void KinematicUnitObserver::onConnectObserver()
// ********************************************************************
// KinematicUnitListener interface implementation
// ********************************************************************
-void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
try
{
-
-
if (jointModes.size() == 0)
{
return;
@@ -150,7 +148,7 @@ void KinematicUnitObserver::reportControlModeChanged(const NameControlModeMap& j
}
}
-void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
try
{
@@ -161,7 +159,7 @@ void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles,
return;
}
- nameValueMapToDataFields("jointangles", jointAngles);
+ nameValueMapToDataFields("jointangles", jointAngles, timestamp);
updateChannel("jointangles");
@@ -173,7 +171,7 @@ void KinematicUnitObserver::reportJointAngles(const NameValueMap& jointAngles,
}
-void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
try
{
@@ -182,7 +180,7 @@ void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVeloc
return;
}
- nameValueMapToDataFields("jointvelocities", jointVelocities);
+ nameValueMapToDataFields("jointvelocities", jointVelocities, timestamp);
updateChannel("jointvelocities");
}
catch (...)
@@ -191,7 +189,7 @@ void KinematicUnitObserver::reportJointVelocities(const NameValueMap& jointVeloc
}
}
-void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
try
{
@@ -200,7 +198,7 @@ void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques,
return;
}
- nameValueMapToDataFields("jointtorques", jointTorques);
+ nameValueMapToDataFields("jointtorques", jointTorques, timestamp);
updateChannel("jointtorques");
}
catch (...)
@@ -209,12 +207,12 @@ void KinematicUnitObserver::reportJointTorques(const NameValueMap& jointTorques,
}
}
-void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
}
-void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
try
{
@@ -223,7 +221,7 @@ void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrent
return;
}
- nameValueMapToDataFields("jointcurrents", jointCurrents);
+ nameValueMapToDataFields("jointcurrents", jointCurrents, timestamp);
updateChannel("jointcurrents");
}
catch (...)
@@ -232,7 +230,7 @@ void KinematicUnitObserver::reportJointCurrents(const NameValueMap& jointCurrent
}
}
-void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
try
{
@@ -241,7 +239,7 @@ void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& joi
return;
}
- nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures);
+ nameValueMapToDataFields("jointmotortemperatures", jointMotorTemperatures, timestamp);
updateChannel("jointmotortemperatures");
}
catch (...)
@@ -250,21 +248,27 @@ void KinematicUnitObserver::reportJointMotorTemperatures(const NameValueMap& joi
}
}
-void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitObserver::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
}
// ********************************************************************
// private methods
// ********************************************************************
-void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap)
+void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp)
{
- NameValueMap::const_iterator iter = nameValueMap.begin();
+ // ARMARX_INFO << deactivateSpam(10) << " timestamp is " << (IceUtil::Time::now() - IceUtil::Time::microSeconds(timestamp)).toMicroSecondsDouble() << " µs old";
StringVariantBaseMap map;
- while (iter != nameValueMap.end())
+ for (const auto & it : nameValueMap)
{
- map[iter->first] = new Variant(iter->second);
- iter++;
+ if (timestamp < 0)
+ {
+ map[it.first] = new Variant(it.second);
+ }
+ else
+ {
+ map[it.first] = new TimedVariant(it.second, IceUtil::Time::microSecondsDouble(timestamp));
+ }
}
setDataFieldsFlatCopy(channelName, map);
}
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index ecbc321b7..8272327b7 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -63,14 +63,14 @@ namespace armarx
// 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());
+ void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
+ void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c = ::Ice::Current());
virtual std::string getDefaultName() const
{
@@ -138,7 +138,7 @@ namespace armarx
}
protected:
- void nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap);
+ void nameValueMapToDataFields(const std::string& channelName, const NameValueMap& nameValueMap, Ice::Long timestamp);
private:
std::string robotNodeSetName;
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 411fb9bf1..819cbcd3e 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -158,9 +158,9 @@ void KinematicUnitSimulation::simulationFunction()
previousJointStates = jointStates;
}
-
- listenerPrx->reportJointAngles(actualJointAngles, aPosValueChanged);
- listenerPrx->reportJointVelocities(actualJointVelocities, aVelValueChanged);
+ auto timestamp = IceUtil::Time::now().toMicroSeconds();
+ listenerPrx->reportJointAngles(actualJointAngles, timestamp, aPosValueChanged);
+ listenerPrx->reportJointVelocities(actualJointVelocities, timestamp, aVelValueChanged);
}
void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
@@ -196,7 +196,7 @@ void KinematicUnitSimulation::switchControlMode(const NameControlModeMap& target
}
}
// only report control modes which have been changed
- listenerPrx->reportControlModeChanged(changedControlModes, aValueChanged);
+ listenerPrx->reportControlModeChanged(changedControlModes, aValueChanged, IceUtil::Time::now().toMicroSeconds());
}
@@ -241,7 +241,7 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
if (aValueChanged)
{
- listenerPrx->reportJointAngles(actualJointAngles, aValueChanged);
+ listenerPrx->reportJointAngles(actualJointAngles, aValueChanged, IceUtil::Time::now().toMicroSeconds());
}
}
@@ -274,7 +274,7 @@ void KinematicUnitSimulation::setJointVelocities(const NameValueMap& targetJoint
if (aValueChanged)
{
- listenerPrx->reportJointVelocities(actualJointVelocities, aValueChanged);
+ listenerPrx->reportJointVelocities(actualJointVelocities, aValueChanged, IceUtil::Time::now().toMicroSeconds());
}
}
@@ -307,7 +307,7 @@ void KinematicUnitSimulation::setJointTorques(const NameValueMap& targetJointTor
if (aValueChanged)
{
- listenerPrx->reportJointTorques(actualJointTorques, aValueChanged);
+ listenerPrx->reportJointTorques(actualJointTorques, aValueChanged, IceUtil::Time::now().toMicroSeconds());
}
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.cpp b/source/RobotAPI/components/units/TCPControlUnit.cpp
index f21bd0df2..6af845d80 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.cpp
+++ b/source/RobotAPI/components/units/TCPControlUnit.cpp
@@ -88,7 +88,7 @@ namespace armarx
{
auto nodesets = localReportRobot->getRobotNodeSets();
- for (RobotNodeSetPtr& set : nodesets)
+ for (RobotNodeSetPtr & set : nodesets)
{
if (set->getTCP())
{
@@ -104,7 +104,7 @@ namespace armarx
boost::is_any_of(","),
boost::token_compress_on);
- for (auto& name : nodesetNames)
+ for (auto & name : nodesetNames)
{
auto node = localReportRobot->getRobotNode(name);
@@ -1339,17 +1339,26 @@ namespace armarx
-void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&)
{
ScopedLock lock(reportMutex);
FramedPoseBaseMap tcpPoses;
std::string rootFrame = localReportRobot->getRootNode()->getName();
- localReportRobot->setJointValues(jointAngles);
-
+ auto it = jointAngles.find("timestamp");
+ if (it == jointAngles.end())
+ {
+ localReportRobot->setJointValues(jointAngles);
+ }
+ else
+ {
+ NameValueMap tempMap = jointAngles;
+ tempMap.erase("timestamp");
+ localReportRobot->setJointValues(tempMap);
+ }
//IceUtil::Time start = IceUtil::Time::now();
for (unsigned int i = 0; i < nodesToReport.size(); i++)
{
@@ -1364,7 +1373,7 @@ void armarx::TCPControlUnit::reportJointAngles(const NameValueMap& jointAngles,
listener->reportTCPPose(tcpPoses);
}
-void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel, Ice::Long timestamp, bool, const Ice::Current&)
{
if ((IceUtil::Time::now() - lastTopicReportTime).toMilliSeconds() < cycleTime)
{
@@ -1400,9 +1409,12 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
for (NameValueMap::iterator it = tempJointAngles.begin(); it != tempJointAngles.end(); it++)
{
NameValueMap::const_iterator itSrc = jointVel.find(it->first);
-
if (itSrc != jointVel.end())
{
+ if (itSrc->first == "timestamp")
+ {
+ continue;
+ }
it->second += itSrc->second * tDelta;
}
}
@@ -1436,23 +1448,23 @@ void armarx::TCPControlUnit::reportJointVelocities(const NameValueMap& jointVel,
listener->reportTCPVelocities(tcpTranslationVelocities, tcpOrientationVelocities);
}
-void armarx::TCPControlUnit::reportJointTorques(const NameValueMap&, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c)
+void armarx::TCPControlUnit::reportJointAccelerations(const armarx::NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
}
-void armarx::TCPControlUnit::reportJointCurrents(const NameValueMap&, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap&, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&)
{
}
-void armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap&, bool, const Ice::Current&)
+void armarx::TCPControlUnit::reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current&)
{
}
diff --git a/source/RobotAPI/components/units/TCPControlUnit.h b/source/RobotAPI/components/units/TCPControlUnit.h
index 10d8bde11..5475c6b2a 100644
--- a/source/RobotAPI/components/units/TCPControlUnit.h
+++ b/source/RobotAPI/components/units/TCPControlUnit.h
@@ -227,14 +227,14 @@ namespace armarx
// KinematicUnitListener interface
protected:
- void reportControlModeChanged(const NameControlModeMap&, bool, const Ice::Current&);
- void reportJointAngles(const NameValueMap& jointAngles, bool, const Ice::Current&);
- void reportJointVelocities(const NameValueMap& jointVel, bool, const Ice::Current&);
- void reportJointTorques(const NameValueMap&, bool, const Ice::Current&);
- void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
- void reportJointCurrents(const NameValueMap&, bool, const Ice::Current&);
- void reportJointMotorTemperatures(const NameValueMap&, bool, const Ice::Current&);
- void reportJointStatuses(const NameStatusMap&, bool, const Ice::Current&);
+ void reportControlModeChanged(const NameControlModeMap&, Ice::Long timestamp, bool, const Ice::Current&);
+ void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool, const Ice::Current&);
+ void reportJointVelocities(const NameValueMap& jointVel, Ice::Long timestamp, bool, const Ice::Current&);
+ void reportJointTorques(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&);
+ void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointCurrents(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&);
+ void reportJointMotorTemperatures(const NameValueMap&, Ice::Long timestamp, bool, const Ice::Current&);
+ void reportJointStatuses(const NameStatusMap&, Ice::Long timestamp, bool, const Ice::Current&);
};
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
index fc779ad5e..72df5f416 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp
@@ -1098,7 +1098,7 @@ void KinematicUnitWidgetController::updateJointMotorTemperaturesTable()
}
}
-void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointAngles.size() > 0)
{
@@ -1110,7 +1110,7 @@ void KinematicUnitWidgetController::reportJointAngles(const NameValueMap& jointA
emit jointAnglesReported();
}
-void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointVelocities.size() > 0)
{
@@ -1122,7 +1122,7 @@ void KinematicUnitWidgetController::reportJointVelocities(const NameValueMap& jo
emit jointVelocitiesReported();
}
-void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointTorques.size() > 0)
{
@@ -1135,12 +1135,12 @@ void KinematicUnitWidgetController::reportJointTorques(const NameValueMap& joint
emit jointTorquesReported();
}
-void KinematicUnitWidgetController::reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
}
-void KinematicUnitWidgetController::reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
// if(!aValueChanged && reportedJointControlModes.size() > 0)
// return;
@@ -1156,7 +1156,7 @@ void KinematicUnitWidgetController::reportControlModeChanged(const NameControlMo
emit jointControlModesReported();
}
-void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointCurrents.size() > 0)
{
@@ -1169,7 +1169,7 @@ void KinematicUnitWidgetController::reportJointCurrents(const NameValueMap& join
emit jointCurrentsReported();
}
-void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c)
+void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c)
{
if (!aValueChanged && reportedJointMotorTemperatures.size() > 0)
{
@@ -1182,7 +1182,7 @@ void KinematicUnitWidgetController::reportJointMotorTemperatures(const NameValue
emit jointMotorTemperaturesReported();
}
-void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current&)
+void KinematicUnitWidgetController::reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current&)
{
if (!aValueChanged && reportedJointStatuses.size() > 0)
{
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
index b96569876..a997f684d 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h
@@ -246,14 +246,14 @@ namespace armarx
armarx::DebugDrawerComponentPtr debugDrawer;
// interface implementation
- void reportControlModeChanged(const NameControlModeMap& jointModes, bool aValueChanged, const Ice::Current& c);
- void reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Ice::Current& c);
- void reportJointVelocities(const NameValueMap& jointVelocities, bool aValueChanged, const Ice::Current& c);
- void reportJointTorques(const NameValueMap& jointTorques, bool aValueChanged, const Ice::Current& c);
- void reportJointAccelerations(const NameValueMap& jointAccelerations, bool aValueChanged, const Ice::Current& c);
- void reportJointCurrents(const NameValueMap& jointCurrents, bool aValueChanged, const Ice::Current& c);
- void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Ice::Current& c);
- void reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Ice::Current&);
+ void reportControlModeChanged(const NameControlModeMap& jointModes, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointVelocities(const NameValueMap& jointVelocities, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointTorques(const NameValueMap& jointTorques, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointAccelerations(const NameValueMap& jointAccelerations, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointCurrents(const NameValueMap& jointCurrents, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, Ice::Long timestamp, bool aValueChanged, const Ice::Current& c);
+ void reportJointStatuses(const NameStatusMap& jointStatuses, Ice::Long timestamp, bool aValueChanged, const Ice::Current&);
void updateModel();
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index f6e442055..bf49e7c30 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -151,7 +151,7 @@ module armarx
/**
* Return the joint values from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
- * @return Snapshot
+ * @return Jointname-jointvalue map
*
* */
["cpp:const"]
@@ -160,7 +160,7 @@ module armarx
/**
* Return the Robot configuration, including joint values and global pose of the root node, from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
- * @return Snapshot
+ * @return Robot configuration containing jointvalue map and globalpose
*
* */
["cpp:const"]
diff --git a/source/RobotAPI/interface/units/KinematicUnitInterface.ice b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
index 9b52ec9da..3d6fdb380 100644
--- a/source/RobotAPI/interface/units/KinematicUnitInterface.ice
+++ b/source/RobotAPI/interface/units/KinematicUnitInterface.ice
@@ -226,55 +226,55 @@ module armarx
* @param actualJointModes Map of control modes and corresponding joint names.
* @param aValueChanged Is set to true if a mode has changed.
**/
- void reportControlModeChanged(NameControlModeMap actualJointModes, bool aValueChanged);
+ void reportControlModeChanged(NameControlModeMap actualJointModes, long timestamp, bool aValueChanged);
/**
* reportJointAngles reports joint angle values.
* @param actualJointAngles Map of joint angle values and corresponding joint names.
* @param aValueChanged Is set to true if a joint angle value has changed.
**/
- void reportJointAngles(NameValueMap actualJointAngles, bool aValueChanged);
+ void reportJointAngles(NameValueMap actualJointAngles, long timestamp, bool aValueChanged);
/**
* reportJointVelocities reports joint angle velocities.
* @param actualJointVelocities Map of joint angle velocities and corresponding joint names.
* @param aValueChanged Is set to true if a joint angle velocity has changed.
**/
- void reportJointVelocities(NameValueMap actualJointVelocities, bool aValueChanged);
+ void reportJointVelocities(NameValueMap actualJointVelocities, long timestamp, bool aValueChanged);
/**
* reportJointTorques reports joint torques.
* @param actualJointTorques Map of joint torques and corresponding joint names.
* @param aValueChanged Is set to true if a joint torque has changed.
**/
- void reportJointTorques(NameValueMap actualJointTorques, bool aValueChanged);
+ void reportJointTorques(NameValueMap actualJointTorques, long timestamp, bool aValueChanged);
/**
* reportJointAccelerations reports joint accelerations.
* @param actualJointAccelerations Map of joint accelerations and corresponding joint names.
* @param aValueChanged Is set to true if a joint acceleration has changed.
**/
- void reportJointAccelerations(NameValueMap actualJointAccelerations, bool aValueChanged);
+ void reportJointAccelerations(NameValueMap actualJointAccelerations, long timestamp, bool aValueChanged);
/**
* reportJointCurrents reports joint currents.
* @param actualJointCurrents Map of joint currents and corresponding joint names.
* @param aValueChanged Is set to true if a joint current has changed.
**/
- void reportJointCurrents(NameValueMap actualJointCurrents, bool aValueChanged);
+ void reportJointCurrents(NameValueMap actualJointCurrents, long timestamp, bool aValueChanged);
/**
* reportJointMotorTemperatures reports joint motor temperatures.
* @param actualJointMotorTemperatures Map of joint motor temperatures and corresponding joint names.
* @param aValueChanged Is set to true if a joint motor temperature has changed.
**/
- void reportJointMotorTemperatures(NameValueMap actualJointMotorTemperatures, bool aValueChanged);
+ void reportJointMotorTemperatures(NameValueMap actualJointMotorTemperatures, long timestamp, bool aValueChanged);
/**
* reportJointStatuses reports current joint statuses.
* @param actualJointStatuses Map of joint statuses and corresponding joint names.
* @param aValueChanged Is set to true if a joint status has changed.
**/
- void reportJointStatuses(NameStatusMap actualJointStatuses, bool aValueChanged);
+ void reportJointStatuses(NameStatusMap actualJointStatuses, long timestamp, bool aValueChanged);
};
/*interface KinematicUnitHandInterface extends KinematicUnitInterface
--
GitLab