Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19500] pkg/trunk
|
From: <is...@us...> - 2009-07-23 04:43:32
|
Revision: 19500
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19500&view=rev
Author: isucan
Date: 2009-07-23 04:43:29 +0000 (Thu, 23 Jul 2009)
Log Message:
-----------
parallel collision checking should work in planning
Modified Paths:
--------------
pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h
pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/StateValidator.cpp
pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
Modified: pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h 2009-07-23 04:04:45 UTC (rev 19499)
+++ pkg/trunk/motion_planning/ompl_planning/include/ompl_planning/extensions/StateValidator.h 2009-07-23 04:43:29 UTC (rev 19500)
@@ -44,6 +44,7 @@
#include "ompl_planning/ModelBase.h"
#include "ompl_planning/extensions/SpaceInformation.h"
+#include <boost/thread.hpp>
#include <iostream>
namespace ompl_planning
@@ -56,16 +57,14 @@
{
ksi_ = si;
dsi_ = NULL;
- model_ = model;
- setupModel();
+ setupModel(model);
}
StateValidityPredicate(SpaceInformationDynamicModel *si, ModelBase *model) : ompl::base::StateValidityChecker()
{
dsi_ = si;
ksi_ = NULL;
- model_ = model;
- setupModel();
+ setupModel(model);
}
virtual ~StateValidityPredicate(void)
@@ -81,13 +80,13 @@
protected:
+ void setupModel(ModelBase *model);
bool check(const ompl::base::State *s, collision_space::EnvironmentModel *em, planning_models::KinematicModel *km,
planning_environment::KinematicConstraintEvaluatorSet *kce) const;
void useConstraints(planning_environment::KinematicConstraintEvaluatorSet *kce, planning_models::KinematicModel *km) const;
- void setupModel(void);
void clearClones(void);
-
+
ModelBase *model_;
// one of the next two will be instantiated
@@ -105,11 +104,9 @@
planning_environment::KinematicConstraintEvaluatorSet *kce;
};
- mutable std::vector<Clone> clones_;
- mutable int position_;
+ mutable std::map<boost::thread::id, Clone> clones_;
mutable boost::mutex lock_;
-
};
} // ompl_planning
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 04:04:45 UTC (rev 19499)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/ompl_extensions/StateValidator.cpp 2009-07-23 04:43:29 UTC (rev 19500)
@@ -36,6 +36,15 @@
#include "ompl_planning/extensions/StateValidator.h"
+void ompl_planning::StateValidityPredicate::setupModel(ModelBase *model)
+{
+ model_ = model;
+ boost::thread::id id = boost::this_thread::get_id();
+ clones_[id].em = model_->collisionSpace;
+ clones_[id].km = model_->kmodel;
+ clones_[id].kce = &kce_;
+}
+
bool ompl_planning::StateValidityPredicate::operator()(const ompl::base::State *s) const
{
// for dynamic state spaces, we may get outside bounds
@@ -44,31 +53,23 @@
if (!dsi_->satisfiesBounds(s))
return false;
}
+
+ boost::thread::id id = boost::this_thread::get_id();
lock_.lock();
- int p = position_++;
-
- // if this is a new thread, we create an additional clone
- if (p == (int)clones_.size())
+ if (clones_.find(id) == clones_.end())
{
- ROS_DEBUG("Cloning collision environment (index %d)", p);
- clones_.resize(p + 1);
- clones_[p].em = clones_[0].em->clone();
- clones_[p].km = clones_[p].em->getRobotModel().get();
- clones_[p].kce = new planning_environment::KinematicConstraintEvaluatorSet();
- useConstraints(clones_[p].kce, clones_[p].km);
+ ROS_DEBUG("Cloning collision environment");
+ Clone &add = clones_[id];
+ add.em = model_->collisionSpace->clone();
+ add.km = add.em->getRobotModel().get();
+ add.kce = new planning_environment::KinematicConstraintEvaluatorSet();
+ useConstraints(add.kce, add.km);
}
-
+ const Clone c = clones_[id];
lock_.unlock();
- const Clone &c = clones_[p];
- bool res = check(s, c.em, c.km, c.kce);
-
- lock_.lock();
- --position_;
- lock_.unlock();
-
- return res;
+ return check(s, c.em, c.km, c.kce);
}
void ompl_planning::StateValidityPredicate::setConstraints(const motion_planning_msgs::KinematicConstraints &kc)
@@ -108,7 +109,6 @@
{
clearConstraints();
clearClones();
- position_ = 0;
}
void ompl_planning::StateValidityPredicate::printSettings(std::ostream &out) const
@@ -132,21 +132,17 @@
return valid;
}
-void ompl_planning::StateValidityPredicate::setupModel(void)
-{
- clones_.resize(1);
- clones_[0].km = model_->kmodel;
- clones_[0].em = model_->collisionSpace;
- clones_[0].kce = &kce_;
- position_ = 0;
-}
-
void ompl_planning::StateValidityPredicate::clearClones(void)
{
- for (unsigned int i = 1 ; i < clones_.size() ; ++i)
+ boost::thread::id id = boost::this_thread::get_id();
+ Clone keep = clones_[id];
+ for (std::map<boost::thread::id, Clone>::iterator it = clones_.begin() ; it != clones_.end() ; ++it)
{
- delete clones_[i].em; // .km is owned & deleted by .em
- delete clones_[i].kce;
+ if (it->first == id)
+ continue;
+ delete it->second.em; // .km is owned & deleted by .em
+ delete it->second.kce;
}
- clones_.resize(1);
+ clones_.clear();
+ clones_[id] = keep;
}
Modified: pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
===================================================================
--- pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml 2009-07-23 04:04:45 UTC (rev 19499)
+++ pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml 2009-07-23 04:43:29 UTC (rev 19500)
@@ -28,7 +28,7 @@
l_wrist_flex_link
l_wrist_roll_link
planner_configs:
- RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l
+ RRTkConfig2 pRRTkConfig1 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l
right_arm:
links:
@@ -42,7 +42,7 @@
r_wrist_flex_link
r_wrist_roll_link
planner_configs:
- RRTkConfig2 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l
+ RRTkConfig2 pRRTkConfig1 SBLkConfig2 KPIECEkConfig1 KPIECEkConfig2l LBKPIECEkConfig2l
arms:
links:
@@ -73,7 +73,7 @@
planner_configs:
- pRRTConfig1:
+ pRRTkConfig1:
type: kinematic::pRRT
range: 0.75
thread_count: 2
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
