Limit the use of new operator in tesseract_collision

Author: Levi-Armstrong

tesseract/tesseract_collision/examples/box_box_example.cpp

@@ -19,7 +19,7 @@ int main(int /*argc*/, char** /*argv*/)
   tesseract_collision_bullet::BulletDiscreteBVHManager checker;
 
   // Add box to checker
-  CollisionShapePtr box(new Box(1, 1, 1));
+  CollisionShapePtr box = std::make_shared<Box>(1, 1, 1);
   Eigen::Isometry3d box_pose;
   box_pose.setIdentity();
 
@@ -31,7 +31,7 @@ int main(int /*argc*/, char** /*argv*/)
   checker.addCollisionObject("box_link", 0, obj1_shapes, obj1_poses);
 
   // Add thin box to checker which is disabled
-  CollisionShapePtr thin_box(new Box(0.1, 1, 1));
+  CollisionShapePtr thin_box = std::make_shared<Box>(0.1, 1, 1);
   Eigen::Isometry3d thin_box_pose;
   thin_box_pose.setIdentity();
 
@@ -50,10 +50,10 @@ int main(int /*argc*/, char** /*argv*/)
   loadSimplePlyFile(std::string(DATA_DIR) + "/box_2m.ply", mesh_vertices, mesh_faces);
 
   // This is required because convex hull cannot have multiple faces on the same plane.
-  std::shared_ptr<tesseract_common::VectorVector3d> ch_verticies(new tesseract_common::VectorVector3d());
-  std::shared_ptr<Eigen::VectorXi> ch_faces(new Eigen::VectorXi());
+  auto ch_verticies = std::make_shared<tesseract_common::VectorVector3d>();
+  auto ch_faces = std::make_shared<Eigen::VectorXi>();
   int ch_num_faces = createConvexHull(*ch_verticies, *ch_faces, mesh_vertices);
-  second_box.reset(new ConvexMesh(ch_verticies, ch_faces, ch_num_faces));
+  second_box = std::make_shared<ConvexMesh>(ch_verticies, ch_faces, ch_num_faces);
 
   Eigen::Isometry3d second_box_pose;
   second_box_pose.setIdentity();

tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_utils.h

@@ -132,18 +132,12 @@ class CollisionObjectWrapper : public btCollisionObject
   using Ptr = std::shared_ptr<CollisionObjectWrapper>;
   using ConstPtr = std::shared_ptr<const CollisionObjectWrapper>;
 
+  CollisionObjectWrapper() = default;
   CollisionObjectWrapper(std::string name,
                          const int& type_id,
                          CollisionShapesConst shapes,
                          tesseract_common::VectorIsometry3d shape_poses);
 
-  /** @brief This is a special constructor used by the clone method */
-  CollisionObjectWrapper(std::string name,
-                         const int& type_id,
-                         CollisionShapesConst shapes,
-                         tesseract_common::VectorIsometry3d shape_poses,
-                         std::vector<std::shared_ptr<void>> data);
-
   short int m_collisionFilterGroup;
   short int m_collisionFilterMask;
   bool m_enabled;
@@ -188,7 +182,12 @@ class CollisionObjectWrapper : public btCollisionObject
    */
   std::shared_ptr<CollisionObjectWrapper> clone()
   {
-    auto clone_cow = std::make_shared<CollisionObjectWrapper>(m_name, m_type_id, m_shapes, m_shape_poses, m_data);
+    auto clone_cow = std::make_shared<CollisionObjectWrapper>();
+    clone_cow->m_name = m_name;
+    clone_cow->m_type_id = m_type_id;
+    clone_cow->m_shapes = m_shapes;
+    clone_cow->m_shape_poses = m_shape_poses;
+    clone_cow->m_data = m_data;
     clone_cow->setCollisionShape(getCollisionShape());
     clone_cow->setWorldTransform(getWorldTransform());
     clone_cow->m_collisionFilterGroup = m_collisionFilterGroup;
@@ -198,25 +197,16 @@ class CollisionObjectWrapper : public btCollisionObject
     return clone_cow;
   }
 
-  template <class T>
-  void manage(T* t)
-  {  // manage memory of this object
-    m_data.push_back(std::shared_ptr<T>(t));
-  }
-  template <class T>
-  void manage(std::shared_ptr<T> t)
-  {
-    m_data.push_back(t);
-  }
+  void manage(const std::shared_ptr<btCollisionShape>& t) { m_data.push_back(t); }
 
 protected:
   std::string m_name;                               /**< @brief The name of the collision object */
   int m_type_id;                                    /**< @brief A user defined type id */
   CollisionShapesConst m_shapes;                    /**< @brief The shapes that define the collison object */
   tesseract_common::VectorIsometry3d m_shape_poses; /**< @brief The shpaes poses information */
 
-  std::vector<std::shared_ptr<void>> m_data; /**< @brief This manages the collision shape pointer so they get destroyed
-                                              */
+  /** @brief This manages the collision shape pointer so they get destroyed */
+  std::vector<std::shared_ptr<btCollisionShape>> m_data;
 };
 
 using COW = CollisionObjectWrapper;
@@ -898,9 +888,9 @@ class TesseractOverlapFilterCallback : public btOverlapFilterCallback
  * bullet collision shape by calling getUserIndex function.
  * @return Bullet collision shape.
  */
-btCollisionShape* createShapePrimitive(const CollisionShapeConstPtr& geom,
-                                       CollisionObjectWrapper* cow,
-                                       int shape_index);
+std::shared_ptr<btCollisionShape> createShapePrimitive(const CollisionShapeConstPtr& geom,
+                                                       CollisionObjectWrapper* cow,
+                                                       int shape_index);
 
 /**
  * @brief Update a collision objects filters
@@ -939,7 +929,7 @@ inline COW::Ptr createCollisionObject(const std::string& name,
     return nullptr;
   }
 
-  COW::Ptr new_cow(new COW(name, type_id, shapes, shape_poses));
+  auto new_cow = std::make_shared<COW>(name, type_id, shapes, shape_poses);
 
   new_cow->m_enabled = enabled;
   new_cow->setContactProcessingThreshold(BULLET_DEFAULT_CONTACT_DISTANCE);
@@ -1036,17 +1026,18 @@ inline COW::Ptr makeCastCollisionObject(const COW::Ptr& cow)
     assert(convex->getShapeType() != CUSTOM_CONVEX_SHAPE_TYPE);  // This checks if the collision object is already a
                                                                  // cast collision object
 
-    auto* shape = new CastHullShape(convex, tf);
+    auto shape = std::make_shared<CastHullShape>(convex, tf);
     assert(shape != nullptr);
 
     new_cow->manage(shape);
-    new_cow->setCollisionShape(shape);
+    new_cow->setCollisionShape(shape.get());
   }
   else if (btBroadphaseProxy::isCompound(new_cow->getCollisionShape()->getShapeType()))
   {
     assert(dynamic_cast<btCompoundShape*>(new_cow->getCollisionShape()) != nullptr);
     auto* compound = static_cast<btCompoundShape*>(new_cow->getCollisionShape());
-    auto* new_compound = new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes());
+    auto new_compound =
+        std::make_shared<btCompoundShape>(BULLET_COMPOUND_USE_DYNAMIC_AABB, compound->getNumChildShapes());
 
     for (int i = 0; i < compound->getNumChildShapes(); ++i)
     {
@@ -1057,18 +1048,18 @@ inline COW::Ptr makeCastCollisionObject(const COW::Ptr& cow)
 
         btTransform geomTrans = compound->getChildTransform(i);
 
-        auto* subshape = new CastHullShape(convex, tf);
+        auto subshape = std::make_shared<CastHullShape>(convex, tf);
         assert(subshape != nullptr);
 
         new_cow->manage(subshape);
         subshape->setMargin(BULLET_MARGIN);
-        new_compound->addChildShape(geomTrans, subshape);
+        new_compound->addChildShape(geomTrans, subshape.get());
       }
       else if (btBroadphaseProxy::isCompound(compound->getChildShape(i)->getShapeType()))
       {
         auto* second_compound = static_cast<btCompoundShape*>(compound->getChildShape(i));
-        auto* new_second_compound =
-            new btCompoundShape(BULLET_COMPOUND_USE_DYNAMIC_AABB, second_compound->getNumChildShapes());
+        auto new_second_compound =
+            std::make_shared<btCompoundShape>(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()));
@@ -1079,12 +1070,12 @@ inline COW::Ptr makeCastCollisionObject(const COW::Ptr& cow)
 
           btTransform geomTrans = second_compound->getChildTransform(j);
 
-          auto* subshape = new CastHullShape(convex, tf);
+          auto subshape = std::make_shared<CastHullShape>(convex, tf);
           assert(subshape != nullptr);
 
           new_cow->manage(subshape);
           subshape->setMargin(BULLET_MARGIN);
-          new_second_compound->addChildShape(geomTrans, subshape);
+          new_second_compound->addChildShape(geomTrans, subshape.get());
         }
 
         btTransform geomTrans = compound->getChildTransform(i);
@@ -1095,7 +1086,7 @@ inline COW::Ptr makeCastCollisionObject(const COW::Ptr& cow)
                                                         // but has an effect when
                                                         // negative
 
-        new_compound->addChildShape(geomTrans, new_second_compound);
+        new_compound->addChildShape(geomTrans, new_second_compound.get());
       }
       else
       {
@@ -1108,7 +1099,7 @@ inline COW::Ptr makeCastCollisionObject(const COW::Ptr& cow)
                                              // but has an effect when
                                              // negative
     new_cow->manage(new_compound);
-    new_cow->setCollisionShape(new_compound);
+    new_cow->setCollisionShape(new_compound.get());
     new_cow->setWorldTransform(cow->getWorldTransform());
   }
   else

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

@@ -84,6 +84,7 @@ class CollisionObjectWrapper
   using Ptr = std::shared_ptr<CollisionObjectWrapper>;
   using ConstPtr = std::shared_ptr<const CollisionObjectWrapper>;
 
+  CollisionObjectWrapper() = default;
   CollisionObjectWrapper(std::string name,
                          const int& type_id,
                          CollisionShapesConst shapes,
@@ -137,8 +138,25 @@ class CollisionObjectWrapper
   std::vector<CollisionObjectRawPtr>& getCollisionObjectsRaw() { return collision_objects_raw_; }
   std::shared_ptr<CollisionObjectWrapper> clone() const
   {
-    CollisionObjectWrapper::Ptr clone_cow(
-        new CollisionObjectWrapper(name_, type_id_, shapes_, shape_poses_, collision_geometries_, collision_objects_));
+    auto clone_cow = std::make_shared<CollisionObjectWrapper>();
+    clone_cow->name_ = name_;
+    clone_cow->type_id_ = type_id_;
+    clone_cow->shapes_ = shapes_;
+    clone_cow->shape_poses_ = shape_poses_;
+    clone_cow->collision_geometries_ = collision_geometries_;
+
+    clone_cow->collision_objects_.reserve(collision_objects_.size());
+    clone_cow->collision_objects_raw_.reserve(collision_objects_.size());
+    for (const auto& co : collision_objects_)
+    {
+      auto collObj = std::make_shared<FCLCollisionObjectWrapper>(*co);
+      collObj->setUserData(clone_cow.get());
+      collObj->setTransform(co->getTransform());
+      collObj->updateAABB();
+      clone_cow->collision_objects_.push_back(collObj);
+      clone_cow->collision_objects_raw_.push_back(collObj.get());
+    }
+
     clone_cow->m_collisionFilterGroup = m_collisionFilterGroup;
     clone_cow->m_collisionFilterMask = m_collisionFilterMask;
     clone_cow->m_enabled = m_enabled;
@@ -153,13 +171,6 @@ class CollisionObjectWrapper
   int getShapeIndex(const fcl::CollisionObjectd* co) const;
 
 protected:
-  CollisionObjectWrapper(std::string name,
-                         const int& type_id,
-                         CollisionShapesConst shapes,
-                         tesseract_common::VectorIsometry3d shape_poses,
-                         std::vector<CollisionGeometryPtr> collision_geometries,
-                         const std::vector<CollisionObjectPtr>& collision_objects);
-
   std::string name_;             // name of the collision object
   int type_id_;                  // user defined type id
   Eigen::Isometry3d world_pose_; /**< @brief Collision Object World Transformation */

tesseract/tesseract_collision/src/bullet/bullet_cast_bvh_manager.cpp

@@ -79,7 +79,7 @@ BulletCastBVHManager::~BulletCastBVHManager()
 
 ContinuousContactManager::Ptr BulletCastBVHManager::clone() const
 {
-  BulletCastBVHManager::Ptr manager(new BulletCastBVHManager());
+  auto manager = std::make_shared<BulletCastBVHManager>();
 
   for (const auto& cow : link2cow_)
   {

tesseract/tesseract_collision/src/bullet/bullet_cast_simple_manager.cpp

@@ -64,7 +64,7 @@ BulletCastSimpleManager::BulletCastSimpleManager()
 
 ContinuousContactManager::Ptr BulletCastSimpleManager::clone() const
 {
-  BulletCastSimpleManager::Ptr manager(new BulletCastSimpleManager());
+  auto manager = std::make_shared<BulletCastSimpleManager>();
 
   for (const auto& cow : link2cow_)
   {

tesseract/tesseract_collision/src/bullet/bullet_discrete_bvh_manager.cpp

@@ -75,7 +75,7 @@ BulletDiscreteBVHManager::~BulletDiscreteBVHManager()
 
 DiscreteContactManager::Ptr BulletDiscreteBVHManager::clone() const
 {
-  BulletDiscreteBVHManager::Ptr manager(new BulletDiscreteBVHManager());
+  auto manager = std::make_shared<BulletDiscreteBVHManager>();
 
   for (const auto& cow : link2cow_)
   {

tesseract/tesseract_collision/src/bullet/bullet_discrete_simple_manager.cpp

@@ -65,7 +65,7 @@ BulletDiscreteSimpleManager::BulletDiscreteSimpleManager()
 
 DiscreteContactManager::Ptr BulletDiscreteSimpleManager::clone() const
 {
-  BulletDiscreteSimpleManager::Ptr manager(new BulletDiscreteSimpleManager());
+  auto manager = std::make_shared<BulletDiscreteSimpleManager>();
 
   for (const auto& cow : link2cow_)
   {