37 #ifndef OMPL_GEOMETRIC_PLANNERS_SBL_SBL_
38 #define OMPL_GEOMETRIC_PLANNERS_SBL_SBL_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/ProjectionEvaluator.h"
42 #include "ompl/datastructures/Grid.h"
43 #include "ompl/datastructures/PDF.h"
130 virtual void setup(
void);
134 virtual void clear(
void);
182 Motion* operator[](
unsigned int i)
186 std::vector<Motion*>::iterator begin (
void)
188 return motions_.begin ();
190 void erase (std::vector<Motion*>::iterator iter)
192 motions_.erase (iter);
196 motions_.push_back(m);
198 unsigned int size(
void)
const
200 return motions_.size();
202 bool empty(
void)
const
204 return motions_.empty();
207 std::vector<Motion*> motions_;
208 CellPDF::Element* elem_;
239 void addMotion(TreeData &tree, Motion *motion);
255 bool checkSolution(
bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
TreeData tGoal_
The goal tree.
Motion * parent
The parent motion – it contains the state this motion originates at.
Representation of a simple grid.
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
void addMotion(TreeData &tree, Motion *motion)
Add a motion to a tree.
std::vector< Motion * > children
The set of motions descending from the current motion.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const base::ProjectionEvaluatorPtr & getProjectionEvaluator(void) const
Get the projection evaluator.
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
bool isPathValid(TreeData &tree, Motion *motion)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
base::State * state
The state this motion leads to.
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
void freeMemory(void)
Free the memory allocated by the planner.
void freeGridMotions(Grid< MotionInfo > &grid)
Free the memory used by the motions contained in a grid.
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state...
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
unsigned int size
The number of motions (in total) from the tree.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double getRange(void) const
Get the range the planner is using.
A struct containing an array of motions and a corresponding PDF element.
Base class for a planner.
Motion(void)
Default constructor. Allocates no memory.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
const base::State * root
The root of the tree this motion would get to, if we were to follow parent pointers.
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
SBL(const base::SpaceInformationPtr &si)
The constructor needs the instance of the space information.
Definition of an abstract state.
Grid< MotionInfo > grid
The grid of motions corresponding to this tree.
bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector< Motion * > &solution)
Check if a solution can be obtained by connecting two trees using a specified motion.
void setRange(double distance)
Set the range the planner is supposed to use.
Definition of a cell in this grid.
base::ValidStateSamplerPtr sampler_
The employed state sampler.
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Grid< MotionInfo >::Cell GridCell
A grid cell.
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates storage for a state.
bool valid
Flag indicating whether this motion has been checked for validity.
RNG rng_
The random number generator to be used.
Representation of a motion.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Representation of a search tree. Two instances will be used. One for start and one for goal...
SpaceInformationPtr si_
The space information for which planning is done.
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Single-Query Bi-Directional Probabilistic Roadmap Planner with Lazy Collision Checking.
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...
PDF< GridCell * > CellPDF
A PDF of grid cells.
double maxDistance_
The maximum length of a motion to be added in the tree.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
TreeData tStart_
The start tree.