fix some memory leaks in ImportURDF / PhysicsServerCommandProcessor

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -38,8 +38,10 @@ struct BulletURDFInternalData
 	struct GUIHelperInterface* m_guiHelper;
 	char m_pathPrefix[1024];
 	btHashMap<btHashInt,btVector4> m_linkColors;
+    btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
 };
 
+
 void BulletURDFImporter::printTree()
 {
 //	btAssert(0);
@@ -507,7 +509,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 	}
 
 	//if we have a convex, tesselate into localVertices/localIndices
-	if (convexColShape)
+	if ((glmesh==0) && convexColShape)
 	{
 		btShapeHull* hull = new btShapeHull(convexColShape);
 		hull->buildHull(0.0);
@@ -549,6 +551,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 			glmesh->m_numvertices = glmesh->m_vertices->size();
 			glmesh->m_numIndices = glmesh->m_indices->size();
 		}
+        delete hull;
 		delete convexColShape;
 		convexColShape = 0;
 
@@ -582,6 +585,8 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
 			verticesOut.push_back(v);
 		}
 	}
+    delete glmesh;
+    
 }
 
 
@@ -811,12 +816,13 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 							b3Warning("issue extracting mesh from STL file %s\n", fullPath);
 						}
 
+                        delete glmesh;
+                       
 					} else
 					{
 						b3Warning("mesh geometry not found %s\n",fullPath);
 					}
 							
-							
 				}
 			}
 
@@ -888,10 +894,23 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
         
 }
 
+int BulletURDFImporter::getNumAllocatedCollisionShapes() const
+{
+    return m_data->m_allocatedCollisionShapes.size();
+}
+
+
+btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
+{
+    return m_data->m_allocatedCollisionShapes[index];
+}
+
  class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
 {
         
     btCompoundShape* compoundShape = new btCompoundShape();
+    m_data->m_allocatedCollisionShapes.push_back(compoundShape);
+    
     compoundShape->setMargin(0.001);
 #if USE_ROS_URDF_PARSER
     for (int v=0;v<(int)m_data->m_links[linkIndex]->collision_array.size();v++)

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -38,8 +38,13 @@ public:
 
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
 
+    ///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
+    
 	virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
     
+    virtual int getNumAllocatedCollisionShapes() const;
+    virtual class btCollisionShape* getAllocatedCollisionShape(int index);
+
 };
 
 

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -9,7 +9,37 @@ UrdfParser::UrdfParser()
 }
 UrdfParser::~UrdfParser()
 {
-	//todo(erwincoumans) delete memory
+    
+    for (int i=0;i<m_model.m_materials.size();i++)
+    {
+        UrdfMaterial** matPtr = m_model.m_materials.getAtIndex(i);
+        if (matPtr)
+        {
+            UrdfMaterial* mat = *matPtr;
+            delete mat;
+        }
+    }
+    
+    for (int i=0;i<m_model.m_links.size();i++)
+    {
+        UrdfLink** linkPtr = m_model.m_links.getAtIndex(i);
+        if (linkPtr)
+        {
+            UrdfLink* link = *linkPtr;
+            delete link;
+        }
+    }
+
+    for (int i=0;i<m_model.m_joints.size();i++)
+    {
+        UrdfJoint** jointPtr = m_model.m_joints.getAtIndex(i);
+        if (jointPtr)
+        {
+            UrdfJoint* joint = *jointPtr;
+            delete joint;
+        }
+    }
+    
 }
 
 static bool parseVector4(btVector4& vec4, const std::string& vector_str)

examples/OpenGLWindow/GLInstanceGraphicsShape.h

@@ -16,6 +16,19 @@ struct GLInstanceGraphicsShape
 	b3AlignedObjectArray<int>* 		m_indices;
 	int				m_numIndices;
 	float			m_scaling[4];
+    
+    GLInstanceGraphicsShape()
+    :m_vertices(0),
+    m_indices(0)
+    {
+        
+    }
+    
+    virtual ~GLInstanceGraphicsShape()
+    {
+        delete m_vertices;
+        delete m_indices;
+    }
 };
 
 #endif //GL_INSTANCE_GRAPHICS_SHAPE_H

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -565,6 +565,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 	}
 	m_data->m_strings.clear();
 
+    btAlignedObjectArray<btTypedConstraint*> constraints;
+    btAlignedObjectArray<btMultiBodyConstraint*> mbconstraints;
+    
 	
 	if (m_data->m_dynamicsWorld)
 	{
@@ -572,8 +575,17 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 		int i;
 		for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
 		{
-			m_data->m_dynamicsWorld->removeConstraint(m_data->m_dynamicsWorld->getConstraint(i));
+            btTypedConstraint* constraint =m_data->m_dynamicsWorld->getConstraint(i);
+            constraints.push_back(constraint);
+			m_data->m_dynamicsWorld->removeConstraint(constraint);
 		}
+        for (i=m_data->m_dynamicsWorld->getNumMultiBodyConstraints()-1;i>=0;i--)
+        {
+            btMultiBodyConstraint* mbconstraint = m_data->m_dynamicsWorld->getMultiBodyConstraint(i);
+            mbconstraints.push_back(mbconstraint);
+            m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint);
+        }
+        
 		for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
 		{
 			btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
@@ -585,7 +597,24 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 			m_data->m_dynamicsWorld->removeCollisionObject(obj);
 			delete obj;
 		}
+        for (i=m_data->m_dynamicsWorld->getNumMultibodies()-1;i>=0;i--)
+        {
+            btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(i);
+            m_data->m_dynamicsWorld->removeMultiBody(mb);
+            delete mb;
         }
+	}
+
+    for (int i=0;i<constraints.size();i++)
+    {
+        delete constraints[i];
+    }
+    constraints.clear();
+    for (int i=0;i<mbconstraints.size();i++)
+    {
+        delete mbconstraints[i];
+    }
+    mbconstraints.clear();
     //delete collision shapes
 	for (int j = 0; j<m_data->m_collisionShapes.size(); j++)
 	{
@@ -665,6 +694,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 
    
     bool loadOk =  u2b.loadURDF(fileName, useFixedBase);
+   
+    
     if (loadOk)
     {
 		//get a body index
@@ -695,9 +726,29 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 		MyMultiBodyCreator creation(m_data->m_guiHelper);
         
         ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
+        
+        
+        ///todo(erwincoumans) refactor this memory allocation issue
+        for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
+        {
+            btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
+            m_data->m_collisionShapes.push_back(shape);
+            if (shape->isCompound())
+            {
+                btCompoundShape* compound = (btCompoundShape*) shape;
+                for (int childIndex=0;childIndex<compound->getNumChildShapes();childIndex++)
+                {
+                    m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex));
+                }
+            }
+            
+        }
+        
         btMultiBody* mb = creation.getBulletMultiBody();
 		if (useMultiBody)
 		{
+            
+            
 			if (mb)
 			{
 				bodyHandle->m_multiBody = mb;
@@ -943,6 +994,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
 						
 						serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
+                        serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0;
+                        
 						if (m_data->m_urdfLinkNameMapper.size())
 						{
 							serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();