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 b8b3190d1bae586226c60b490bb399abe5799272..41eaf6e2ec2f2bcead697ec6ca7e86868bca44c4 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp +++ b/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. diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h index 66222e0cf8dc6cf6043e6db35ebb7ba00e0cab00..99321b656be9977a23096e3b9bdd9ed4ba62a101 100644 --- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h +++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h @@ -71,6 +71,11 @@ namespace armarx::armem::server::robot_state viz::Layer& layerFrames, const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames); + static + void visualizeFramesIndividual( + viz::Layer& layerFrames, + const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames); + private: