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)
45 specs_.recognizedGoal = base::GOAL_STATE;
46 specs_.multithreaded =
true;
49 Planner::declareParam<double>(
"range",
this, &pSBL::setRange, &pSBL::getRange,
"0.:1.:10000.");
50 Planner::declareParam<unsigned int>(
"thread_count",
this, &pSBL::setThreadCount, &pSBL::getThreadCount,
"1:64");
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] { threadSolve(i, ptc, &sol); });
246 for (
unsigned int i = 0; i < threadCount_; ++i)
252 OMPL_INFORM(
"%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)", getName().c_str(),
253 tStart_.size + tGoal_.size, tStart_.size, tGoal_.size, tStart_.grid.size() + tGoal_.grid.size(),
254 tStart_.grid.size(), tGoal_.grid.size());
259 bool ompl::geometric::pSBL::checkSolution(
RNG &rng,
bool start, TreeData &tree, TreeData &otherTree, Motion *motion,
260 std::vector<Motion *> &solution)
263 projectionEvaluator_->computeCoordinates(motion->state, coord);
265 otherTree.lock.lock();
268 if (cell && !cell->
data.empty())
271 otherTree.lock.unlock();
273 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root,
274 start ? connectOther->root : motion->root))
276 auto *connect =
new Motion(si_);
278 si_->copyState(connect->state, connectOther->state);
279 connect->parent = motion;
280 connect->root = motion->root;
283 motion->children.push_back(connect);
284 motion->lock.unlock();
286 addMotion(tree, connect);
288 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
291 connectionPoint_ = std::make_pair(motion->state, connectOther->state);
293 connectionPoint_ = std::make_pair(connectOther->state, motion->state);
297 std::vector<Motion *> mpath1;
298 while (motion !=
nullptr)
300 mpath1.push_back(motion);
301 motion = motion->parent;
304 std::vector<Motion *> mpath2;
305 while (connectOther !=
nullptr)
307 mpath2.push_back(connectOther);
308 connectOther = connectOther->parent;
314 for (
int i = mpath1.size() - 1; i >= 0; --i)
315 solution.push_back(mpath1[i]);
316 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
323 otherTree.lock.unlock();
328 bool ompl::geometric::pSBL::isPathValid(TreeData &tree, Motion *motion)
330 std::vector<Motion *> mpath;
333 while (motion !=
nullptr)
335 mpath.push_back(motion);
336 motion = motion->parent;
342 for (
int i = mpath.size() - 1; result && i >= 0; --i)
344 mpath[i]->lock.lock();
345 if (!mpath[i]->valid)
347 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
348 mpath[i]->valid =
true;
352 PendingRemoveMotion prm;
354 prm.motion = mpath[i];
355 removeList_.lock.lock();
356 removeList_.motions.push_back(prm);
357 removeList_.lock.unlock();
361 mpath[i]->lock.unlock();
370 GridCell *cell = tree.pdf.sample(rng.uniform01());
371 Motion *result = cell && !cell->
data.empty() ? cell->data[rng.uniformInt(0, cell->data.size() - 1)] :
nullptr;
376 void ompl::geometric::pSBL::removeMotion(TreeData &tree, Motion *motion, std::map<Motion *, bool> &seen)
382 projectionEvaluator_->computeCoordinates(motion->state, coord);
383 Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
386 for (
unsigned int i = 0; i < cell->data.size(); ++i)
387 if (cell->data[i] == motion)
389 cell->data.erase(cell->data.begin() + i);
393 if (cell->data.empty())
395 tree.pdf.remove(cell->data.elem_);
396 tree.grid.remove(cell);
397 tree.grid.destroyCell(cell);
401 tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
409 for (
unsigned int i = 0; i < motion->parent->children.size(); ++i)
410 if (motion->parent->children[i] == motion)
412 motion->parent->children.erase(motion->parent->children.begin() + i);
418 for (
auto &i : motion->children)
421 removeMotion(tree, i, seen);
425 si_->freeState(motion->state);
429 void ompl::geometric::pSBL::addMotion(TreeData &tree, Motion *motion)
432 projectionEvaluator_->computeCoordinates(motion->state, coord);
434 Grid<MotionInfo>::Cell *cell = tree.grid.getCell(coord);
437 cell->data.push_back(motion);
438 tree.pdf.update(cell->data.elem_, 1.0 / cell->data.size());
442 cell = tree.grid.createCell(coord);
443 cell->data.push_back(motion);
445 cell->data.elem_ = tree.pdf.add(cell, 1.0);
453 Planner::getPlannerData(data);
455 std::vector<MotionInfo> motionInfo;
456 tStart_.grid.getContent(motionInfo);
458 for (
auto &m : motionInfo)
459 for (
auto &motion : m.motions_)
460 if (motion->parent ==
nullptr)
467 tGoal_.grid.getContent(motionInfo);
468 for (
auto &m : motionInfo)
469 for (
auto &motion : m.motions_)
470 if (motion->parent ==
nullptr)
482 assert(nthreads > 0);
483 threadCount_ = nthreads;
Representation of a simple grid.
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition of a goal state.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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...
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...
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...
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...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of an abstract state.
void setThreadCount(unsigned int nthreads)
Set the number of threads the planner should use. Default is 2.
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...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition of a cell in this grid.
_T data
The data we store in the cell.
A class to store the exit status of Planner::solve()
@ INVALID_START
Invalid start state or no start state specified.
@ EXACT_SOLUTION
The planner found an exact solution.
@ INVALID_GOAL
Invalid goal state.
@ UNRECOGNIZED_GOAL_TYPE
The goal is of a type that a planner does not recognize.
@ TIMEOUT
The planner failed to find a solution.