Draft: Make RobotStateMemory ready

Goal:

• Make stable (especially race conditions)
• Improve runtime
• Add profiling/monitoring
• Refactor code
• Add missing interfaces

source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp

@@ -10,12 +10,12 @@
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
 #include <SimoxUtility/math/pose.h>
 
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/CycleUtil.h"
+#include <ArmarXCore/core/logging/Logging.h>
+#include <ArmarXCore/core/time/CycleUtil.h>
 #include <ArmarXCore/core/time/TimeUtil.h>
 
-#include "RobotAPI/components/ArViz/Client/Elements.h"
-#include "RobotAPI/libraries/armem/core/Time.h"
+#include <RobotAPI/components/ArViz/Client/Elements.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
 #include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
 #include <exception>
 #include <string>
@@ -211,12 +211,20 @@ namespace armarx::armem::server::robot_state
 
                     try
                     {
+                        TIMING_START(tRobotDescriptions);
                         const auto robotDescriptions =
                             descriptionSegment.getRobotDescriptions(timestamp);
+                        TIMING_END_STREAM(tRobotDescriptions, ARMARX_VERBOSE);
+
+                        TIMING_START(tGlobalRobotPoseMap);
                         const auto globalRobotPoseMap =
                             localizationSegment.getRobotGlobalPoses(timestamp);
+                        TIMING_END_STREAM(tGlobalRobotPoseMap, ARMARX_VERBOSE);
+
+                        TIMING_START(tRobotJointPositionMap);
                         const auto robotJointPositionMap =
                             proprioceptionSegment.getRobotJointPositions(timestamp);
+                        TIMING_END_STREAM(tRobotJointPositionMap, ARMARX_VERBOSE);
 
                         const auto frames = localizationSegment.getRobotFramePoses(timestamp);