From 89f81e1f49f210ab70be479808c2f4b00dd1e885 Mon Sep 17 00:00:00 2001
From: Andre Meixner <andre.meixner@kit.edu>
Date: Tue, 13 Feb 2024 15:41:09 +0100
Subject: [PATCH] Added optional visualization of robot collision models in
visualization of the robot state memory
---
.../armem_robot_state/server/common/Visu.cpp | 22 ++++++++++++++++---
.../armem_robot_state/server/common/Visu.h | 3 ++-
2 files changed, 21 insertions(+), 4 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 c5f3fe36a..526d92d04 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -53,6 +53,9 @@ namespace armarx::armem::server::robot_state
defs->optional(p.forceTorque.forceScale,
prefix + "forceTorque.forceScale",
"Scaling of force arrows.");
+ defs->optional(p.collisionModelEnabled,
+ prefix + "collisionModelEnabled",
+ "Enable visualization of collision model.");
}
void
@@ -81,7 +84,7 @@ namespace armarx::armem::server::robot_state
}
void
- Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots)
+ Visu::visualizeRobots(viz::Layer& layer, const robot::Robots& robots, bool useColModel)
{
for (const robot::Robot& robot : robots)
{
@@ -93,7 +96,14 @@ namespace armarx::armem::server::robot_state
.joints(robot.config.jointMap)
.pose(robot.config.globalPose);
- robotVisu.useFullModel();
+ if (not useColModel)
+ {
+ robotVisu.useFullModel();
+ }
+ else
+ {
+ robotVisu.useCollisionModel();
+ }
// clang-format on
layer.add(robotVisu);
@@ -233,7 +243,13 @@ namespace armarx::armem::server::robot_state
{
viz::Layer& layer = layers.emplace_back(arviz.layer("Robots"));
- visualizeRobots(layer, robots);
+ visualizeRobots(layer, robots, false);
+ }
+
+ if (p.collisionModelEnabled)
+ {
+ viz::Layer& layer = layers.emplace_back(arviz.layer("RobotsCol"));
+ visualizeRobots(layer, robots, true);
}
ARMARX_DEBUG << "Visualize frames ...";
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 2afa9a27f..ede55c7a3 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -58,7 +58,7 @@ namespace armarx::armem::server::robot_state
void visualizeOnce(const Time& timestamp);
- static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots);
+ static void visualizeRobots(viz::Layer& layer, const robot::Robots& robots, bool useColModel = false);
static void visualizeFrames(
viz::Layer& layerFrames,
@@ -97,6 +97,7 @@ namespace armarx::armem::server::robot_state
float frequencyHz = 25.f;
bool framesEnabled = false;
+ bool collisionModelEnabled = false;
struct ForceTorque
{
--
GitLab