37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_RRT_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
65 class RRT :
public base::Planner
69 RRT(
const base::SpaceInformationPtr &si);
75 base::PlannerStatus
solve(
const base::PlannerTerminationCondition &ptc)
override;
77 void clear()
override;
116 template <
template <
typename T>
class NN>
119 if (
nn_ &&
nn_->size() != 0)
120 OMPL_WARN(
"Calling setNearestNeighbors will clear all states.");
122 nn_ = std::make_shared<NN<Motion *>>();
126 void setup()
override;
139 Motion(
const base::SpaceInformationPtr &si) :
state(si->allocState())
146 base::State *
state{
nullptr};
158 return si_->distance(a->state, b->state);
165 std::shared_ptr<NearestNeighbors<Motion *>>
nn_;
double getRange() const
Get the range the planner is using.
void freeMemory()
Free the memory allocated by this planner.
Motion * parent
The parent motion in the exploration tree.
base::State * state
The state contained by the motion.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
base::StateSamplerPtr sampler_
State sampler.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
SpaceInformationPtr si_
The space information for which planning is done.
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
void setGoalBias(double goalBias)
Set the goal bias.
void setRange(double distance)
Set the range the planner is supposed to use.
double maxDistance_
The maximum length of a motion to be added to a tree.
double getGoalBias() const
Get the goal bias the planner is using.
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
RRT(const base::SpaceInformationPtr &si)
Constructor.
RNG rng_
The random number generator.
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
Main namespace. Contains everything in this library.