Add collision shape caching based on tesseract_geometry::Geometry object

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

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 <map>
+#include <memory>
+#include <shared_mutex>
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <BulletCollision/CollisionShapes/btCollisionShape.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_geometry/fwd.h>
+
+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<const tesseract_geometry::Geometry>& key,
+                     const std::shared_ptr<btCollisionShape>& 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<btCollisionShape> get(const std::shared_ptr<const tesseract_geometry::Geometry>& key);
+
+private:
+  /** @brief The static cache */
+  static std::map<std::shared_ptr<const tesseract_geometry::Geometry>, std::shared_ptr<btCollisionShape>> 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

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<btCollisionShape> createShapePrimitive(const CollisionShapeConstPtr& geom,
-                                                       CollisionObjectWrapper* cow,
-                                                       int shape_index);
+std::shared_ptr<btCollisionShape> createShapePrimitive(const CollisionShapeConstPtr& geom, CollisionObjectWrapper* cow);
 
 /**
  * @brief Update a collision objects filters

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 <tesseract_collision/bullet/bullet_collision_shape_cache.h>
+#include <tesseract_geometry/geometry.h>
+#include <mutex>
+
+namespace tesseract_collision::tesseract_collision_bullet
+{
+// Static member definitions
+std::map<std::shared_ptr<const tesseract_geometry::Geometry>, std::shared_ptr<btCollisionShape>>
+    BulletCollisionShapeCache::cache_;
+std::shared_mutex BulletCollisionShapeCache::mutex_;
+
+void BulletCollisionShapeCache::insert(const std::shared_ptr<const tesseract_geometry::Geometry>& key,
+                                       const std::shared_ptr<btCollisionShape>& value)
+{
+  std::unique_lock lock(mutex_);
+  cache_[key] = value;
+}
+
+std::shared_ptr<btCollisionShape>
+BulletCollisionShapeCache::get(const std::shared_ptr<const tesseract_geometry::Geometry>& key)
+{
+  std::shared_lock lock(mutex_);
+  auto it = cache_.find(key);
+  return ((it != cache_.end()) ? it->second : nullptr);
+}
+
+}  // namespace tesseract_collision::tesseract_collision_bullet