Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19515] pkg/trunk
|
From: <is...@us...> - 2009-07-23 17:07:37
|
Revision: 19515
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19515&view=rev
Author: isucan
Date: 2009-07-23 17:07:25 +0000 (Thu, 23 Jul 2009)
Log Message:
-----------
made ODE collision checker more robust wrt multiple threads
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/StateValidator.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-23 17:07:25 UTC (rev 19515)
@@ -413,7 +413,7 @@
psetup->smoother->smoothMax(path);
double tsmooth = (ros::WallTime::now() - startTime).toSec();
ROS_DEBUG(" Smoother spent %g seconds (%g seconds in total)", tsmooth, tsmooth + tsolve);
- dynamic_cast<SpaceInformationKinematicModel*>(psetup->si)->interpolatePath(path);
+ dynamic_cast<SpaceInformationKinematicModel*>(psetup->si)->interpolatePath(path, 3.0);
}
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/StateValidator.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/StateValidator.cpp 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/StateValidator.cpp 2009-07-23 17:07:25 UTC (rev 19515)
@@ -134,15 +134,20 @@
void ompl_planning::StateValidityPredicate::clearClones(void)
{
- boost::thread::id id = boost::this_thread::get_id();
- Clone keep = clones_[id];
+ std::map<boost::thread::id, Clone>::iterator keep = clones_.end();
for (std::map<boost::thread::id, Clone>::iterator it = clones_.begin() ; it != clones_.end() ; ++it)
{
- if (it->first == id)
+ if (it->second.em == model_->collisionSpace)
+ {
+ keep = it;
continue;
+ }
delete it->second.em; // .km is owned & deleted by .em
delete it->second.kce;
}
+ assert(keep != clones_.end());
+ boost::thread::id id = keep->first;
+ Clone c = keep->second;
clones_.clear();
- clones_[id] = keep;
+ clones_[id] = c;
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-23 17:07:25 UTC (rev 19515)
@@ -46,7 +46,7 @@
OMPLPlanning(void)
{
// display the first 3 coordinates of states in diffusion trees
- requestHandler_.enableDebugMode(0, 1);
+ // requestHandler_.enableDebugMode(0, 1);
// register with ROS
collisionModels_ = new planning_environment::CollisionModels("robot_description");
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environment.h 2009-07-23 17:07:25 UTC (rev 19515)
@@ -183,7 +183,7 @@
/** \brief Check the state of verbosity */
bool getVerbose(void) const;
- /** \brief Clone the environment. If this clone is to be used for collision checking in another thread, the call to clone() MUST be made from that thread. */
+ /** \brief Clone the environment. */
virtual EnvironmentModel* clone(void) const = 0;
protected:
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentBullet.h 2009-07-23 17:07:25 UTC (rev 19515)
@@ -106,7 +106,7 @@
/** \brief Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
virtual int setCollisionCheck(const std::string &link, bool state);
- /** \brief Clone the environment. If this clone is to be used for collision checking in another thread, the call to clone() MUST be made from that thread. */
+ /** \brief Clone the environment */
virtual EnvironmentModel* clone(void) const;
protected:
Modified: pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/world_models/collision_space/include/collision_space/environmentODE.h 2009-07-23 17:07:25 UTC (rev 19515)
@@ -52,17 +52,8 @@
public:
- EnvironmentModelODE(void) : EnvironmentModel()
- {
- dInitODE2(0);
- dAllocateODEDataForThread(dAllocateMaskAll);
- }
-
- virtual ~EnvironmentModelODE(void)
- {
- freeMemory();
- dCloseODE();
- }
+ EnvironmentModelODE(void);
+ virtual ~EnvironmentModelODE(void);
/** \brief Get the list of contacts (collisions) */
virtual bool getCollisionContacts(std::vector<Contact> &contacts, unsigned int max_count = 1);
@@ -105,7 +96,7 @@
/** \brief Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
virtual int setCollisionCheck(const std::string &link, bool state);
- /** \brief Clone the environment. If this clone is to be used for collision checking in another thread, the call to clone() MUST be made from that thread. */
+ /** \brief Clone the environment */
virtual EnvironmentModel* clone(void) const;
protected:
@@ -325,6 +316,9 @@
void createODERobotModel(void);
dGeomID createODEGeom(dSpaceID space, ODEStorage &storage, const shapes::Shape *shape, double scale, double padding);
void updateGeom(dGeomID geom, const btTransform &pose) const;
+
+ /** \brief Check if thread-specific routines have been called */
+ void checkThreadInit(void) const;
void freeMemory(void);
ModelInfo m_modelGeom;
Modified: pkg/trunk/world_models/collision_space/src/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/world_models/collision_space/src/environmentODE.cpp 2009-07-23 17:07:25 UTC (rev 19515)
@@ -35,12 +35,52 @@
/** \author Ioan Sucan */
#include "collision_space/environmentODE.h"
+#include <boost/thread.hpp>
#include <cassert>
#include <cstdio>
#include <cmath>
#include <algorithm>
#include <map>
+static int ODEInitCount = 0;
+static boost::mutex ODEInitCountLock;
+
+static std::map<boost::thread::id, int> ODEThreadMap;
+static boost::mutex ODEThreadMapLock;
+
+collision_space::EnvironmentModelODE::EnvironmentModelODE(void) : EnvironmentModel()
+{
+ ODEInitCountLock.lock();
+ if (ODEInitCount == 0)
+ dInitODE2(0);
+ ODEInitCount++;
+ ODEInitCountLock.unlock();
+
+ checkThreadInit();
+}
+
+collision_space::EnvironmentModelODE::~EnvironmentModelODE(void)
+{
+ freeMemory();
+ ODEInitCountLock.lock();
+ ODEInitCount--;
+ if (ODEInitCount == 0)
+ dCloseODE();
+ ODEInitCountLock.unlock();
+}
+
+void collision_space::EnvironmentModelODE::checkThreadInit(void) const
+{
+ boost::thread::id id = boost::this_thread::get_id();
+ ODEThreadMapLock.lock();
+ if (ODEThreadMap.find(id) == ODEThreadMap.end())
+ {
+ ODEThreadMap[id] = 1;
+ dAllocateODEDataForThread(dAllocateMaskAll);
+ }
+ ODEThreadMapLock.unlock();
+}
+
void collision_space::EnvironmentModelODE::freeMemory(void)
{
for (unsigned int j = 0 ; j < m_modelGeom.linkGeom.size() ; ++j)
@@ -440,6 +480,7 @@
cdata.contacts = &contacts;
cdata.max_contacts = max_count;
contacts.clear();
+ checkThreadInit();
testCollision(&cdata);
return cdata.collides;
}
@@ -448,6 +489,7 @@
{
CollisionData cdata;
cdata.selfCollisionTest = &m_selfCollisionTest;
+ checkThreadInit();
testCollision(&cdata);
return cdata.collides;
}
@@ -456,6 +498,7 @@
{
CollisionData cdata;
cdata.selfCollisionTest = &m_selfCollisionTest;
+ checkThreadInit();
testSelfCollision(&cdata);
return cdata.collides;
}
Modified: pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
===================================================================
--- pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-07-23 13:46:02 UTC (rev 19514)
+++ pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp 2009-07-23 17:07:25 UTC (rev 19515)
@@ -180,16 +180,9 @@
ROS_INFO("Thread %d: %f collision tests per second (with self collision checking)", tid, (double)K/(ros::WallTime::now() - tm).toSec());
delete em;
}
-
- void testThreads(void)
+
+ void collisionSetupThread(collision_space::EnvironmentModel *em)
{
- if (!cm_->loadedModels())
- return;
-
- collision_space::EnvironmentModel *em = cm_->getODECollisionModel().get();
- em->setSelfCollision(true);
- em->updateRobotModel();
-
const int n = 10000;
double *data = new double[n * 4];
@@ -213,8 +206,20 @@
ROS_INFO("Added %d points", n);
delete[] data;
-
+ }
+
+ void testThreads(void)
+ {
+ if (!cm_->loadedModels())
+ return;
+ collision_space::EnvironmentModel *em = cm_->getODECollisionModel().get();
+ em->setSelfCollision(true);
+ em->updateRobotModel();
+
+ boost::thread t(boost::bind(&CollisionTestSpeed::collisionSetupThread, this, em));
+ t.join();
+
int nt = 2;
std::vector<boost::thread*> th(nt);
for (int i = 0 ; i < nt ; ++i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
