Add visualization of collision meshes to CollisionModelHelper

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

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_;
     };