From 2f1d4f7cb80e6d02e414bc82d7515b60ca002fe0 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Fri, 17 Feb 2023 16:58:59 +0100
Subject: [PATCH] Add visualization of collision meshes to CollisionModelHelper
---
.../CollisionModelHelper.cpp | 25 +++++++++++++++++++
.../obstacle_avoidance/CollisionModelHelper.h | 5 ++++
2 files changed, 30 insertions(+)
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
index f8be378fc..d53a8dd6a 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
@@ -25,9 +25,12 @@
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
+
namespace armarx::obstacle_avoidance
{
@@ -151,4 +154,26 @@ namespace armarx::obstacle_avoidance
return std::make_shared<ManipulationObjectSet>(set);
}
+ void
+ CollisionModelHelper::visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz)
+ {
+ armarx::viz::Mesh mesh(model->getName());
+ auto faces = model->getTriMeshModel()->faces;
+ std::vector<armarx::viz::data::Face> viz_faces;
+ std::transform(
+ faces.begin(),
+ faces.end(),
+ std::back_inserter(viz_faces),
+ [](const auto& face)
+ {
+ return armarx::viz::data::Face(
+ face.id1, face.id2, face.id3, face.idColor1, face.idColor2, face.idColor3);
+ });
+ mesh.vertices(model->getTriMeshModel()->vertices)
+ .faces(viz_faces)
+ .pose(model->getGlobalPose());
+ arviz.commitLayerContaining("CollisionModel", mesh);
+ }
+
} // namespace armarx::obstacle_avoidance
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
index ca677b511..d56a2d5bd 100644
--- a/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
+++ b/source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
@@ -24,6 +24,7 @@
#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
@@ -47,10 +48,14 @@ namespace armarx::obstacle_avoidance
static VirtualRobot::SceneObjectSetPtr
asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
const OccupancyGridHelper::Params& params);
+ static void visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
+ viz::Client& arviz);
+
CollisionModelHelper(const objpose::ObjectPoseClient& client);
VirtualRobot::SceneObjectSetPtr fetchSceneObjects();
ManipulationObjectSetPtr fetchManipulationObjects();
+
private:
objpose::ObjectPoseClient objectPoseClient_;
};
--
GitLab