diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_collision_shape_cache.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_collision_shape_cache.h index 475bc2e8a15..4352462a716 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_collision_shape_cache.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_collision_shape_cache.h @@ -29,17 +29,28 @@ #include #include -#include +#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include namespace tesseract_collision::tesseract_collision_bullet { +struct BulletCollisionShape +{ + BulletCollisionShape() = default; + BulletCollisionShape(std::shared_ptr top_level_); + + std::shared_ptr top_level; + std::vector> children; +}; + class BulletCollisionShapeCache { public: @@ -49,20 +60,23 @@ class BulletCollisionShapeCache * @param value The value to store */ static void insert(const std::shared_ptr& key, - const std::shared_ptr& value); + const std::shared_ptr& value); /** * @brief Retrieve the cache entry by key * @param key The cache key * @return If key exists the entry is returned, otherwise a nullptr is returned */ - static std::shared_ptr get(const std::shared_ptr& key); + static std::shared_ptr get(const std::shared_ptr& key); + + /** @brief Remove any entries which are no longer valid */ + static void prune(); private: /** @brief The static cache */ - static std::map, std::shared_ptr> cache_; + static std::map> cache_; /** @brief The shared mutex for thread safety */ - static std::shared_mutex mutex_; + static std::mutex mutex_; }; } // namespace tesseract_collision::tesseract_collision_bullet diff --git a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h index 53b6c0e61c4..5f12998d6bd 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h @@ -53,6 +53,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include namespace tesseract_collision::tesseract_collision_bullet { @@ -129,7 +130,7 @@ class CollisionObjectWrapper : public btCollisionObject */ std::shared_ptr clone(); - void manage(const std::shared_ptr& t); + void manage(const std::shared_ptr& t); void manageReserve(std::size_t s); @@ -143,7 +144,7 @@ class CollisionObjectWrapper : public btCollisionObject /**< @brief The shapes poses information */ tesseract_common::VectorIsometry3d m_shape_poses{}; /** @brief This manages the collision shape pointer so they get destroyed */ - std::vector> m_data{}; + std::vector> m_data{}; }; using COW = CollisionObjectWrapper; @@ -362,10 +363,9 @@ class TesseractOverlapFilterCallback : public btOverlapFilterCallback /** * @brief Create a bullet collision shape from tesseract collision shape * @param geom Tesseract collision shape - * @param cow The collision object wrapper the collision shape is associated with * @return Bullet collision shape. */ -std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, CollisionObjectWrapper* cow); +std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom); /** * @brief Update a collision objects filters diff --git a/tesseract_collision/bullet/src/bullet_collision_shape_cache.cpp b/tesseract_collision/bullet/src/bullet_collision_shape_cache.cpp index 93e6d117ac9..7e950870db8 100644 --- a/tesseract_collision/bullet/src/bullet_collision_shape_cache.cpp +++ b/tesseract_collision/bullet/src/bullet_collision_shape_cache.cpp @@ -32,23 +32,49 @@ namespace tesseract_collision::tesseract_collision_bullet { // Static member definitions -std::map, std::shared_ptr> - BulletCollisionShapeCache::cache_; -std::shared_mutex BulletCollisionShapeCache::mutex_; +std::map> BulletCollisionShapeCache::cache_; +std::mutex BulletCollisionShapeCache::mutex_; + +BulletCollisionShape::BulletCollisionShape(std::shared_ptr top_level_) + : top_level(std::move(top_level_)) +{ +} void BulletCollisionShapeCache::insert(const std::shared_ptr& key, - const std::shared_ptr& value) + const std::shared_ptr& value) { - std::unique_lock lock(mutex_); - cache_[key] = value; + assert(!key->getUUID().is_nil()); + std::scoped_lock lock(mutex_); + cache_[key->getUUID()] = value; } -std::shared_ptr +std::shared_ptr BulletCollisionShapeCache::get(const std::shared_ptr& key) { - std::shared_lock lock(mutex_); - auto it = cache_.find(key); - return ((it != cache_.end()) ? it->second : nullptr); + assert(!key->getUUID().is_nil()); + std::scoped_lock lock(mutex_); + auto it = cache_.find(key->getUUID()); + if (it != cache_.end()) + { + std::shared_ptr collision_shape = it->second.lock(); + if (collision_shape != nullptr) + return collision_shape; + + cache_.erase(key->getUUID()); + } + return nullptr; +} + +void BulletCollisionShapeCache::prune() +{ + std::scoped_lock lock(mutex_); + for (auto it = cache_.begin(); it != cache_.end();) + { + if (it->second.expired()) + it = cache_.erase(it); + else + ++it; + } } } // namespace tesseract_collision::tesseract_collision_bullet diff --git a/tesseract_collision/bullet/src/bullet_utils.cpp b/tesseract_collision/bullet/src/bullet_utils.cpp index 61c97ed3b12..82e1d55875a 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -110,49 +110,50 @@ Eigen::Isometry3d convertBtToEigen(const btTransform& t) return i; } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Box::ConstPtr& geom) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Box::ConstPtr& geom) { auto a = static_cast(geom->getX() / 2); auto b = static_cast(geom->getY() / 2); auto c = static_cast(geom->getZ() / 2); - return std::make_shared(btVector3(a, b, c)); + return std::make_shared(std::make_shared(btVector3(a, b, c))); } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Sphere::ConstPtr& geom) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Sphere::ConstPtr& geom) { - return std::make_shared(static_cast(geom->getRadius())); + return std::make_shared( + std::make_shared(static_cast(geom->getRadius()))); } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Cylinder::ConstPtr& geom) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Cylinder::ConstPtr& geom) { auto r = static_cast(geom->getRadius()); auto l = static_cast(geom->getLength() / 2); - return std::make_shared(btVector3(r, r, l)); + return std::make_shared(std::make_shared(btVector3(r, r, l))); } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Cone::ConstPtr& geom) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Cone::ConstPtr& geom) { auto r = static_cast(geom->getRadius()); auto l = static_cast(geom->getLength()); - return std::make_shared(r, l); + return std::make_shared(std::make_shared(r, l)); } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Capsule::ConstPtr& geom) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Capsule::ConstPtr& geom) { auto r = static_cast(geom->getRadius()); auto l = static_cast(geom->getLength()); - return std::make_shared(r, l); + return std::make_shared(std::make_shared(r, l)); } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Mesh::ConstPtr& geom, - CollisionObjectWrapper* cow) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Mesh::ConstPtr& geom) { int vertice_count = geom->getVertexCount(); int triangle_count = geom->getFaceCount(); const tesseract_common::VectorVector3d& vertices = *(geom->getVertices()); const Eigen::VectorXi& triangles = *(geom->getFaces()); + auto collision_shape = std::make_shared(); if (vertice_count > 0 && triangle_count > 0) { auto compound = @@ -161,6 +162,7 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: // effect when positive but has an // effect when negative + collision_shape->children.reserve(static_cast(triangle_count)); for (int i = 0; i < triangle_count; ++i) { btVector3 v[3]; // NOLINT @@ -176,21 +178,21 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: std::shared_ptr subshape = std::make_shared(v[0], v[1], v[2]); if (subshape != nullptr) { - cow->manage(subshape); + collision_shape->children.push_back(subshape); subshape->setMargin(BULLET_MARGIN); btTransform geomTrans; geomTrans.setIdentity(); compound->addChildShape(geomTrans, subshape.get()); } } - - return compound; + collision_shape->top_level = compound; + return collision_shape; } CONSOLE_BRIDGE_logError("The mesh is empty!"); return nullptr; } -std::shared_ptr createShapePrimitive(const tesseract_geometry::ConvexMesh::ConstPtr& geom) +std::shared_ptr createShapePrimitive(const tesseract_geometry::ConvexMesh::ConstPtr& geom) { int vertice_count = geom->getVertexCount(); int triangle_count = geom->getFaceCount(); @@ -198,23 +200,22 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: if (vertice_count > 0 && triangle_count > 0) { - auto subshape = std::make_shared(); + auto convex_hull_shape = std::make_shared(); for (const auto& v : vertices) - subshape->addPoint( + convex_hull_shape->addPoint( btVector3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]))); - return subshape; + return std::make_shared(convex_hull_shape); } CONSOLE_BRIDGE_logError("The mesh is empty!"); return nullptr; } -std::shared_ptr createShapePrimitive(const tesseract_geometry::Octree::ConstPtr& geom, - CollisionObjectWrapper* cow) +std::shared_ptr createShapePrimitive(const tesseract_geometry::Octree::ConstPtr& geom) { const octomap::OcTree& octree = *(geom->getOctree()); - auto subshape = std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(octree.size())); double occupancy_threshold = octree.getOccupancyThres(); + auto subshape = std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(octree.size())); std::vector> managed_shapes; managed_shapes.resize(octree.getTreeDepth() + 1); @@ -246,14 +247,16 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: } } - cow->manageReserve(managed_shapes.size()); + auto collision_shape = std::make_shared(); + collision_shape->top_level = subshape; + collision_shape->children.reserve(managed_shapes.size()); for (const auto& managed_shape : managed_shapes) { if (managed_shape != nullptr) - cow->manage(managed_shape); + collision_shape->children.push_back(managed_shape); } - return subshape; + return collision_shape; } case tesseract_geometry::OctreeSubType::SPHERE_INSIDE: { @@ -280,14 +283,16 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: } } - cow->manageReserve(managed_shapes.size()); + auto collision_shape = std::make_shared(); + collision_shape->top_level = subshape; + collision_shape->children.reserve(managed_shapes.size()); for (const auto& managed_shape : managed_shapes) { if (managed_shape != nullptr) - cow->manage(managed_shape); + collision_shape->children.push_back(managed_shape); } - return subshape; + return collision_shape; } case tesseract_geometry::OctreeSubType::SPHERE_OUTSIDE: { @@ -315,14 +320,16 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: } } - cow->manageReserve(managed_shapes.size()); + auto collision_shape = std::make_shared(); + collision_shape->top_level = subshape; + collision_shape->children.reserve(managed_shapes.size()); for (const auto& managed_shape : managed_shapes) { if (managed_shape != nullptr) - cow->manage(managed_shape); + collision_shape->children.push_back(managed_shape); } - return subshape; + return collision_shape; } } @@ -331,22 +338,44 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: return nullptr; } -std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, CollisionObjectWrapper* cow) +std::shared_ptr createShapePrimitive(const tesseract_geometry::CompoundMesh::ConstPtr& geom) { - // std::shared_ptr shape = BulletCollisionShapeCache::get(geom); - // if (shape != nullptr) - // { - // // sleep(1000); - // return shape; - // } + const auto& meshes = geom->getMeshes(); + auto compound_mesh = + std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(meshes.size())); + compound_mesh->setMargin(BULLET_MARGIN); // margin: compound. seems to have no + // effect when positive but has an + // effect when negative + + auto shape = std::make_shared(compound_mesh); + btTransform trans; + trans.setIdentity(); + for (const auto& mesh : meshes) + { + std::shared_ptr subshape = createShapePrimitive(mesh); + if (subshape != nullptr) + { + shape->children.push_back(subshape->top_level); + shape->children.insert(shape->children.end(), subshape->children.begin(), subshape->children.end()); + compound_mesh->addChildShape(trans, subshape->top_level.get()); + } + } + + return shape; +} + +std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom) +{ + std::shared_ptr shape = BulletCollisionShapeCache::get(geom); + if (shape != nullptr) + return shape; - std::shared_ptr shape; switch (geom->getType()) { case tesseract_geometry::GeometryType::BOX: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setMargin(BULLET_MARGIN); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::SPHERE: @@ -358,42 +387,44 @@ std::shared_ptr createShapePrimitive(const CollisionShapeConst case tesseract_geometry::GeometryType::CYLINDER: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setMargin(BULLET_MARGIN); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::CONE: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setMargin(BULLET_MARGIN); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::CAPSULE: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setMargin(BULLET_MARGIN); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::MESH: { - shape = createShapePrimitive(std::static_pointer_cast(geom), cow); - shape->setMargin(BULLET_MARGIN); + shape = createShapePrimitive(std::static_pointer_cast(geom)); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::CONVEX_MESH: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setMargin(BULLET_MARGIN); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::OCTREE: { - shape = createShapePrimitive(std::static_pointer_cast(geom), cow); - shape->setMargin(BULLET_MARGIN); + shape = createShapePrimitive(std::static_pointer_cast(geom)); + shape->top_level->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::COMPOUND_MESH: { - throw std::runtime_error("CompundMesh type should not be passed to this function!"); + shape = createShapePrimitive(std::static_pointer_cast(geom)); + shape->top_level->setMargin(BULLET_MARGIN); + break; } // LCOV_EXCL_START default: @@ -442,18 +473,17 @@ 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() && - m_shapes[0]->getType() != tesseract_geometry::GeometryType::COMPOUND_MESH) + if (m_shapes.size() == 1 && m_shape_poses[0].matrix().isIdentity()) { - std::shared_ptr shape = createShapePrimitive(m_shapes[0], this); + std::shared_ptr shape = createShapePrimitive(m_shapes[0]); manage(shape); - setCollisionShape(shape.get()); + setCollisionShape(shape->top_level.get()); } else { auto compound = std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(m_shapes.size())); - manage(compound); + manage(std::make_shared(compound)); compound->setMargin(BULLET_MARGIN); // margin: compound. seems to have no // effect when positive but has an // effect when negative @@ -461,29 +491,12 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, for (std::size_t j = 0; j < m_shapes.size(); ++j) { - if (m_shapes[j]->getType() == tesseract_geometry::GeometryType::COMPOUND_MESH) - { - const auto& meshes = std::static_pointer_cast(m_shapes[j])->getMeshes(); - for (const auto& mesh : meshes) - { - std::shared_ptr subshape = createShapePrimitive(mesh, this); - if (subshape != nullptr) - { - manage(subshape); - btTransform geomTrans = convertEigenToBt(m_shape_poses[j]); - compound->addChildShape(geomTrans, subshape.get()); - } - } - } - else + std::shared_ptr subshape = createShapePrimitive(m_shapes[j]); + if (subshape != nullptr) { - std::shared_ptr subshape = createShapePrimitive(m_shapes[j], this); - if (subshape != nullptr) - { - manage(subshape); - btTransform geomTrans = convertEigenToBt(m_shape_poses[j]); - compound->addChildShape(geomTrans, subshape.get()); - } + manage(subshape); + btTransform geomTrans = convertEigenToBt(m_shape_poses[j]); + compound->addChildShape(geomTrans, subshape->top_level.get()); } } } @@ -541,7 +554,7 @@ std::shared_ptr CollisionObjectWrapper::clone() return clone_cow; } -void CollisionObjectWrapper::manage(const std::shared_ptr& t) { m_data.push_back(t); } +void CollisionObjectWrapper::manage(const std::shared_ptr& t) { m_data.push_back(t); } void CollisionObjectWrapper::manageReserve(std::size_t s) { m_data.reserve(s); } @@ -1251,7 +1264,7 @@ COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) auto shape = std::make_shared(convex, tf); assert(shape != nullptr); - new_cow->manage(shape); + new_cow->manage(std::make_shared(shape)); new_cow->setCollisionShape(shape.get()); } else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType())) @@ -1261,6 +1274,9 @@ COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) auto new_compound = std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes()); + auto collision_shape = std::make_shared(); + collision_shape->top_level = new_compound; + collision_shape->children.reserve(static_cast(compound->getNumChildShapes())); for (int i = 0; i < compound->getNumChildShapes(); ++i) { if (btBroadphaseProxy::isConvex(compound->getChildShape(i)->getShapeType())) @@ -1273,7 +1289,7 @@ COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) auto subshape = std::make_shared(convex, tf); assert(subshape != nullptr); - new_cow->manage(subshape); + collision_shape->children.push_back(subshape); subshape->setMargin(BULLET_MARGIN); new_compound->addChildShape(geomTrans, subshape.get()); } @@ -1282,6 +1298,7 @@ COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) auto* second_compound = static_cast(compound->getChildShape(i)); // NOLINT auto new_second_compound = std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, second_compound->getNumChildShapes()); + for (int j = 0; j < second_compound->getNumChildShapes(); ++j) { assert(!btBroadphaseProxy::isCompound(second_compound->getChildShape(j)->getShapeType())); @@ -1295,14 +1312,14 @@ COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) auto subshape = std::make_shared(convex, tf); assert(subshape != nullptr); - new_cow->manage(subshape); + collision_shape->children.push_back(subshape); subshape->setMargin(BULLET_MARGIN); new_second_compound->addChildShape(geomTrans, subshape.get()); } btTransform geomTrans = compound->getChildTransform(i); - new_cow->manage(new_second_compound); + collision_shape->children.push_back(new_second_compound); new_second_compound->setMargin(BULLET_MARGIN); // margin: compound. seems to // have no effect when positive // but has an effect when @@ -1322,8 +1339,8 @@ COW::Ptr makeCastCollisionObject(const COW::Ptr& cow) // have no effect when positive // but has an effect when // negative - new_cow->manage(new_compound); - new_cow->setCollisionShape(new_compound.get()); + new_cow->manage(collision_shape); + new_cow->setCollisionShape(collision_shape->top_level.get()); new_cow->setWorldTransform(cow->getWorldTransform()); } else diff --git a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_geometry_cache.h b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_geometry_cache.h index b3e6a3c6a4d..260e0dcff30 100644 --- a/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_geometry_cache.h +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_geometry_cache.h @@ -29,11 +29,12 @@ #include #include -#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -58,11 +59,14 @@ class FCLCollisionGeometryCache */ static std::shared_ptr get(const std::shared_ptr& key); + /** @brief Remove any entries which are no longer valid */ + static void prune(); + private: /** @brief The static cache */ - static std::map, std::shared_ptr> cache_; + static std::map> cache_; /** @brief The shared mutex for thread safety */ - static std::shared_mutex mutex_; + static std::mutex mutex_; }; } // namespace tesseract_collision::tesseract_collision_fcl diff --git a/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp b/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp index 3a029d9d574..34c0c6c2d1c 100644 --- a/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp +++ b/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp @@ -32,23 +32,44 @@ namespace tesseract_collision::tesseract_collision_fcl { // Static member definitions -std::map, std::shared_ptr> - FCLCollisionGeometryCache::cache_; -std::shared_mutex FCLCollisionGeometryCache::mutex_; +std::map> FCLCollisionGeometryCache::cache_; +std::mutex FCLCollisionGeometryCache::mutex_; void FCLCollisionGeometryCache::insert(const std::shared_ptr& key, const std::shared_ptr& value) { - std::unique_lock lock(mutex_); - cache_[key] = value; + assert(!key->getUUID().is_nil()); + std::scoped_lock lock(mutex_); + cache_[key->getUUID()] = value; } std::shared_ptr FCLCollisionGeometryCache::get(const std::shared_ptr& key) { - std::shared_lock lock(mutex_); - auto it = cache_.find(key); - return ((it != cache_.end()) ? it->second : nullptr); + assert(!key->getUUID().is_nil()); + std::scoped_lock lock(mutex_); + auto it = cache_.find(key->getUUID()); + if (it != cache_.end()) + { + std::shared_ptr collision_shape = it->second.lock(); + if (collision_shape != nullptr) + return collision_shape; + + cache_.erase(key->getUUID()); + } + return nullptr; +} + +void FCLCollisionGeometryCache::prune() +{ + std::scoped_lock lock(mutex_); + for (auto it = cache_.begin(); it != cache_.end();) + { + if (it->second.expired()) + it = cache_.erase(it); + else + ++it; + } } } // namespace tesseract_collision::tesseract_collision_fcl diff --git a/tesseract_environment/test/tesseract_environment_collision.cpp b/tesseract_environment/test/tesseract_environment_collision.cpp index e59a7e962f0..68fbef6959c 100644 --- a/tesseract_environment/test/tesseract_environment_collision.cpp +++ b/tesseract_environment/test/tesseract_environment_collision.cpp @@ -112,26 +112,51 @@ TEST(TesseractEnvironmentCollisionUnit, runEnvironmentDiscreteCollisionTest) // tesseract_collision::CollisionMarginData margin_data(0.0); mCollisionCheckConfig.contact_manager_config.margin_data = margin_data; - // Setup collision checker - DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); - { // Check for collisions + { // Setup collision checker + DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); + { // Check for collisions + tesseract_collision::ContactResultMap collision; + std::vector active_links = { "link_n1" }; + manager->setActiveCollisionObjects(active_links); + manager->contactTest(collision, mCollisionCheckConfig.contact_request); + EXPECT_FALSE(collision.empty()); + } + + auto pose = Eigen::Isometry3d::Identity(); + pose.translation() = Eigen::Vector3d(0, 0, 1.5); + manager->setCollisionObjectsTransform("link_n1", pose); + manager->setCollisionMarginData(mCollisionCheckConfig.contact_manager_config.margin_data); + + // Check for collisions tesseract_collision::ContactResultMap collision; - std::vector active_links = { "link_n1" }; - manager->setActiveCollisionObjects(active_links); manager->contactTest(collision, mCollisionCheckConfig.contact_request); + EXPECT_FALSE(collision.empty()); } - auto pose = Eigen::Isometry3d::Identity(); - pose.translation() = Eigen::Vector3d(0, 0, 1.5); - manager->setCollisionObjectsTransform("link_n1", pose); - manager->setCollisionMarginData(mCollisionCheckConfig.contact_manager_config.margin_data); + env->setActiveDiscreteContactManager("BulletDiscreteBVHManager"); - // Check for collisions - tesseract_collision::ContactResultMap collision; - manager->contactTest(collision, mCollisionCheckConfig.contact_request); + { // Setup collision checker + DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); + { // Check for collisions + tesseract_collision::ContactResultMap collision; + std::vector active_links = { "link_n1" }; + manager->setActiveCollisionObjects(active_links); + manager->contactTest(collision, mCollisionCheckConfig.contact_request); + EXPECT_FALSE(collision.empty()); + } - EXPECT_FALSE(collision.empty()); + auto pose = Eigen::Isometry3d::Identity(); + pose.translation() = Eigen::Vector3d(0, 0, 1.5); + manager->setCollisionObjectsTransform("link_n1", pose); + manager->setCollisionMarginData(mCollisionCheckConfig.contact_manager_config.margin_data); + + // Check for collisions + tesseract_collision::ContactResultMap collision; + manager->contactTest(collision, mCollisionCheckConfig.contact_request); + + EXPECT_FALSE(collision.empty()); + } } TEST(TesseractEnvironmentCollisionUnit, runEnvironmentContinuousCollisionTest) // NOLINT diff --git a/tesseract_geometry/include/tesseract_geometry/geometry.h b/tesseract_geometry/include/tesseract_geometry/geometry.h index 637144fe73e..cab889f7d5a 100644 --- a/tesseract_geometry/include/tesseract_geometry/geometry.h +++ b/tesseract_geometry/include/tesseract_geometry/geometry.h @@ -29,6 +29,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include +#include #include #include #include @@ -75,18 +76,25 @@ class Geometry Geometry(Geometry&&) = default; Geometry& operator=(Geometry&&) = default; - /** \brief Create a copy of this shape */ + /** @brief Create a copy of this shape */ virtual Geometry::Ptr clone() const = 0; - GeometryType getType() const { return type_; } + /** @brief Get the geometry type */ + GeometryType getType() const; + + /** @brief Get the geometry UUID */ + const boost::uuids::uuid& getUUID() const; bool operator==(const Geometry& rhs) const; bool operator!=(const Geometry& rhs) const; private: - /** \brief The type of the shape */ + /** @brief The type of the shape */ GeometryType type_; + /** @brief The uuid of the shape */ + boost::uuids::uuid uuid_{}; + friend class boost::serialization::access; template void serialize(Archive& ar, const unsigned int version); // NOLINT diff --git a/tesseract_geometry/src/geometry.cpp b/tesseract_geometry/src/geometry.cpp index 29ece7746c0..63cbf9b8663 100644 --- a/tesseract_geometry/src/geometry.cpp +++ b/tesseract_geometry/src/geometry.cpp @@ -27,6 +27,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -34,15 +37,20 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP namespace tesseract_geometry { -Geometry::Geometry(GeometryType type) : type_(type) {} +Geometry::Geometry(GeometryType type) : type_(type), uuid_(boost::uuids::random_generator()()) {} -bool Geometry::operator==(const Geometry& rhs) const { return (type_ == rhs.type_); } +GeometryType Geometry::getType() const { return type_; } + +const boost::uuids::uuid& Geometry::getUUID() const { return uuid_; } + +bool Geometry::operator==(const Geometry& rhs) const { return (type_ == rhs.type_ && uuid_ == rhs.uuid_); } bool Geometry::operator!=(const Geometry& rhs) const { return !operator==(rhs); } // LCOV_EXCL_LINE template void Geometry::serialize(Archive& ar, const unsigned int /*version*/) { ar& boost::serialization::make_nvp("type", type_); + ar& boost::serialization::make_nvp("uuid", uuid_); } } // namespace tesseract_geometry