From 4c668234bbf30df66a642756b44e7409dd7a6f46 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 11 May 2021 12:00:52 +0200
Subject: [PATCH] all segments with timestamp lookup
---
.../armem_robot_state/server/common/Visu.cpp | 12 +++++++-----
.../armem_robot_state/server/description/Segment.cpp | 2 +-
.../armem_robot_state/server/description/Segment.h | 2 +-
.../server/proprioception/Segment.cpp | 2 +-
.../server/proprioception/Segment.h | 2 +-
5 files changed, 11 insertions(+), 9 deletions(-)
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index 43af7dbcd..cdaa6d236 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -186,15 +186,17 @@ namespace armarx::armem::server::robot_state
if (p.enabled)
{
- // TIMING_START(Visu);
+ TIMING_START(Visu);
// TODO(fabian.reister): use timestamp
+ const auto timestamp = IceUtil::Time::now();
+
try
{
- const auto robotDescriptions = descriptionSegment.getRobotDescriptions();
- const auto globalRobotPoseMap = localizationSegment.getRobotGlobalPoses();
- const auto robotJointPositionMap = proprioceptionSegment.getRobotJointPositions();
+ const auto robotDescriptions = descriptionSegment.getRobotDescriptions(timestamp);
+ const auto globalRobotPoseMap = localizationSegment.getRobotGlobalPoses(timestamp);
+ const auto robotJointPositionMap = proprioceptionSegment.getRobotJointPositions(timestamp);
// we need all 3 informations:
@@ -216,7 +218,7 @@ namespace armarx::armem::server::robot_state
ARMARX_DEBUG << "Done committing";
- // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
+ TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
// if (debugObserver)
// {
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index f00a970c1..011d37e33 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -105,7 +105,7 @@ namespace armarx::armem::server::robot_state::description
storeRobotDescription(robotDescription);
}
- Segment::RobotDescriptionMap Segment::getRobotDescriptions() const
+ Segment::RobotDescriptionMap Segment::getRobotDescriptions(const armem::Time& timestamp) const
{
RobotDescriptionMap robotDescriptions;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
index 8bb9ec017..21bfd98fe 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.h
@@ -72,7 +72,7 @@ namespace armarx::armem::server::robot_state::description
/// mapping "robot name" -> "robot description"
using RobotDescriptionMap = std::unordered_map<std::string, robot::RobotDescription>;
- RobotDescriptionMap getRobotDescriptions() const;
+ RobotDescriptionMap getRobotDescriptions(const armem::Time& timestamp) const;
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 38fe69a91..500925f2d 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -89,7 +89,7 @@ namespace armarx::armem::server::robot_state::proprioception
robotUnitProviderID.providerSegmentName = providerSegmentName;
}
- std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions() const
+ std::unordered_map<std::string, std::map<std::string, float>> Segment::getRobotJointPositions(const armem::Time& timestamp) const
{
std::unordered_map<std::string, std::map<std::string, float>> jointMap;
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
index 09bae1077..bc83a946b 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h
@@ -68,7 +68,7 @@ namespace armarx::armem::server::robot_state::proprioception
void init();
- std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions() const;
+ std::unordered_map<std::string, std::map<std::string, float>> getRobotJointPositions(const armem::Time& timestamp) const;
const armem::MemoryID& getRobotUnitProviderID() const;
--
GitLab