all segments with timestamp lookup

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)
                         // {

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;

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:
 

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;
 

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;