Add geometry type CompoundMesh

tesseract_collision/bullet/src/bullet_utils.cpp

@@ -50,6 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <BulletCollision/Gimpact/btTriangleShapeEx.h>
 #include <boost/thread/mutex.hpp>
 #include <memory>
+#include <stdexcept>
 #include <octomap/octomap.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
@@ -399,6 +400,10 @@ std::shared_ptr<btCollisionShape> createShapePrimitive(const CollisionShapeConst
       shape->setMargin(BULLET_MARGIN);
       break;
     }
+    case tesseract_geometry::GeometryType::COMPOUND_MESH:
+    {
+      throw std::runtime_error("CompundMesh type should not be passed to this function!");
+    }
     // LCOV_EXCL_START
     default:
     {
@@ -445,7 +450,8 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
   m_collisionFilterGroup = btBroadphaseProxy::KinematicFilter;
   m_collisionFilterMask = btBroadphaseProxy::StaticFilter | btBroadphaseProxy::KinematicFilter;
 
-  if (m_shapes.size() == 1 && m_shape_poses[0].matrix().isIdentity())
+  if (m_shapes.size() == 1 && m_shape_poses[0].matrix().isIdentity() &&
+      m_shapes[0]->getType() != tesseract_geometry::GeometryType::COMPOUND_MESH)
   {
     std::shared_ptr<btCollisionShape> shape = createShapePrimitive(m_shapes[0], this, 0);
     manage(shape);
@@ -461,14 +467,32 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
                                          // effect when negative
     setCollisionShape(compound.get());
 
+    int shape_id{ 0 };
     for (std::size_t j = 0; j < m_shapes.size(); ++j)
     {
-      std::shared_ptr<btCollisionShape> subshape = createShapePrimitive(m_shapes[j], this, static_cast<int>(j));
-      if (subshape != nullptr)
+      if (m_shapes[j]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH)
       {
-        manage(subshape);
-        btTransform geomTrans = convertEigenToBt(m_shape_poses[j]);
-        compound->addChildShape(geomTrans, subshape.get());
+        const auto& meshes = std::static_pointer_cast<const tesseract_geometry::CompoundMesh>(m_shapes[j])->getMeshes();
+        for (const auto& mesh : meshes)
+        {
+          std::shared_ptr<btCollisionShape> subshape = createShapePrimitive(mesh, this, shape_id++);
+          if (subshape != nullptr)
+          {
+            manage(subshape);
+            btTransform geomTrans = convertEigenToBt(m_shape_poses[j]);
+            compound->addChildShape(geomTrans, subshape.get());
+          }
+        }
+      }
+      else
+      {
+        std::shared_ptr<btCollisionShape> subshape = createShapePrimitive(m_shapes[j], this, shape_id++);
+        if (subshape != nullptr)
+        {
+          manage(subshape);
+          btTransform geomTrans = convertEigenToBt(m_shape_poses[j]);
+          compound->addChildShape(geomTrans, subshape.get());
+        }
       }
     }
   }

tesseract_collision/fcl/src/fcl_utils.cpp

@@ -50,6 +50,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <fcl/geometry/shape/capsule-inl.h>
 #include <fcl/geometry/octree/octree-inl.h>
 #include <memory>
+#include <stdexcept>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_collision/fcl/fcl_utils.h>
@@ -190,6 +191,10 @@ CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom)
     {
       return createShapePrimitive(std::static_pointer_cast<const tesseract_geometry::Octree>(geom));
     }
