From 45ee816cf9d1938825e224eda6eb9159fde5ff9b Mon Sep 17 00:00:00 2001
From: Patrick Hegemann <335495-patrickhegemann@users.noreply.gitlab.com>
Date: Wed, 9 Feb 2022 10:41:36 +0100
Subject: [PATCH] Use correct timestamp in getGlobalRobotPose
---
.../armem/server/RobotStateMemory/RobotStateMemory.cpp | 10 ++++++++--
.../armem/server/RobotStateMemory/RobotStateMemory.h | 2 +-
source/RobotAPI/interface/core/RobotLocalization.ice | 1 +
3 files changed, 10 insertions(+), 3 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 5de9bc3fb..f8ba26cd1 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -21,6 +21,7 @@
*/
#include "RobotStateMemory.h"
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include <RobotAPI/interface/core/PoseBase.h>
#include <RobotAPI/libraries/core/Pose.h>
@@ -33,7 +34,10 @@
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <ArmarXCore/core/logging/Logging.h>
+#include <IceUtil/Time.h>
+
#include <SimoxUtility/algorithm/get_map_keys_values.h>
+#include <SimoxUtility/algorithm/contains.h>
#include <SimoxUtility/algorithm/string.h>
@@ -187,9 +191,11 @@ namespace armarx::armem::server::robot_state
armarx::PoseBasePtr RobotStateMemory::getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current& /*unused*/)
{
- auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::now());
+ auto poseMap = localizationSegment.getRobotGlobalPoses(armem::Time::microSeconds(timestamp));
- bool robotNameFound = !(poseMap.find(robotName) == poseMap.end());
+ ARMARX_DEBUG << "Getting robot pose of robot " << robotName << " at timestamp " << armem::Time::microSeconds(timestamp);
+
+ bool robotNameFound = simox::alg::contains_key(poseMap, robotName);
ARMARX_CHECK(robotNameFound)
<< "Robot with name " << robotName << " does not exist. "
<< "Available robots are: " << simox::alg::get_keys(poseMap);
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index bd7eceda9..d40249681 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -77,7 +77,7 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
- virtual armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
+ armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
protected:
diff --git a/source/RobotAPI/interface/core/RobotLocalization.ice b/source/RobotAPI/interface/core/RobotLocalization.ice
index 19b0ebae4..6e5b38a32 100644
--- a/source/RobotAPI/interface/core/RobotLocalization.ice
+++ b/source/RobotAPI/interface/core/RobotLocalization.ice
@@ -30,6 +30,7 @@
module armarx{
interface GlobalRobotPoseProvider extends armem::server::MemoryInterface {
+ // timestamp in microseconds
PoseBase getGlobalRobotPose(long timestamp, string robotName);
}
--
GitLab