From 57c1a82287e4a925a75f3ad7bcb2c20fc4ef982f Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 23 Mar 2016 01:52:50 +0100
Subject: [PATCH] robotstatecomponent uses double-timestamp for history now
---
.../RobotState/RobotStateComponent.cpp | 35 +++++++++++--------
.../RobotState/RobotStateComponent.h | 10 +++---
.../RobotState/test/RobotStateTest.cpp | 4 +--
source/RobotAPI/interface/core/RobotState.ice | 8 ++---
4 files changed, 31 insertions(+), 26 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 447dca068..cedadbba4 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -170,14 +170,14 @@ namespace armarx
auto clone = this->_synchronized->clone(_synchronized->getName());
SharedRobotServantPtr p = new SharedRobotServant(clone);
- p->setTimestamp(IceUtil::Time::microSeconds(_sharedRobotServant->getTimestamp()->timestamp));
+ p->setTimestamp(IceUtil::Time::microSecondsDouble(_sharedRobotServant->getTimestamp()->timestamp));
auto result = getObjectAdapter()->addWithUUID(p);
// virtal robot clone is buggy -> set global pose here
p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
return SharedRobotInterfacePrx::uncheckedCast(result);
}
- SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(Ice::Long time, const Current& current)
+ SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double time, const Current& current)
{
if (!_synchronized)
{
@@ -195,7 +195,7 @@ namespace armarx
clone->setJointValues(config.jointMap);
clone->setGlobalPose(FramedPosePtr::dynamicCast(config.globalPose)->toEigen());
SharedRobotServantPtr p = new SharedRobotServant(clone);
- p->setTimestamp(IceUtil::Time::microSeconds(_sharedRobotServant->getTimestamp()->timestamp));
+ p->setTimestamp(IceUtil::Time::microSecondsDouble(time));
auto result = getObjectAdapter()->addWithUUID(p);
// virtal robot clone is buggy -> set global pose here
p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
@@ -205,7 +205,7 @@ namespace armarx
}
- NameValueMap RobotStateComponent::getJointConfigAtTimestamp(Ice::Long timestamp, const Current&) const
+ NameValueMap RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
{
RobotStateConfig config;
if (!interpolate(timestamp, config))
@@ -217,7 +217,7 @@ namespace armarx
}
- RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(Long timestamp, const Current&) const
+ RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(double timestamp, const Current&) const
{
RobotStateConfig config;
if (!interpolate(timestamp, config))
@@ -228,7 +228,7 @@ namespace armarx
return config;
}
- bool RobotStateComponent::interpolate(long time, RobotStateConfig& config) const
+ bool RobotStateComponent::interpolate(double time, RobotStateConfig& config) const
{
auto it = history.lower_bound(time);
@@ -243,15 +243,20 @@ namespace armarx
else
{
config = it->second;
- if (it->first != time)
+ if (it->first == time)
+ {
+// ARMARX_INFO_S << "No Interpolation needed: " << time;
+ }
+ else
{
// interpolate
auto prevIt = std::prev(it);
if (prevIt != history.end())
{
long deltaT = it->first - prevIt->first;
- float influenceNext = 1.0f - (float)(it->first - time) / deltaT;
- float influencePrev = 1.0f - (float)(time - prevIt->first) / deltaT;
+ double influenceNext = 1.0f - (double)(it->first - time) / deltaT;
+ double influencePrev = 1.0f - (double)(time - prevIt->first) / deltaT;
+// ARMARX_INFO_S << "Interpolating: " << time << " prev: " << time - prevIt->first << " next: " << it->first - time << VAROUT(influenceNext) << VAROUT(influencePrev) << " sum: " << (influenceNext + influencePrev);
auto jointIt = prevIt->second.jointMap.begin();
for (auto & joint : config.jointMap)
{
@@ -305,16 +310,16 @@ namespace armarx
robotStateObs->updatePoses();
}
}
-
+ auto time = IceUtil::Time::microSeconds(timestamp);
if (_sharedRobotServant)
{
- _sharedRobotServant->setTimestamp(IceUtil::Time::microSeconds(timestamp));
+ _sharedRobotServant->setTimestamp(time);
}
- history[timestamp] = {timestamp,
- new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
- jointAngles
- };
+ history[time.toMicroSecondsDouble()] = {time.toMicroSecondsDouble(),
+ new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
+ jointAngles
+ };
if (history.size() > historyLength)
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index f34b22478..828930463 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -96,9 +96,9 @@ namespace armarx
*/
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;
+ SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(double time, const Ice::Current& current);
+ NameValueMap getJointConfigAtTimestamp(double, const Ice::Current&) const;
+ RobotStateConfig getRobotStateAtTimestamp(double timestamp, const Ice::Current&) const;
/**
* \return the robot xml filename as specified in the configuration
@@ -134,7 +134,7 @@ namespace armarx
}
void setRobotStateObserver(RobotStateObserverPtr observer);
- bool interpolate(long time, RobotStateConfig& config) const;
+ bool interpolate(double time, RobotStateConfig& config) const;
float getScaling(const Ice::Current&) const;
protected:
@@ -171,7 +171,7 @@ namespace armarx
std::string robotFile;
std::string relativeRobotFile;
RobotStateObserverPtr robotStateObs;
- std::map<long, RobotStateConfig> history;
+ std::map<double, RobotStateConfig> history;
size_t historyLength;
std::string robotNodeSetName;
diff --git a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
index 46bd21639..74d791ec3 100644
--- a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
+++ b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
@@ -96,8 +96,8 @@ BOOST_AUTO_TEST_CASE(RobotStateComponentHistoryTest)
config = env._rsc->getJointConfigAtTimestamp(t2 + 1); // future timestamp -> latest values should be returned
BOOST_CHECK_EQUAL(config["Elbow L"], 1);
config = env._rsc->getJointConfigAtTimestamp((t1 + t2) * 0.5); // interpolated values in the middle
- BOOST_CHECK_CLOSE(config["Elbow L"], 0.5f, 1);
+ BOOST_CHECK_CLOSE(config["Elbow L"], 0.5f, 0.001);
config = env._rsc->getJointConfigAtTimestamp(t1 + (t2 - t1) * 0.7); // interpolated values
ARMARX_INFO_S << "value at t=0.7%: " << config["Elbow L"];
- BOOST_CHECK_CLOSE(config["Elbow L"], 0.7f, 1);
+ BOOST_CHECK_CLOSE(config["Elbow L"], 0.7f, 0.001);
}
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 211e62273..ecf90dbaf 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -43,7 +43,7 @@ module armarx
struct RobotStateConfig
{
- long timestamp;
+ double timestamp;
FramedPoseBase globalPose;
NameValueMap jointMap;
};
@@ -147,7 +147,7 @@ module armarx
* @return Snapshot
*
* */
- SharedRobotInterface* getRobotSnapshotAtTimestamp(long time) throws NotInitializedException;
+ SharedRobotInterface* getRobotSnapshotAtTimestamp(double time) throws NotInitializedException;
/**
* Return the joint values from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
@@ -156,7 +156,7 @@ module armarx
* */
["cpp:const"]
idempotent
- NameValueMap getJointConfigAtTimestamp(long time) throws NotInitializedException;
+ NameValueMap getJointConfigAtTimestamp(double time) throws NotInitializedException;
/**
* 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.
@@ -165,7 +165,7 @@ module armarx
* */
["cpp:const"]
idempotent
- RobotStateConfig getRobotStateAtTimestamp(long time) throws NotInitializedException;
+ RobotStateConfig getRobotStateAtTimestamp(double time) throws NotInitializedException;
/**
* @return the robot xml filename as specified in the configuration
--
GitLab