37 #include "ompl/geometric/planners/sbl/pSBL.h" 38 #include "ompl/base/goals/GoalState.h" 39 #include "ompl/tools/config/SelfConfig.h" 43 ompl::geometric::pSBL::pSBL(
const base::SpaceInformationPtr &si) : base::Planner(si,
"pSBL"), samplerArray_(si)
53 ompl::geometric::pSBL::~pSBL()
65 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
66 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
73 samplerArray_.clear();
85 removeList_.motions.clear();
86 connectionPoint_ = std::make_pair<base::State *, base::State *>(
nullptr,
nullptr);
91 for (
const auto &it : grid)
93 for (
unsigned int i = 0; i < it.second->data.size(); ++i)
95 if (it.second->data[i]->state)
96 si_->freeState(it.second->data[i]->state);
97 delete it.second->data[i];
102 void ompl::geometric::pSBL::threadSolve(
unsigned int tid,
const base::PlannerTerminationCondition &ptc,
107 std::vector<Motion *> solution;
108 base::State *xstate = si_->allocState();
109 bool startTree = rng.uniformBool();
111 while (!sol->found && ptc ==
false)
114 while (retry && !sol->found && ptc ==
false)
116 removeList_.lock.lock();
117 if (!removeList_.motions.empty())
119 if (loopLock_.try_lock())
122 std::map<Motion *, bool> seen;
123 for (
auto &motion : removeList_.motions)
124 if (seen.find(motion.motion) == seen.end())
125 removeMotion(*motion.tree, motion.motion, seen);
126 removeList_.motions.clear();
132 removeList_.lock.unlock();
135 if (sol->found || ptc)
138 loopLockCounter_.lock();
139 if (loopCounter_ == 0)
142 loopLockCounter_.unlock();
144 TreeData &tree = startTree ? tStart_ : tGoal_;
145 startTree = !startTree;
146 TreeData &otherTree = startTree ? tStart_ : tGoal_;
148 Motion *existing = selectMotion(rng, tree);
149 if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_))
153 auto *motion =
new Motion(si_);
154 si_->copyState(motion->state, xstate);
155 motion->parent = existing;
156 motion->root = existing->root;
158 existing->lock.lock();
159 existing->children.push_back(motion);
160 existing->lock.unlock();
162 addMotion(tree, motion);
164 if (checkSolution(rng, !startTree, tree, otherTree, motion, solution))
170 auto path(std::make_shared<PathGeometric>(si_));
171 for (
auto &i : solution)
172 path->append(i->state);
173 pdef_->addSolutionPath(path,
false, 0.0, getName());
178 loopLockCounter_.lock();
180 if (loopCounter_ == 0)
182 loopLockCounter_.unlock();
185 si_->freeState(xstate);
196 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
202 auto *motion =
new Motion(si_);
203 si_->copyState(motion->state, st);
204 motion->valid =
true;
205 motion->root = motion->state;
206 addMotion(tStart_, motion);
209 if (tGoal_.size == 0)
211 if (si_->satisfiesBounds(goal->getState()) && si_->isValid(goal->getState()))
213 auto *motion =
new Motion(si_);
214 si_->copyState(motion->state, goal->getState());
215 motion->valid =
true;
216 motion->root = motion->state;
217 addMotion(tGoal_, motion);
220 OMPL_ERROR(
"%s: Goal state is invalid!", getName().c_str());
223 if (tStart_.size == 0)
225 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!", getName().c_str());
228 if (tGoal_.size == 0)
230 OMPL_ERROR(
"%s: Motion planning goal tree could not be initialized!", getName().c_str());
234 samplerArray_.resize(threadCount_);
236 OMPL_INFORM(
"%s: Starting planning with %d states already in datastructure", getName().c_str(),
237 (
int)(tStart_.size + tGoal_.size));
243 std::vector<std::thread *> th(threadCount_);
244 for (
unsigned int i = 0; i < threadCount_; ++i)
245 th[i] =
new std::thread([
this, i, &ptc, &sol]
247 threadSolve(i, ptc, &sol);
249 for (
unsigned int i = 0; i < threadCount_; ++i)
255 OMPL_INFORM(
"%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
256 tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(),
257 tStart_.grid.size(), tGoal_.grid.size());
262 bool ompl::geometric::pSBL::checkSolution(
RNG &rng,
bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
263 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,
277 start ? connectOther->root : motion->root))
279 auto *connect =
new Motion(si_);
281 si_->copyState(connect->state, connectOther->state);
282 connect->parent = motion;
283 connect->root = motion->root;
286 motion->children.push_back(connect);
287 motion->lock.unlock();
289 addMotion(tree, connect);
291 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
294 connectionPoint_ = std::make_pair(motion->state, connectOther->state);
296 connectionPoint_ = std::make_pair(connectOther->state, motion->state);
300 std::vector<Motion *> mpath1;
301 while (motion !=
nullptr)
303 mpath1.push_back(motion);
304 motion = motion->parent;
307 std::vector<Motion *> mpath2;
308 while (connectOther !=
nullptr)
310 mpath2.push_back(connectOther);
311 connectOther = connectOther->parent;
317 for (
int i = mpath1.size() - 1; i >= 0; --i)
318 solution.push_back(mpath1[i]);
319 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
326 otherTree.lock.unlock();
331 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
333 std::vector<Motion *> mpath;
336 while (motion !=
nullptr)
338 mpath.push_back(motion);
339 motion = motion->parent;
345 for (
int i = mpath.size() - 1; result && i >= 0; --i)
347 mpath[i]->lock.lock();
348 if (!mpath[i]->valid)
350 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
351 mpath[i]->valid =
true;
355 PendingRemoveMotion prm;
357 prm.motion = mpath[i];
358 removeList_.lock.lock();
359 removeList_.motions.push_back(prm);
360 removeList_.lock.unlock();
364 mpath[i]->lock.unlock();
373 GridCell *cell = tree.pdf.sample(rng.uniform01());
374 Motion *result = cell && !cell->
data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] :
nullptr;
379 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen)
385 projectionEvaluator_->computeCoordinates(motion->state, coord);
386 Grid<MotionInfo>::Cell *cell = tree.grid.
getCell(coord);
389 for (
unsigned int i = 0; i < cell->data.size(); ++i)
390 if (cell->data[i] == motion)
392 cell->data.erase(cell->data.begin() + i);
396 if (cell->data.empty())
398 tree.pdf.remove(cell->data.elem_);
399 tree.grid.remove(cell);
400 tree.grid.destroyCell(cell);
404 tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
412 for (
unsigned int i = 0; i < motion->parent->children.size(); ++i)
413 if (motion->parent->children[i] == motion)
415 motion->parent->children.erase(motion->parent->children.begin() + i);
421 for (
auto &i : motion->children)
424 removeMotion(tree, i, seen);
428 si_->freeState(motion->state);
432 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
435 projectionEvaluator_->computeCoordinates(motion->state, coord);
437 Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
440 cell->data.push_back(motion);
441 tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
445 cell = tree.grid.createCell(coord);
446 cell->data.push_back(motion);
448 cell->data.elem_ = tree.pdf.add(cell, 1.0);
456 Planner::getPlannerData(data);
458 std::vector<MotionInfo> motionInfo;
459 tStart_.grid.getContent(motionInfo);
461 for (
auto &m : motionInfo)
462 for (
auto &motion : m.motions_)
463 if (motion->parent ==
nullptr)
470 tGoal_.grid.getContent(motionInfo);
471 for (
auto &m : motionInfo)
472 for (
auto &motion : m.motions_)
473 if (motion->parent ==
nullptr)
485 assert(nthreads > 0);
486 threadCount_ = nthreads;
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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.
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...
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...
_T data
The data we store in the cell.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
std::vector< int > Coord
Definition of a coordinate within this grid.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
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.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
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.
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.
unsigned int getThreadCount() const
Get the thread count.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition of a cell in this grid.
This bit is set if casting to goal state (ompl::base::GoalState) is possible.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
void setRange(double distance)
Set the range the planner is supposed to use.
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
double getRange() const
Get the range the planner is using.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.