37 #include "ompl/geometric/planners/sbl/SBL.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
52 ompl::geometric::SBL::~SBL(
void)
64 tStart_.grid.setDimension(projectionEvaluator_->getDimension());
65 tGoal_.grid.setDimension(projectionEvaluator_->getDimension());
72 for (
unsigned int i = 0 ; i < it->second->data.size() ; ++i)
74 if (it->second->data[i]->state)
75 si_->freeState(it->second->data[i]->state);
76 delete it->second->data[i];
88 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
95 si_->copyState(motion->
state, st);
98 addMotion(tStart_, motion);
101 if (tStart_.size == 0)
103 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!", getName().c_str());
109 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
114 sampler_ = si_->allocValidStateSampler();
116 OMPL_INFORM(
"%s: Starting with %d states", getName().c_str(), (
int)(tStart_.size + tGoal_.size));
118 std::vector<Motion*> solution;
121 bool startTree =
true;
126 TreeData &tree = startTree ? tStart_ : tGoal_;
127 startTree = !startTree;
128 TreeData &otherTree = startTree ? tStart_ : tGoal_;
131 if (tGoal_.size == 0 || pis_.getSampledGoalsCount() < tGoal_.size / 2)
133 const base::State *st = tGoal_.size == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
137 si_->copyState(motion->
state, st);
139 motion->
valid =
true;
140 addMotion(tGoal_, motion);
142 if (tGoal_.size == 0)
144 OMPL_ERROR(
"%s: Unable to sample any valid states for goal tree", getName().c_str());
149 Motion *existing = selectMotion(tree);
151 if (!sampler_->sampleNear(xstate, existing->
state, maxDistance_))
156 si_->copyState(motion->
state, xstate);
157 motion->
parent = existing;
159 existing->
children.push_back(motion);
161 addMotion(tree, motion);
163 if (checkSolution(!startTree, tree, otherTree, motion, solution))
166 for (
unsigned int i = 0 ; i < solution.size() ; ++i)
167 path->
append(solution[i]->state);
175 si_->freeState(xstate);
177 OMPL_INFORM(
"%s: Created %u (%u start + %u goal) states in %u cells (%u start + %u goal)",
178 getName().c_str(), tStart_.size + tGoal_.size, tStart_.size, tGoal_.size,
179 tStart_.grid.size() + tGoal_.grid.size(), tStart_.grid.size(), tGoal_.grid.size());
187 projectionEvaluator_->computeCoordinates(motion->
state, coord);
190 if (cell && !cell->
data.empty())
192 Motion *connectOther = cell->
data[rng_.uniformInt(0, cell->
data.size() - 1)];
194 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->
root : connectOther->
root, start ? connectOther->
root : motion->
root))
198 si_->copyState(connect->
state, connectOther->
state);
201 motion->
children.push_back(connect);
202 addMotion(tree, connect);
204 if (isPathValid(tree, connect) && isPathValid(otherTree, connectOther))
207 connectionPoint_ = std::make_pair(motion->
state, connectOther->
state);
209 connectionPoint_ = std::make_pair(connectOther->
state, motion->
state);
213 std::vector<Motion*> mpath1;
214 while (motion != NULL)
216 mpath1.push_back(motion);
220 std::vector<Motion*> mpath2;
221 while (connectOther != NULL)
223 mpath2.push_back(connectOther);
224 connectOther = connectOther->
parent;
230 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
231 solution.push_back(mpath1[i]);
232 solution.insert(solution.end(), mpath2.begin(), mpath2.end());
243 std::vector<Motion*> mpath;
246 while (motion != NULL)
248 mpath.push_back(motion);
253 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
254 if (!mpath[i]->valid)
256 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state))
257 mpath[i]->valid =
true;
260 removeMotion(tree, mpath[i]);
270 return cell && !cell->
data.empty() ? cell->
data[rng_.uniformInt(0, cell->
data.size() - 1)] : NULL;
278 projectionEvaluator_->computeCoordinates(motion->
state, coord);
282 for (
unsigned int i = 0 ; i < cell->
data.size(); ++i)
284 if (cell->
data[i] == motion)
286 cell->
data.erase(cell->
data.begin() + i);
291 if (cell->
data.empty())
294 tree.
grid.remove(cell);
295 tree.
grid.destroyCell(cell);
307 for (
unsigned int i = 0 ; i < motion->
parent->
children.size() ; ++i)
318 for (
unsigned int i = 0 ; i < motion->
children.size() ; ++i)
321 removeMotion(tree, motion->
children[i]);
325 si_->freeState(motion->
state);
332 projectionEvaluator_->computeCoordinates(motion->
state, coord);
336 cell->
data.push_back(motion);
341 cell = tree.
grid.createCell(coord);
342 cell->
data.push_back(motion);
357 tStart_.grid.clear();
364 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
369 Planner::getPlannerData(data);
371 std::vector<MotionInfo> motions;
372 tStart_.grid.getContent(motions);
374 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
375 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
376 if (motions[i][j]->parent == NULL)
383 tGoal_.grid.getContent(motions);
384 for (
unsigned int i = 0 ; i < motions.size() ; ++i)
385 for (
unsigned int j = 0 ; j < motions[i].size() ; ++j)
386 if (motions[i][j]->parent == NULL)
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Motion * parent
The parent motion – it contains the state this motion originates at.
Representation of a simple grid.
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.
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...
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...
_T data
The data we store in the cell.
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.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
Motion * selectMotion(TreeData &tree)
Select a motion from a tree.
void freeGridMotions(Grid< MotionInfo > &grid)
Free the memory used by the motions contained in a grid.
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
void remove(Element *elem)
Removes the data in the given Element from the PDF. After calling this function, the Element object s...
Invalid start state or no start state specified.
CellPDF pdf
The PDF used for selecting a cell from which to sample a motion.
Abstract definition of a goal region that can be sampled.
unsigned int size
The number of motions (in total) from the tree.
iterator begin(void) const
Return the begin() iterator for the grid.
The goal is of a type that a planner does not recognize.
double getRange(void) const
Get the range the planner is using.
#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...
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.
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
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.
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.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition of a cell in this grid.
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Element * add(const _T &d, const double w)
Adds a piece of data with a given weight to the PDF. Returns a corresponding Element, which can be used to subsequently update or remove the data from the PDF.
bool valid
Flag indicating whether this motion has been checked for validity.
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...
Definition of a geometric path.
iterator end(void) const
Return the end() iterator for the grid.
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
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...
double maxDistance_
The maximum length of a motion to be added in the tree.
A boost shared pointer wrapper for ompl::base::Path.
CoordHash::const_iterator iterator
We only allow const iterators.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.