SBL.cpp
81 ompl::base::PlannerStatus ompl::geometric::SBL::solve(const base::PlannerTerminationCondition &ptc)
84 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
116 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(tStart_.size + tGoal_.size));
184 bool ompl::geometric::SBL::checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution)
194 if (pdef_->getGoal()->isStartGoalPairValid(start ? motion->root : connectOther->root, start ? connectOther->root : motion->root))
270 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
393 data.addEdge(data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second));
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
Motion * parent
The parent motion – it contains the state this motion originates at.
Definition: SBL.h:170
The planner failed to find a solution.
Definition: PlannerStatus.h:62
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:208
const State * nextGoal(const PlannerTerminationCondition &ptc)
Return the next valid goal state or NULL if no more valid goal states are available. Because sampling of goal states may also produce invalid goals, this function takes an argument that specifies whether a termination condition has been reached. If the termination condition evaluates to true the function terminates even if no valid goal has been found.
Definition: Planner.cpp:271
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...
Definition: PlannerData.cpp:434
std::vector< Motion * > children
The set of motions descending from the current motion.
Definition: SBL.h:176
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
base::ProjectionEvaluatorPtr projectionEvaluator_
The employed projection evaluator.
Definition: SBL.h:261
bool isPathValid(TreeData &tree, Motion *motion)
Since solutions are computed in a lazy fashion, once trees are connected, the solution found needs to...
Definition: SBL.cpp:241
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
Definition: PathGeometric.cpp:432
void freeGridMotions(Grid< MotionInfo > &grid)
Free the memory used by the motions contained in a grid.
Definition: SBL.cpp:68
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Definition: PlannerData.h:60
void remove(Element *elem)
Removes the data in the given Element from the PDF. After calling this function, the Element object s...
Definition: PDF.h:177
Invalid start state or no start state specified.
Definition: PlannerStatus.h:56
Abstract definition of a goal region that can be sampled.
Definition: GoalSampleableRegion.h:49
The goal is of a type that a planner does not recognize.
Definition: PlannerStatus.h:60
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: SBL.cpp:57
The planner found an exact solution.
Definition: PlannerStatus.h:66
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...
Definition: PlannerData.cpp:328
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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...
Definition: PlannerData.cpp:444
const base::State * root
The root of the tree this motion would get to, if we were to follow parent pointers.
Definition: SBL.h:164
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void removeMotion(TreeData &tree, Motion *motion)
Remove a motion from a tree.
Definition: SBL.cpp:273
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:181
SBL(const base::SpaceInformationPtr &si)
The constructor needs the instance of the space information.
Definition: SBL.cpp:43
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: PlannerData.cpp:425
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
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.
Definition: SBL.cpp:184
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
void configureProjectionEvaluator(base::ProjectionEvaluatorPtr &proj)
If proj is undefined, it is set to the default projection reported by base::StateSpace::getDefaultPro...
Definition: SelfConfig.cpp:238
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Definition: SBL.h:276
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.
Definition: PDF.h:97
virtual bool couldSample() 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.
Definition: GoalSampleableRegion.h:78
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: SBL.cpp:349
void configurePlannerRange(double &range)
Compute what a good length for motion segments is.
Definition: SelfConfig.cpp:232
This class contains methods that automatically configure various parameters for motion planning...
Definition: SelfConfig.h:58
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: SBL.cpp:367
Representation of a search tree. Two instances will be used. One for start and one for goal...
Definition: SBL.h:212
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:75
_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...
Definition: PDF.h:132
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...
Definition: SBL.cpp:81
A boost shared pointer wrapper for ompl::base::Path.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55