Updates/a6u1/2023 07 26

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

@@ -117,6 +117,24 @@ namespace armarx::armem::server::robot_state
         }
     }
 
+    void Visu::visualizeFramesIndividual(
+        viz::Layer& layerFrames,
+        const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+    {
+
+        std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
+
+        for (const auto& [robotName, robotFrames] : frames)
+        {
+            int i = 0;
+            for (const auto& frame : robotFrames)
+            {
+                layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
+            }
+        }
+    }
+
+
 
     void Visu::visualizeRun()
     {
@@ -213,6 +231,12 @@ namespace armarx::armem::server::robot_state
 
         TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
 
+        {
+            viz::Layer layerFrames = arviz.layer("FramesIndividual");
+            visualizeFramesIndividual(layerFrames, frames);
+            layers.push_back(layerFrames);
+        }
+
 
         // Commit layers.