+    case tesseract_geometry::GeometryType::COMPOUND_MESH:
+    {
+      throw std::runtime_error("CompundMesh type should not be passed to this function!");
+    }
     default:
     {
       CONSOLE_BRIDGE_logError("This geometric shape type (%d) is not supported using fcl yet",
@@ -349,16 +354,37 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
   collision_objects_raw_.reserve(shapes_.size());
   for (std::size_t i = 0; i < shapes_.size(); ++i)  // NOLINT
   {
-    CollisionGeometryPtr subshape = createShapePrimitive(shapes_[i]);
-    if (subshape != nullptr)
+    if (shapes_[i]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH)
+    {
+      const auto& meshes = std::static_pointer_cast<const tesseract_geometry::CompoundMesh>(shapes_[i])->getMeshes();
+      for (const auto& mesh : meshes)
+      {
+        CollisionGeometryPtr subshape = createShapePrimitive(mesh);
+        if (subshape != nullptr)
+        {
+          collision_geometries_.push_back(subshape);
+          auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
+          co->setUserData(this);
+          co->setTransform(shape_poses_[i]);
+          co->updateAABB();
+          collision_objects_.push_back(co);
+          collision_objects_raw_.push_back(co.get());
+        }
+      }
+    }
+    else
     {
-      collision_geometries_.push_back(subshape);
-      auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
-      co->setUserData(this);
-      co->setTransform(shape_poses_[i]);
-      co->updateAABB();
-      collision_objects_.push_back(co);
-      collision_objects_raw_.push_back(co.get());
+      CollisionGeometryPtr subshape = createShapePrimitive(shapes_[i]);
+      if (subshape != nullptr)
+      {
+        collision_geometries_.push_back(subshape);
+        auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
+        co->setUserData(this);
+        co->setTransform(shape_poses_[i]);
+        co->updateAABB();
+        collision_objects_.push_back(co);
+        collision_objects_raw_.push_back(co.get());
+      }
     }
   }
 }

tesseract_collision/package.xml

@@ -35,6 +35,7 @@
   <test_depend>benchmark</test_depend>
   <test_depend>gtest</test_depend>
   <test_depend>tesseract_scene_graph</test_depend>
+  <test_depend>tesseract_support</test_depend>
 
   <export>
     <build_type>cmake</build_type>

tesseract_collision/test/CMakeLists.txt

@@ -65,6 +65,7 @@ add_gtest(${PROJECT_NAME}_octomap_mesh_unit collision_octomap_mesh_unit.cpp)
 add_gtest(${PROJECT_NAME}_clone_unit collision_clone_unit.cpp)
 add_gtest(${PROJECT_NAME}_box_box_cast_unit collision_box_box_cast_unit.cpp)
 add_gtest(${PROJECT_NAME}_compound_compound_unit collision_compound_compound_unit.cpp)
+add_gtest(${PROJECT_NAME}_compound_mesh_sphere_unit collision_compound_mesh_sphere_unit.cpp)
 add_gtest(${PROJECT_NAME}_sphere_sphere_cast_unit collision_sphere_sphere_cast_unit.cpp)
 add_gtest(${PROJECT_NAME}_octomap_octomap_unit collision_octomap_octomap_unit.cpp)
 add_gtest(${PROJECT_NAME}_collision_margin_data_unit collision_margin_data_unit.cpp)

tesseract_collision/test/collision_compound_mesh_sphere_unit.cpp

@@ -0,0 +1,36 @@
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <gtest/gtest.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp>
+#include <tesseract_collision/bullet/bullet_discrete_simple_manager.h>
+#include <tesseract_collision/bullet/bullet_discrete_bvh_manager.h>
+#include <tesseract_collision/fcl/fcl_discrete_managers.h>
+
+using namespace tesseract_collision;
+
+TEST(TesseractCollisionUnit, BulletDiscreteSimpleCollisionCompoundMeshSphereUnit)  // NOLINT
+{
+  tesseract_collision_bullet::BulletDiscreteSimpleManager checker;
+  test_suite::runTest(checker);
+}
+
+TEST(TesseractCollisionUnit, BulletDiscreteBVHCollisionCompoundMeshSphereUnit)  // NOLINT
+{
+  tesseract_collision_bullet::BulletDiscreteBVHManager checker;
+  test_suite::runTest(checker);
+}
+
+TEST(TesseractCollisionUnit, FCLDiscreteBVHCollisionCompoundMeshSphereUnit)  // NOLINT
+{
+  tesseract_collision_fcl::FCLDiscreteBVHManager checker;
+  test_suite::runTest(checker);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+
+  return RUN_ALL_TESTS();
+}