37 #include "ompl/geometric/planners/sbl/pSBL.h"
38 #include "ompl/base/goals/GoalState.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include <boost/thread.hpp>
44 ompl::geometric::pSBL::pSBL(
const base::SpaceInformationPtr &si) : base::Planner(si,
"pSBL"),
57 ompl::geometric::pSBL::~pSBL(
void)
69 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
70 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
77 samplerArray_.clear();
89 removeList_.motions.clear();
90 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
97 for (
unsigned int i = 0 ; i < it->second->data.size() ; ++i)
99 if (it->second->data[i]->state)
100 si_->freeState(it->second->data[i]->state);
101 delete it->second->data[i];
106 void ompl::geometric::pSBL::threadSolve(
unsigned int tid,
const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
110 std::vector<Motion*> solution;
111 base::State *xstate = si_->allocState();
112 bool startTree = rng.uniformBool();
114 while (!sol->found && ptc ==
false)
117 while (retry && !sol->found && ptc ==
false)
119 removeList_.lock.lock();
120 if (!removeList_.motions.empty())
122 if (loopLock_.try_lock())
125 std::map<Motion*, bool> seen;
126 for (
unsigned int i = 0 ; i < removeList_.motions.size() ; ++i)
127 if (seen.find(removeList_.motions[i].motion) == seen.end())
128 removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen);
129 removeList_.motions.clear();
135 removeList_.lock.unlock();
138 if (sol->found || ptc)
141 loopLockCounter_.lock();
142 if (loopCounter_ == 0)
145 loopLockCounter_.unlock();
148 TreeData &tree = startTree ? tStart_ : tGoal_;
149 startTree = !startTree;
150 TreeData &otherTree = startTree ? tStart_ : tGoal_;
152 Motion *existing = selectMotion(rng, tree);
153 if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
157 Motion *motion =
new Motion(si_);
158 si_->copyState(motion->state, xstate);
159 motion->parent = existing;
160 motion->root = existing->root;
162 existing->lock.lock();
163 existing->children.push_back(motion);
164 existing->lock.unlock();
166 addMotion(tree, motion);
168 if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
174 PathGeometric *path =
new PathGeometric(si_);
175 for (
unsigned int i = 0 ; i < solution.size() ; ++i)
176 path->append(solution[i]->state);
177 pdef_->addSolutionPath(base::PathPtr(path),
false, 0.0);
183 loopLockCounter_.lock();
185 if (loopCounter_ == 0)
187 loopLockCounter_.unlock();
190 si_->freeState(xstate);
201 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
208 si_->copyState(motion->state, st);
209 motion->valid =
true;
210 motion->root = motion->state;
211 addMotion(tStart_, motion);
214 if (tGoal_.size == 0)
216 if (si_->satisfiesBounds(goal->
getState()) && si_->isValid(goal->
getState()))
219 si_->copyState(motion->state, goal->
getState());
220 motion->valid =
true;
221 motion->root = motion->state;
222 addMotion(tGoal_, motion);
225 OMPL_ERROR(
"%s: Goal state is invalid!", getName().c_str());
228 if (tStart_.size == 0)
230 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!", getName().c_str());
233 if (tGoal_.size == 0)
235 OMPL_ERROR(
"%s: Motion planning goal tree could not be initialized!", getName().c_str());
239 samplerArray_.resize(threadCount_);
241 OMPL_INFORM(
"%s: Starting with %d states", getName().c_str(), (
int)(tStart_.size + tGoal_.size));
247 std::vector<boost::thread*> th(threadCount_);
248 for (
unsigned int i = 0 ; i < threadCount_ ; ++i)
249 th[i] =
new boost::thread(boost::bind(&pSBL::threadSolve,
this, i, ptc, &sol));
250 for (
unsigned int i = 0 ; i < threadCount_ ; ++i)
256 OMPL_INFORM(
"%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)",
257 getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
258 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
263 bool ompl::geometric::pSBL::checkSolution(
RNG &rng,
bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
266 projectionEvaluator_->computeCoordinates(motion->state, coord);
268 otherTree.lock.lock();
271 if (cell && !cell->
data.empty())
274 otherTree.lock.unlock();
276 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
278 Motion *connect =
new Motion(si_);
280 si_->copyState(connect->state, connectOther->state);
281 connect->parent = motion;
282 connect->root = motion->root;
285 motion->children.push_back(connect);
286 motion->lock.unlock();
288 addMotion(tree, connect);
290 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
293 connectionPoint_ = std::make_pair(motion->state, connectOther->state);
295 connectionPoint_ = std::make_pair(connectOther->state, motion->state);
299 std::vector<Motion*> mpath1;
300 while (motion != NULL)
302 mpath1.push_back(motion);
303 motion = motion->parent;
306 std::vector<Motion*> mpath2;
307 while (connectOther != NULL)
309 mpath2.push_back(connectOther);
310 connectOther = connectOther->parent;
316 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
317 solution.push_back(mpath1[i]);
318 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
325 otherTree.lock.unlock();
330 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
332 std::vector<Motion*> mpath;
335 while (motion != NULL)
337 mpath.push_back(motion);
338 motion = motion->parent;
344 for (
int i = mpath.size() - 1 ; result && i >= 0 ; --i)
346 mpath[i]->lock.lock();
347 if (!mpath[i]->valid)
349 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
350 mpath[i]->valid =
true;
354 PendingRemoveMotion prm;
356 prm.motion = mpath[i];
357 removeList_.lock.lock();
358 removeList_.motions.push_back(prm);
359 removeList_.lock.unlock();
363 mpath[i]->lock.unlock();
372 GridCell* cell = tree.pdf.sample(rng.uniform01());
373 Motion* result = cell && !cell->
data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] : NULL;
378 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen)
383 Grid<MotionInfo>::Coord coord;
384 projectionEvaluator_->computeCoordinates(motion->state, coord);
385 Grid<MotionInfo>::Cell* cell = tree.grid.
getCell(coord);
388 for (
unsigned int i = 0 ; i < cell->data.size(); ++i)
389 if (cell->data[i] == motion)
391 cell->data.erase(cell->data.begin() + i);
395 if (cell->data.empty())
397 tree.pdf.remove(cell->data.elem_);
398 tree.grid.remove(cell);
399 tree.grid.destroyCell(cell);
403 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
411 for (
unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
412 if (motion->parent->children[i] == motion)
414 motion->parent->children.erase(motion->parent->children.begin() + i);
420 for (
unsigned int i = 0 ; i < motion->children.size() ; ++i)
422 motion->children[i]->parent = NULL;
423 removeMotion(tree, motion->children[i], seen);
427 si_->freeState(motion->state);
431 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
433 Grid<MotionInfo>::Coord coord;
434 projectionEvaluator_->computeCoordinates(motion->state, coord);
436 Grid<MotionInfo>::Cell* cell = tree.grid.getCell(coord);
439 cell->data.push_back(motion);
440 tree.pdf.update(cell->data.elem_, 1.0/cell->data.size());
444 cell = tree.grid.createCell(coord);
445 cell->data.push_back(motion);
447 cell->data.elem_ = tree.pdf.add(cell, 1.0);
455 Planner::getPlannerData(data);
457 std::vector<MotionInfo> motions;
458 tStart_.grid.getContent(motions);
460 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
461 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
462 if (motions[i][j]->parent == NULL)
469 tGoal_.grid.getContent(motions);
470 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
471 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
472 if (motions[i][j]->parent == NULL)
484 assert(nthreads > 0);
485 threadCount_ = nthreads;
unsigned int getThreadCount(void) const
Get the thread count.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Representation of a simple grid.
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
The planner failed to find a solution.
GoalType recognizedGoal
The type of goal specification the planner can use.
std::vector< int > Coord
Definition of a coordinate within this grid.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition of a goal state.
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
_T data
The data we store in the cell.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
double getRange(void) const
Get the range the planner is using.
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
iterator begin(void) const
Return the begin() iterator for the grid.
The goal is of a type that a planner does not recognize.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
The planner found an exact solution.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
A class to store the exit status of Planner::solve()
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition of a cell in this grid.
const State * getState(void) const
Get the goal state.
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
This bit is set if casting to goal state (ompl::base::GoalState) is possible.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setRange(double distance)
Set the range the planner is supposed to use.
iterator end(void) const
Return the end() iterator for the grid.
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
double maxDistance_
The maximum length of a motion to be added in the tree.
CoordHash::const_iterator iterator
We only allow const iterators.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.