37 #ifndef OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
38 #define OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/ProjectionEvaluator.h"
42 #include "ompl/base/StateSamplerArray.h"
43 #include "ompl/datastructures/Grid.h"
44 #include "ompl/datastructures/PDF.h"
45 #include <boost/thread/mutex.hpp>
99 projectionEvaluator_ = projectionEvaluator;
106 projectionEvaluator_ =
si_->getStateSpace()->getProjection(name);
112 return projectionEvaluator_;
122 maxDistance_ = distance;
140 virtual void setup(
void);
144 virtual void clear(
void);
163 Motion(
void) : root(NULL), state(NULL), parent(NULL), valid(
false)
179 std::vector<Motion*> children;
186 Motion* operator[](
unsigned int i)
190 std::vector<Motion*>::iterator begin (
void)
192 return motions_.begin ();
194 void erase (std::vector<Motion*>::iterator iter)
196 motions_.erase (iter);
200 motions_.push_back(m);
202 unsigned int size(
void)
const
204 return motions_.size();
206 bool empty(
void)
const
208 return motions_.empty();
210 std::vector<Motion*> motions_;
211 CellPDF::Element* elem_;
228 std::vector<Motion*> solution;
241 std::vector<PendingRemoveMotion> motions;
247 void freeMemory(
void)
249 freeGridMotions(tStart_.grid);
250 freeGridMotions(tGoal_.grid);
255 void addMotion(TreeData &tree, Motion *motion);
256 Motion* selectMotion(
RNG &rng, TreeData &tree);
257 void removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen);
258 bool isPathValid(TreeData &tree, Motion *motion);
259 bool checkSolution(
RNG &rng,
bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
268 MotionsToBeRemoved removeList_;
269 boost::mutex loopLock_;
270 boost::mutex loopLockCounter_;
271 unsigned int loopCounter_;
275 unsigned int threadCount_;
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.
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
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...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Base class for a planner.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
Definition of an abstract state.
Definition of a cell in this grid.
PDF< GridCell * > CellPDF
A PDF of grid cells.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
const base::ProjectionEvaluatorPtr & getProjectionEvaluator(void) const
Get the projection evaluator.
void setRange(double distance)
Set the range the planner is supposed to use.
SpaceInformationPtr si_
The space information for which planning is done.
Grid< MotionInfo >::Cell GridCell
A grid cell.
Parallel Single-query Bi-directional Lazy collision checking planner.
A struct containing an array of motions and a corresponding PDF element.
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.