37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
74 virtual void clear(
void);
93 template<
template<
typename T>
class NN>
96 tStart_.reset(
new NN<Motion*>());
97 tGoal_.reset(
new NN<Motion*>());
100 virtual void setup(
void);
109 Motion(
void) : root(NULL), state(NULL), parent(NULL)
130 typedef boost::shared_ptr< NearestNeighbors<Motion*> >
TreeData;
157 return si_->distance(a->state, b->state);
RRTConnect(const base::SpaceInformationPtr &si)
Constructor.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
base::StateSamplerPtr sampler_
State sampler.
TreeData tGoal_
The goal tree.
void setRange(double distance)
Set the range the planner is supposed to use.
double getRange(void) const
Get the range the planner is using.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
A boost shared pointer wrapper for ompl::base::StateSampler.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
no progress has been made
boost::shared_ptr< NearestNeighbors< Motion * > > TreeData
A nearest-neighbor datastructure representing a tree of motions.
double maxDistance_
The maximum length of a motion to be added to a tree.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Representation of a motion.
Base class for a planner.
A class to store the exit status of Planner::solve()
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
GrowState
The state of the tree after an attempt to extend it.
RNG rng_
The random number generator.
Definition of an abstract state.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void freeMemory(void)
Free the memory allocated by this planner.
the randomly sampled state was reached
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...
Information attached to growing a tree of motions (used internally)
SpaceInformationPtr si_
The space information for which planning is done.
progress has been made towards the randomly sampled state
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
Grow a tree towards a random state.
TreeData tStart_
The start tree.