From babdb2847fc64df6b82db2d14d98bdbbefa5ac86 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 25 Jan 2025 14:25:07 -0600 Subject: [PATCH] Add collision shape caching based on tesseract_geometry::Geometry object --- tesseract_collision/bullet/CMakeLists.txt | 1 + .../bullet/bullet_collision_shape_cache.h | 69 ++++++++++++ .../tesseract_collision/bullet/bullet_utils.h | 6 +- .../src/bullet_collision_shape_cache.cpp | 54 +++++++++ .../bullet/src/bullet_utils.cpp | 103 ++++++++++-------- tesseract_collision/fcl/CMakeLists.txt | 7 +- .../fcl/fcl_collision_geometry_cache.h | 69 ++++++++++++ .../fcl/src/fcl_collision_geometry_cache.cpp | 54 +++++++++ tesseract_collision/fcl/src/fcl_utils.cpp | 14 ++- .../test_suite/collision_box_box_unit.hpp | 3 +- 10 files changed, 324 insertions(+), 56 deletions(-) create mode 100644 tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_collision_shape_cache.h create mode 100644 tesseract_collision/bullet/src/bullet_collision_shape_cache.cpp create mode 100644 tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_geometry_cache.h create mode 100644 tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp diff --git a/tesseract_collision/bullet/CMakeLists.txt b/tesseract_collision/bullet/CMakeLists.txt index e75232cde3f..8b0964b1a9f 100644 --- a/tesseract_collision/bullet/CMakeLists.txt +++ b/tesseract_collision/bullet/CMakeLists.txt @@ -5,6 +5,7 @@ add_library( ${PROJECT_NAME}_bullet src/bullet_cast_bvh_manager.cpp src/bullet_cast_simple_manager.cpp + src/bullet_collision_shape_cache.cpp src/bullet_discrete_bvh_manager.cpp src/bullet_discrete_simple_manager.cpp src/bullet_utils.cpp 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 new file mode 100644 index 00000000000..475bc2e8a15 --- /dev/null +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_collision_shape_cache.h @@ -0,0 +1,69 @@ +/** + * @file bullet_collision_shape_cache.h + * @brief This is a static cache mapping tesseract geometry shared pointers to bullet collision shapes to avoid + * recreating the same collision object + * + * @author Levi Armstrong + * @date January 25, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COLLISION_BULLET_COLLISION_SHAPE_CACHE_H +#define TESSERACT_COLLISION_BULLET_COLLISION_SHAPE_CACHE_H + +#include +#include +#include + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_collision::tesseract_collision_bullet +{ +class BulletCollisionShapeCache +{ +public: + /** + * @brief Insert a new entry into the cache + * @param key The cache key + * @param value The value to store + */ + static void insert(const std::shared_ptr& key, + 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); + +private: + /** @brief The static cache */ + static std::map, std::shared_ptr> cache_; + /** @brief The shared mutex for thread safety */ + static std::shared_mutex mutex_; +}; +} // namespace tesseract_collision::tesseract_collision_bullet + +#endif // TESSERACT_COLLISION_BULLET_COLLISION_SHAPE_CACHE_H 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 4b5615581f0..53b6c0e61c4 100644 --- a/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h +++ b/tesseract_collision/bullet/include/tesseract_collision/bullet/bullet_utils.h @@ -363,13 +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 - * @param shape_index The collision shapes index within the collision shape wrapper. This can be accessed from the - * bullet collision shape by calling getUserIndex function. * @return Bullet collision shape. */ -std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, - CollisionObjectWrapper* cow, - int shape_index); +std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, CollisionObjectWrapper* cow); /** * @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 new file mode 100644 index 00000000000..93e6d117ac9 --- /dev/null +++ b/tesseract_collision/bullet/src/bullet_collision_shape_cache.cpp @@ -0,0 +1,54 @@ +/** + * @file bullet_collision_shape_cache.cpp + * @brief This is a static cache mapping tesseract geometry shared pointers to bullet collision shapes to avoid + * recreating the same collision object + * + * @author Levi Armstrong + * @date January 25, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +namespace tesseract_collision::tesseract_collision_bullet +{ +// Static member definitions +std::map, std::shared_ptr> + BulletCollisionShapeCache::cache_; +std::shared_mutex BulletCollisionShapeCache::mutex_; + +void BulletCollisionShapeCache::insert(const std::shared_ptr& key, + const std::shared_ptr& value) +{ + std::unique_lock lock(mutex_); + cache_[key] = value; +} + +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); +} + +} // 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 4ca68f3da88..0f788d42620 100644 --- a/tesseract_collision/bullet/src/bullet_utils.cpp +++ b/tesseract_collision/bullet/src/bullet_utils.cpp @@ -41,7 +41,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include "tesseract_collision/bullet/bullet_utils.h" +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include @@ -145,8 +146,7 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: } std::shared_ptr createShapePrimitive(const tesseract_geometry::Mesh::ConstPtr& geom, - CollisionObjectWrapper* cow, - int shape_index) + CollisionObjectWrapper* cow) { int vertice_count = geom->getVertexCount(); int triangle_count = geom->getFaceCount(); @@ -176,7 +176,6 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: std::shared_ptr subshape = std::make_shared(v[0], v[1], v[2]); if (subshape != nullptr) { - subshape->setUserIndex(shape_index); cow->manage(subshape); subshape->setMargin(BULLET_MARGIN); btTransform geomTrans; @@ -211,8 +210,7 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: } std::shared_ptr createShapePrimitive(const tesseract_geometry::Octree::ConstPtr& geom, - CollisionObjectWrapper* cow, - int shape_index) + CollisionObjectWrapper* cow) { const octomap::OcTree& octree = *(geom->getOctree()); auto subshape = std::make_shared(BULLET_COMPOUND_USE_DYNAMIC_AABB, static_cast(octree.size())); @@ -240,7 +238,6 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: if (childshape == nullptr) { childshape = std::make_shared(btVector3(l, l, l)); - childshape->setUserIndex(shape_index); childshape->setMargin(BULLET_MARGIN); managed_shapes.at(it.getDepth()) = childshape; } @@ -275,7 +272,6 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: if (childshape == nullptr) { childshape = std::make_shared(static_cast((size / 2))); - childshape->setUserIndex(shape_index); // Sphere is a special case where you do not modify the margin which is internally set to the radius managed_shapes.at(it.getDepth()) = childshape; } @@ -311,7 +307,6 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: { childshape = std::make_shared(static_cast(std::sqrt(2 * ((size / 2) * (size / 2))))); - childshape->setUserIndex(shape_index); // Sphere is a special case where you do not modify the margin which is internally set to the radius managed_shapes.at(it.getDepth()) = childshape; } @@ -336,67 +331,59 @@ std::shared_ptr createShapePrimitive(const tesseract_geometry: return nullptr; } -std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, - CollisionObjectWrapper* cow, - int shape_index) +std::shared_ptr createShapePrimitive(const CollisionShapeConstPtr& geom, CollisionObjectWrapper* cow) { - std::shared_ptr shape = nullptr; + std::shared_ptr shape = BulletCollisionShapeCache::get(geom); + if (shape != nullptr) + return shape; switch (geom->getType()) { case tesseract_geometry::GeometryType::BOX: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setUserIndex(shape_index); shape->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::SPHERE: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setUserIndex(shape_index); // Sphere is a special case where you do not modify the margin which is internally set to the radius break; } case tesseract_geometry::GeometryType::CYLINDER: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setUserIndex(shape_index); shape->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::CONE: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setUserIndex(shape_index); shape->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::CAPSULE: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setUserIndex(shape_index); shape->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::MESH: { - shape = createShapePrimitive(std::static_pointer_cast(geom), cow, shape_index); - shape->setUserIndex(shape_index); + shape = createShapePrimitive(std::static_pointer_cast(geom), cow); shape->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::CONVEX_MESH: { shape = createShapePrimitive(std::static_pointer_cast(geom)); - shape->setUserIndex(shape_index); shape->setMargin(BULLET_MARGIN); break; } case tesseract_geometry::GeometryType::OCTREE: { - shape = createShapePrimitive(std::static_pointer_cast(geom), cow, shape_index); - shape->setUserIndex(shape_index); + shape = createShapePrimitive(std::static_pointer_cast(geom), cow); shape->setMargin(BULLET_MARGIN); break; } @@ -414,6 +401,7 @@ std::shared_ptr createShapePrimitive(const CollisionShapeConst // LCOV_EXCL_STOP } + BulletCollisionShapeCache::insert(geom, shape); return shape; } @@ -453,7 +441,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, if (m_shapes.size() == 1 && m_shape_poses[0].matrix().isIdentity() && m_shapes[0]->getType() != tesseract_geometry::GeometryType::COMPOUND_MESH) { - std::shared_ptr shape = createShapePrimitive(m_shapes[0], this, 0); + std::shared_ptr shape = createShapePrimitive(m_shapes[0], this); manage(shape); setCollisionShape(shape.get()); } @@ -474,7 +462,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, const auto& meshes = std::static_pointer_cast(m_shapes[j])->getMeshes(); for (const auto& mesh : meshes) { - std::shared_ptr subshape = createShapePrimitive(mesh, this, static_cast(j)); + std::shared_ptr subshape = createShapePrimitive(mesh, this); if (subshape != nullptr) { manage(subshape); @@ -485,7 +473,7 @@ CollisionObjectWrapper::CollisionObjectWrapper(std::string name, } else { - std::shared_ptr subshape = createShapePrimitive(m_shapes[j], this, static_cast(j)); + std::shared_ptr subshape = createShapePrimitive(m_shapes[j], this); if (subshape != nullptr) { manage(subshape); @@ -556,7 +544,6 @@ void CollisionObjectWrapper::manageReserve(std::size_t s) { m_data.reserve(s); } CastHullShape::CastHullShape(btConvexShape* shape, const btTransform& t01) : m_shape(shape), m_t01(t01) { m_shapeType = CUSTOM_CONVEX_SHAPE_TYPE; - setUserIndex(m_shape->getUserIndex()); } void CastHullShape::updateCastTransform(const btTransform& t01) { m_t01 = t01; } @@ -706,13 +693,15 @@ bool needsCollisionCheck(const COW& cow1, btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, + int index0, const btCollisionObjectWrapper* colObj1Wrap, + int index1, ContactTestData& collisions) { - assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); - assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); - const auto* cd0 = static_cast(colObj0Wrap->getCollisionObject()); // NOLINT - const auto* cd1 = static_cast(colObj1Wrap->getCollisionObject()); // NOLINT + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); // NOLINT + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); // NOLINT + const auto* cd0 = static_cast(colObj0Wrap->getCollisionObject()); // NOLINT + const auto* cd1 = static_cast(colObj1Wrap->getCollisionObject()); // NOLINT ObjectPairKey pc = tesseract_common::makeOrderedLinkPair(cd0->getName(), cd1->getName()); @@ -736,8 +725,17 @@ btScalar addDiscreteSingleResult(btManifoldPoint& cp, ContactResult contact; contact.link_names[0] = cd0->getName(); contact.link_names[1] = cd1->getName(); - contact.shape_id[0] = colObj0Wrap->getCollisionShape()->getUserIndex(); - contact.shape_id[1] = colObj1Wrap->getCollisionShape()->getUserIndex(); + + if (cd0->getCollisionGeometries().size() == 1) + contact.shape_id[0] = 0; + else + contact.shape_id[0] = (index0 < 0) ? 0 : index0; + + if (cd1->getCollisionGeometries().size() == 1) + contact.shape_id[1] = 0; + else + contact.shape_id[1] = (index1 < 0) ? 0 : index1; + contact.subshape_id[0] = colObj0Wrap->m_index; contact.subshape_id[1] = colObj1Wrap->m_index; contact.nearest_points[0] = convertBtToEigen(cp.m_positionWorldOnA); @@ -835,15 +833,15 @@ void calculateContinuousData(ContactResult* col, btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, - int /*index0*/, + int index0, const btCollisionObjectWrapper* colObj1Wrap, - int /*index1*/, + int index1, ContactTestData& collisions) { - assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); - assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); - const auto* cd0 = static_cast(colObj0Wrap->getCollisionObject()); // NOLINT - const auto* cd1 = static_cast(colObj1Wrap->getCollisionObject()); // NOLINT + assert(dynamic_cast(colObj0Wrap->getCollisionObject()) != nullptr); // NOLINT + assert(dynamic_cast(colObj1Wrap->getCollisionObject()) != nullptr); // NOLINT + const auto* cd0 = static_cast(colObj0Wrap->getCollisionObject()); // NOLINT + const auto* cd1 = static_cast(colObj1Wrap->getCollisionObject()); // NOLINT const std::pair& pc = cd0->getName() < cd1->getName() ? std::make_pair(cd0->getName(), cd1->getName()) : @@ -868,8 +866,17 @@ btScalar addCastSingleResult(btManifoldPoint& cp, ContactResult contact; contact.link_names[0] = cd0->getName(); contact.link_names[1] = cd1->getName(); - contact.shape_id[0] = colObj0Wrap->getCollisionShape()->getUserIndex(); - contact.shape_id[1] = colObj1Wrap->getCollisionShape()->getUserIndex(); + + if (cd0->getCollisionGeometries().size() == 1) + contact.shape_id[0] = 0; + else + contact.shape_id[0] = (index0 < 0) ? 0 : index0; + + if (cd1->getCollisionGeometries().size() == 1) + contact.shape_id[1] = 0; + else + contact.shape_id[1] = (index1 < 0) ? 0 : index1; + contact.subshape_id[0] = colObj0Wrap->m_index; contact.subshape_id[1] = colObj1Wrap->m_index; contact.nearest_points[0] = convertBtToEigen(cp.m_positionWorldOnA); @@ -995,15 +1002,15 @@ DiscreteBroadphaseContactResultCallback::DiscreteBroadphaseContactResultCallback btScalar DiscreteBroadphaseContactResultCallback::addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, - int /*index0*/, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, - int /*index1*/) + int index1) { if (cp.m_distance1 > static_cast(contact_distance_)) return 0; - return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + return addDiscreteSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); } CastBroadphaseContactResultCallback::CastBroadphaseContactResultCallback(ContactTestData& collisions, @@ -1021,6 +1028,7 @@ btScalar CastBroadphaseContactResultCallback::addSingleResult(btManifoldPoint& c int /*partId1*/, int index1) { + // NOLINTNEXTLINE(clang-analyzer-core.UndefinedBinaryOperatorResult) if (cp.m_distance1 > static_cast(contact_distance_)) return 0; @@ -1170,15 +1178,16 @@ DiscreteCollisionCollector::DiscreteCollisionCollector(ContactTestData& collisio btScalar DiscreteCollisionCollector::addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int /*partId0*/, - int /*index0*/, + int index0, const btCollisionObjectWrapper* colObj1Wrap, int /*partId1*/, - int /*index1*/) + int index1) { + // NOLINTNEXTLINE(clang-analyzer-core.UndefinedBinaryOperatorResult) if (cp.m_distance1 > static_cast(contact_distance_)) return 0; - return addDiscreteSingleResult(cp, colObj0Wrap, colObj1Wrap, collisions_); + return addDiscreteSingleResult(cp, colObj0Wrap, index0, colObj1Wrap, index1, collisions_); } bool DiscreteCollisionCollector::needsCollision(btBroadphaseProxy* proxy0) const diff --git a/tesseract_collision/fcl/CMakeLists.txt b/tesseract_collision/fcl/CMakeLists.txt index c52640147e3..e758b69f8a1 100644 --- a/tesseract_collision/fcl/CMakeLists.txt +++ b/tesseract_collision/fcl/CMakeLists.txt @@ -1,7 +1,12 @@ find_package(fcl 0.6 REQUIRED) # Create target for FCL implementation -add_library(${PROJECT_NAME}_fcl src/fcl_discrete_managers.cpp src/fcl_utils.cpp src/fcl_collision_object_wrapper.cpp) +add_library( + ${PROJECT_NAME}_fcl + src/fcl_collision_geometry_cache.cpp + src/fcl_discrete_managers.cpp + src/fcl_utils.cpp + src/fcl_collision_object_wrapper.cpp) target_link_libraries( ${PROJECT_NAME}_fcl PUBLIC ${PROJECT_NAME}_core 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 new file mode 100644 index 00000000000..b3e6a3c6a4d --- /dev/null +++ b/tesseract_collision/fcl/include/tesseract_collision/fcl/fcl_collision_geometry_cache.h @@ -0,0 +1,69 @@ +/** + * @file fcl_collision_geometry_cache.h + * @brief This is a static cache mapping tesseract geometry shared pointers to fcl collision geometry to avoid + * recreating the same collision object + * + * @author Levi Armstrong + * @date January 25, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TESSERACT_COLLISION_FCL_COLLISION_GEOMETRY_CACHE_H +#define TESSERACT_COLLISION_FCL_COLLISION_GEOMETRY_CACHE_H + +#include +#include +#include + +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_collision::tesseract_collision_fcl +{ +class FCLCollisionGeometryCache +{ +public: + /** + * @brief Insert a new entry into the cache + * @param key The cache key + * @param value The value to store + */ + static void insert(const std::shared_ptr& key, + 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); + +private: + /** @brief The static cache */ + static std::map, std::shared_ptr> cache_; + /** @brief The shared mutex for thread safety */ + static std::shared_mutex mutex_; +}; +} // namespace tesseract_collision::tesseract_collision_fcl + +#endif // TESSERACT_COLLISION_FCL_COLLISION_GEOMETRY_CACHE_H diff --git a/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp b/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp new file mode 100644 index 00000000000..3a029d9d574 --- /dev/null +++ b/tesseract_collision/fcl/src/fcl_collision_geometry_cache.cpp @@ -0,0 +1,54 @@ +/** + * @file fcl_collision_geometry_cache.cpp + * @brief This is a static cache mapping tesseract geometry shared pointers to fcl collision geometry to avoid + * recreating the same collision object + * + * @author Levi Armstrong + * @date January 25, 2025 + * @version TODO + * @bug No known bugs + * + * @copyright Copyright (c) 2025, Levi Armstrong + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include + +namespace tesseract_collision::tesseract_collision_fcl +{ +// Static member definitions +std::map, std::shared_ptr> + FCLCollisionGeometryCache::cache_; +std::shared_mutex FCLCollisionGeometryCache::mutex_; + +void FCLCollisionGeometryCache::insert(const std::shared_ptr& key, + const std::shared_ptr& value) +{ + std::unique_lock lock(mutex_); + cache_[key] = 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); +} + +} // namespace tesseract_collision::tesseract_collision_fcl diff --git a/tesseract_collision/fcl/src/fcl_utils.cpp b/tesseract_collision/fcl/src/fcl_utils.cpp index cda021c0662..7211187bd94 100644 --- a/tesseract_collision/fcl/src/fcl_utils.cpp +++ b/tesseract_collision/fcl/src/fcl_utils.cpp @@ -54,6 +54,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH TESSERACT_COMMON_IGNORE_WARNINGS_POP #include +#include #include namespace tesseract_collision::tesseract_collision_fcl @@ -151,7 +152,7 @@ CollisionGeometryPtr createShapePrimitive(const tesseract_geometry::Octree::Cons } } -CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom) +CollisionGeometryPtr createShapePrimitiveHelper(const CollisionShapeConstPtr& geom) { switch (geom->getType()) { @@ -204,6 +205,17 @@ CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom) } } +CollisionGeometryPtr createShapePrimitive(const CollisionShapeConstPtr& geom) +{ + CollisionGeometryPtr shape = FCLCollisionGeometryCache::get(geom); + if (shape != nullptr) + return shape; + + shape = createShapePrimitiveHelper(geom); + FCLCollisionGeometryCache::insert(geom, shape); + return shape; +} + bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data) { auto* cdata = reinterpret_cast(data); // NOLINT diff --git a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp index 524cb3557a8..3f8feb31d98 100644 --- a/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp +++ b/tesseract_collision/test_suite/include/tesseract_collision/test_suite/collision_box_box_unit.hpp @@ -83,13 +83,12 @@ inline void addCollisionObjects(DiscreteContactManager& checker, bool use_convex ///////////////////////////////////////////// // Add box and remove ///////////////////////////////////////////// - CollisionShapePtr remove_box = std::make_shared(0.1, 1, 1); Eigen::Isometry3d remove_box_pose; remove_box_pose.setIdentity(); CollisionShapesConst obj4_shapes; tesseract_common::VectorIsometry3d obj4_poses; - obj4_shapes.push_back(remove_box); + obj4_shapes.push_back(thin_box); obj4_poses.push_back(remove_box_pose); checker.addCollisionObject("remove_box_link", 0, obj4_shapes, obj4_poses);