[Personalrobots-commit] SF.net SVN: personalrobots:[2016] pkg/trunk

[<is...@us...>]

Revision: 2016
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2016&view=rev
Author:   isucan
Date:     2008-07-24 00:00:40 +0000 (Thu, 24 Jul 2008)

Log Message:
-----------
added clear option for collision spaces

Modified Paths:
--------------
    pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
    pkg/trunk/util/collision_space/include/collision_space/environment.h
    pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
    pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp

Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp	2008-07-24 00:00:40 UTC (rev 2016)
@@ -148,7 +148,8 @@
 	    data[i3 + 1] = m_cloud.pts[i].y;
 	    data[i3 + 2] = m_cloud.pts[i].z;
 	}
-
+	
+	m_collisionSpace->clearObstacles();
 	m_collisionSpace->addPointCloud(n, data, 0.01);
 	
 	delete[] data;

Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h	2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h	2008-07-24 00:00:40 UTC (rev 2016)
@@ -62,6 +62,9 @@
 	/** Check if a model is in collision */
 	virtual bool isCollision(unsigned int model_id) = 0;
 	
+	/** Remove all obstacles from collision model */
+	virtual void clearObstacles(void) = 0;
+	
 	/** Add a point cloud to the collision space */
 	virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
 

Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h	2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h	2008-07-24 00:00:40 UTC (rev 2016)
@@ -56,11 +56,7 @@
 	
 	~EnvironmentModelODE(void)
 	{
-	    for (unsigned int i = 0 ; i < m_kgeoms.size() ; ++i)
-		for (unsigned int j = 0 ; j < m_kgeoms[i].size() ; ++j)
-		    delete m_kgeoms[i][j];
-	    if (m_space)
-		dSpaceDestroy(m_space);
+	    freeMemory();
 	}
 	
 	dSpaceID getODESpace(void) const;
@@ -68,6 +64,9 @@
 	/** Check if a model is in collision */
 	virtual bool isCollision(unsigned int model_id);
 	
+	/** Remove all obstacles from collision model */
+	virtual void clearObstacles(void);
+
 	/** Add a point cloud to the collision space */
 	virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01); 
 
@@ -78,7 +77,7 @@
 	virtual void updateRobotModel(unsigned int model_id);
 
     protected:
-	
+		
 	class ODECollide2
 	{
 	public:
@@ -148,6 +147,8 @@
 	    dGeomID                                geom;
 	    planning_models::KinematicModel::Link *link;
 	};
+
+	void freeMemory(void);	
 	
 	std::vector< std::vector< kGeom* > > m_kgeoms;
 

Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp	2008-07-23 23:53:12 UTC (rev 2015)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp	2008-07-24 00:00:40 UTC (rev 2016)
@@ -34,6 +34,15 @@
 
 #include <collision_space/environmentODE.h>
 
+void collision_space::EnvironmentModelODE::freeMemory(void)
+{ 
+    for (unsigned int i = 0 ; i < m_kgeoms.size() ; ++i)
+	for (unsigned int j = 0 ; j < m_kgeoms[i].size() ; ++j)
+	    delete m_kgeoms[i][j];
+    if (m_space)
+	dSpaceDestroy(m_space);
+}
+
 unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model)
 {
     unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
@@ -194,3 +203,11 @@
     }
     m_collide2.setup();
 }
+
+void collision_space::EnvironmentModelODE::clearObstacles(void)
+{
+    m_collide2.clear();
+    freeMemory();
+    m_space = dHashSpaceCreate(0);
+    m_collide2.setup();
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.