Fix use of compound mesh in FCL

tesseract_collision/bullet/src/bullet_utils.cpp

@@ -472,10 +472,11 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
     {
       if (m_shapes[j]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH)
       {
+        int shape_index = shape_id++;
         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++);
+          std::shared_ptr<btCollisionShape> subshape = createShapePrimitive(mesh, this, shape_index);
           if (subshape != nullptr)
           {
             manage(subshape);

tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_object_wrapper.h

@@ -64,8 +64,23 @@ class FCLCollisionObjectWrapper : public fcl::CollisionObject<double>
    */
   void updateAABB();
 
+  /**
+   * @brief Set the shape index. This is the geometries index in the urdf.
+   * @param index The index
+   */
+  void setShapeIndex(int index);
+
+  /**
+   * @brief Get the shape index. This is the geometries index in the urdf.
+   * @return The shape index
+   */
+  int getShapeIndex() const;
+
 protected:
   double contact_distance_{ 0 }; /**< @brief The contact distance threshold. */
+
+  /** @brief The shape index, which is the geometries index in the urdf. */
+  int shape_index_{ -1 };
 };
 
 }  // namespace tesseract_collision::tesseract_collision_fcl

tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_utils.h

@@ -113,10 +113,9 @@ class CollisionObjectWrapper
   void setCollisionObjectsTransform(const Eigen::Isometry3d& pose)
   {
     world_pose_ = pose;
-    for (unsigned i = 0; i < collision_objects_.size(); ++i)
+    for (auto& co : collision_objects_)
     {
-      CollisionObjectPtr& co = collision_objects_[i];
-      co->setTransform(pose * shape_poses_[i]);
+      co->setTransform(pose * shape_poses_[static_cast<std::size_t>(co->getShapeIndex())]);
       co->updateAABB();  // This a tesseract function that updates abb to take into account contact distance
     }
   }
@@ -168,7 +167,7 @@ class CollisionObjectWrapper
    * @param co fcl collision shape
    * @return links collision shape index
    */
-  int getShapeIndex(const fcl::CollisionObjectd* co) const;
+  static int getShapeIndex(const fcl::CollisionObjectd* co);
 
 protected:
   std::string name_;                                              // name of the collision object

tesseract_collision/fcl/src/fcl_collision_object_wrapper.cpp

@@ -54,4 +54,12 @@ void FCLCollisionObjectWrapper::updateAABB()
   }
 }
 
+void FCLCollisionObjectWrapper::setShapeIndex(int index) { shape_index_ = index; }
+
+int FCLCollisionObjectWrapper::getShapeIndex() const
+{
+  assert(shape_index_ >= 0);
+  return shape_index_;
+}
+
 }  // namespace tesseract_collision::tesseract_collision_fcl

tesseract_collision/fcl/src/fcl_utils.cpp

@@ -246,8 +246,8 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
       ContactResult contact;
       contact.link_names[0] = cd1->getName();
       contact.link_names[1] = cd2->getName();
-      contact.shape_id[0] = static_cast<int>(cd1->getShapeIndex(o1));
-      contact.shape_id[1] = static_cast<int>(cd2->getShapeIndex(o2));
+      contact.shape_id[0] = CollisionObjectWrapper::getShapeIndex(o1);
+      contact.shape_id[1] = CollisionObjectWrapper::getShapeIndex(o2);
       contact.subshape_id[0] = static_cast<int>(fcl_contact.b1);
       contact.subshape_id[1] = static_cast<int>(fcl_contact.b2);
       contact.nearest_points[0] = fcl_contact.pos;
@@ -307,8 +307,8 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
     ContactResult contact;
     contact.link_names[0] = cd1->getName();
     contact.link_names[1] = cd2->getName();
-    contact.shape_id[0] = cd1->getShapeIndex(o1);
-    contact.shape_id[1] = cd2->getShapeIndex(o2);
+    contact.shape_id[0] = CollisionObjectWrapper::getShapeIndex(o1);
+    contact.shape_id[1] = CollisionObjectWrapper::getShapeIndex(o2);
     contact.subshape_id[0] = static_cast<int>(fcl_result.b1);
     contact.subshape_id[1] = static_cast<int>(fcl_result.b2);
     contact.nearest_points[0] = fcl_result.nearest_points[0];
@@ -365,6 +365,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
           collision_geometries_.push_back(subshape);
           auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
           co->setUserData(this);
+          co->setShapeIndex(static_cast<int>(i));
           co->setTransform(shape_poses_[i]);
           co->updateAABB();
           collision_objects_.push_back(co);
@@ -380,6 +381,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
         collision_geometries_.push_back(subshape);
         auto co = std::make_shared<FCLCollisionObjectWrapper>(subshape);
         co->setUserData(this);
+        co->setShapeIndex(static_cast<int>(i));
         co->setTransform(shape_poses_[i]);
         co->updateAABB();
         collision_objects_.push_back(co);
@@ -389,16 +391,9 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name,
   }
 }
 
-int CollisionObjectWrapper::getShapeIndex(const fcl::CollisionObjectd* co) const
+int CollisionObjectWrapper::getShapeIndex(const fcl::CollisionObjectd* co)
 {
-  auto it = std::find_if(collision_objects_.begin(), collision_objects_.end(), [&co](const CollisionObjectPtr& c) {
-    return c.get() == co;
-  });
-
-  if (it != collision_objects_.end())
-    return static_cast<int>(std::distance(collision_objects_.begin(), it));
-
-  return -1;
+  return static_cast<const FCLCollisionObjectWrapper*>(co)->getShapeIndex();
 }
 
 }  // namespace tesseract_collision::tesseract_collision_fcl

tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_compound_mesh_sphere_unit.hpp

@@ -177,7 +177,7 @@ inline void runTest(DiscreteContactManager& checker)
   EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -1.0, 0.001);
 
   // Compound mesh so check shape id
-  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 1);
+  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 0);
 
   ////////////////////////////////////////////////
   // Test object is out side the contact distance
@@ -235,7 +235,7 @@ inline void runTest(DiscreteContactManager& checker)
   EXPECT_NEAR(result_vector[0].normal[2], idx[2] * -1.0, 0.001);
 
   // Compound mesh so check shape id
-  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 1);
+  EXPECT_EQ(result_vector[0].shape_id[static_cast<size_t>(idx[0])], 0);
 
   /////////////////////////////////////////////
   // Test collision against second shape