From da091486a8fb1e594d2e1c236308ceb54c874610 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 17 Sep 2019 17:06:46 +0200
Subject: [PATCH] Return actually used timestamps
---
.../RobotState/RobotStateComponent.cpp | 122 +++++++++++-------
.../RobotState/RobotStateComponent.h | 12 +-
2 files changed, 86 insertions(+), 48 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index f76939c27..545a9bd0a 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -24,21 +24,24 @@
#include "RobotStateComponent.h"
#include <boost/foreach.hpp>
-#include <VirtualRobot/XML/RobotIO.h>
-#include <VirtualRobot/Nodes/RobotNode.h>
+#include <boost/format.hpp>
+
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
-#include <boost/format.hpp>
-#include <boost/foreach.hpp>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/RobotIO.h>
+
#include <ArmarXCore/core/ArmarXManager.h>
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/application/Application.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/time/TimeUtil.h>
+
-using namespace VirtualRobot;
using namespace Eigen;
using namespace Ice;
+using namespace VirtualRobot;
namespace armarx
@@ -227,7 +230,7 @@ namespace armarx
SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(double _time, const Current&)
{
- IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
+ const IceUtil::Time time = IceUtil::Time::microSecondsDouble(_time);
if (!_synchronized)
{
@@ -256,7 +259,7 @@ namespace armarx
NameValueMap RobotStateComponent::getJointConfigAtTimestamp(double timestamp, const Current&) const
{
auto jointAngles = interpolateJoints(IceUtil::Time::microSecondsDouble(timestamp));
- return jointAngles ? *jointAngles : NameValueMap();
+ return jointAngles ? jointAngles->value : NameValueMap();
}
@@ -346,6 +349,7 @@ namespace armarx
}
}
+
void RobotStateComponent::reportNewTargetPose(
Float newPlatformPositionX, Float newPlatformPositionY, Float newPlatformRotation,
const Current&)
@@ -354,6 +358,7 @@ namespace armarx
(void) newPlatformPositionX, (void) newPlatformPositionY, (void) newPlatformRotation;
}
+
void RobotStateComponent::reportPlatformVelocity(
Float currentPlatformVelocityX, Float currentPlatformVelocityY, Float currentPlatformVelocityRotation,
const Current&)
@@ -362,12 +367,14 @@ namespace armarx
(void) currentPlatformVelocityX, (void) currentPlatformVelocityY, (void) currentPlatformVelocityRotation;
}
+
void RobotStateComponent::reportPlatformOdometryPose(Float x, Float y, Float angle, const Current&)
{
// Unused.
(void) x, (void) y, (void) angle;
}
+
std::vector<std::string> RobotStateComponent::getArmarXPackages(const Current&) const
{
std::vector<std::string> result;
@@ -469,13 +476,19 @@ namespace armarx
void RobotStateComponent::insertPose(IceUtil::Time timestamp, const Matrix4f& globalPose)
{
- boost::unique_lock<SharedMutex> lock(poseHistoryMutex);
- poseHistory.emplace_hint(poseHistory.end(),
- timestamp, new FramedPose(globalPose, GlobalFrame, ""));
-
- if (poseHistory.size() > poseHistoryLength)
+ IceUtil::Time duration;
{
- poseHistory.erase(poseHistory.begin());
+ IceUtil::Time start = IceUtil::Time::now();
+ boost::unique_lock<SharedMutex> lock(poseHistoryMutex);
+ duration = IceUtil::Time::now() - start;
+
+ poseHistory.emplace_hint(poseHistory.end(),
+ timestamp, new FramedPose(globalPose, GlobalFrame, ""));
+
+ if (poseHistory.size() > poseHistoryLength)
+ {
+ poseHistory.erase(poseHistory.begin());
+ }
}
if (robotStateObs)
@@ -492,9 +505,11 @@ namespace armarx
if (joints && globalPose)
{
RobotStateConfig config;
- config.timestamp = time.toMicroSeconds();
- config.jointMap = *joints;
- config.globalPose = *globalPose;
+ // Use time stamp from interpolation.
+ // config.timestamp = time.toMicroSeconds();
+ config.timestamp = std::min(joints->timestamp, globalPose->timestamp).toMicroSeconds();
+ config.jointMap = joints->value;
+ config.globalPose = globalPose->value;
return std::move(config);
}
else
@@ -503,7 +518,7 @@ namespace armarx
}
}
- std::optional<NameValueMap> RobotStateComponent::interpolateJoints(IceUtil::Time time) const
+ auto RobotStateComponent::interpolateJoints(IceUtil::Time time) const -> std::optional<Timestamped<NameValueMap>>
{
boost::shared_lock<SharedMutex> lock(jointHistoryMutex);
if (jointHistory.empty())
@@ -516,15 +531,24 @@ namespace armarx
if (time > newestTimeInHistory)
{
IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
- if (time > newestTimeInHistory + maxOffset)
+ if (time <= newestTimeInHistory + maxOffset)
{
- ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
+ ARMARX_INFO << deactivateSpam(5)
+ << "Requested joint timestamp is newer than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
+ << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Requested joint timestamp is substantially newer (>"
<< maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
- << "\nrequested timestamp: " << time.toDateTime()
- << " newest timestamp: " << newestTimeInHistory.toDateTime();
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
return std::nullopt;
}
- return jointHistory.rbegin()->second;
+
+ return Timestamped<NameValueMap> {jointHistory.rbegin()->first, jointHistory.rbegin()->second};
}
// Get the oldest entry whose time >= time.
@@ -532,21 +556,19 @@ namespace armarx
if (nextIt == jointHistory.end())
{
ARMARX_WARNING << deactivateSpam(1)
- << "Could not find or interpolate a robot joint angles for time "
- << time.toDateTime()
- << "\n - oldest available value: " << jointHistory.begin()->first.toDateTime()
- << "\n - newest available value: " << newestTimeInHistory.toDateTime();
+ << "Could not find or interpolate a robot joint angles for time " << time.toDateTime()
+ << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+ << "\n- newest available value: " << newestTimeInHistory.toDateTime();
return std::nullopt;
}
- const NameValueMap& next = nextIt->second;
-
if (nextIt == jointHistory.begin())
{
// Next was oldest entry.
- return next;
+ return Timestamped<NameValueMap> {nextIt->first, nextIt->second};
}
+ const NameValueMap& next = nextIt->second;
auto prevIt = std::prev(nextIt);
@@ -568,13 +590,12 @@ namespace armarx
jointAngles.emplace(name, value);
}
- return std::move(jointAngles);
+ return Timestamped<NameValueMap> {time, std::move(jointAngles)};
}
- std::optional<FramedPosePtr> RobotStateComponent::interpolatePose(IceUtil::Time time) const
+ auto RobotStateComponent::interpolatePose(IceUtil::Time time) const -> std::optional<Timestamped<FramedPosePtr>>
{
boost::shared_lock<SharedMutex> lock(poseHistoryMutex);
- // Get the older newer entry of time.
if (poseHistory.empty())
{
@@ -582,7 +603,9 @@ namespace armarx
<< "Pose history is empty. This can happen if there is no platform unit.";
ReadLockPtr readLock = _synchronized->getReadLock();
- return new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
+
+ FramedPosePtr pose = new FramedPose(_synchronized->getGlobalPose(), armarx::GlobalFrame, "");
+ return Timestamped<FramedPosePtr> {IceUtil::Time::seconds(0), pose};
}
@@ -590,15 +613,23 @@ namespace armarx
if (time > newestTimeInHistory)
{
IceUtil::Time maxOffset = IceUtil::Time::seconds(2);
- if (time > newestTimeInHistory + maxOffset)
+ if (time <= newestTimeInHistory + maxOffset)
{
- ARMARX_WARNING << deactivateSpam(1) << "Requested timestamp is substantially newer (>"
+ ARMARX_INFO << deactivateSpam(5)
+ << "Requested pose timestamp is newer than newest available timestamp!"
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime()
+ << "\n- difference: \t" << (time - newestTimeInHistory).toMicroSeconds() << " us";
+ }
+ else
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Requested pose timestamp is substantially newer (>"
<< maxOffset.toSecondsDouble() << " sec) than newest available timestamp!"
- << "\nrequested timestamp: " << time.toDateTime()
- << " newest timestamp: " << newestTimeInHistory.toDateTime();
+ << "\n- requested timestamp: \t" << time.toDateTime()
+ << "\n- newest timestamp: \t" << newestTimeInHistory.toDateTime();
return std::nullopt;
}
- return poseHistory.rbegin()->second;
+ return Timestamped<FramedPosePtr> {poseHistory.rbegin()->first, poseHistory.rbegin()->second};
}
@@ -607,23 +638,22 @@ namespace armarx
{
ARMARX_WARNING << deactivateSpam(1)
<< "Could not find or interpolate platform pose for time " << time.toDateTime()
- << "\n - oldest available value: " << jointHistory.begin()->first.toDateTime()
- << "\n - newest available value: " << newestTimeInHistory.toDateTime();
+ << "\n- oldest available value: " << jointHistory.begin()->first.toDateTime()
+ << "\n- newest available value: " << newestTimeInHistory.toDateTime();
return std::nullopt;
}
- const FramedPosePtr& next = nextIt->second;
-
if (nextIt == poseHistory.begin())
{
// Next was oldest entry.
- return next;
+ return Timestamped<FramedPosePtr> {nextIt->first, nextIt->second};
}
auto prevIt = std::prev(nextIt);
-
ARMARX_CHECK_EXPRESSION(prevIt->second);
+
+ const FramedPosePtr& next = nextIt->second;
const FramedPosePtr& prev = prevIt->second;
@@ -650,7 +680,7 @@ namespace armarx
math::Helpers::Orientation(globalPose) = rotNew.toRotationMatrix();
- return new FramedPose(globalPose, armarx::GlobalFrame, "");
+ return Timestamped<FramedPosePtr> {time, new FramedPose(globalPose, armarx::GlobalFrame, "")};
}
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index e0fd01378..51f688b3f 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -181,12 +181,20 @@ namespace armarx
void insertPose(IceUtil::Time timestamp, const Eigen::Matrix4f& globalPose);
+
+ template <class ValueT>
+ struct Timestamped
+ {
+ IceUtil::Time timestamp;
+ ValueT value;
+ };
+
/// Interpolate the robot state from histories and store it in `config`.
std::optional<RobotStateConfig> interpolate(IceUtil::Time time) const;
/// Interpolate the joint angles from history and store it in `jointAngles`.
- std::optional<NameValueMap> interpolateJoints(IceUtil::Time time) const;
+ std::optional<Timestamped<NameValueMap>> interpolateJoints(IceUtil::Time time) const;
/// Interpolate the robot pose from history and store it in `pose`.
- std::optional<FramedPosePtr> interpolatePose(IceUtil::Time time) const;
+ std::optional<Timestamped<FramedPosePtr>> interpolatePose(IceUtil::Time time) const;
private:
--
GitLab