|
From: <is...@us...> - 2008-07-24 00:00:33
|
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.
|