37 #ifndef OMPL_CONTRIB_RRT_STAR_RRTSTAR_
38 #define OMPL_CONTRIB_RRT_STAR_RRTSTAR_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/OptimizationObjective.h"
42 #include "ompl/datastructures/NearestNeighbors.h"
87 virtual void clear(
void);
126 template<
template<
typename T>
class NN>
129 nn_.reset(
new NN<Motion*>());
150 virtual void setup(
void);
154 std::string getIterationCount(
void)
const;
156 std::string getCollisionCheckCount(
void)
const;
158 std::string getBestCost(
void)
const;
169 state(si->allocState()),
202 costs_(costs), opt_(opt)
204 bool operator()(
unsigned i,
unsigned j)
208 const std::vector<base::Cost>& costs_;
212 enum DistanceDirection { FROM_NEIGHBORS, TO_NEIGHBORS };
236 boost::shared_ptr< NearestNeighbors<Motion*> >
nn_;
bool getDelayCC(void) const
Get the state of the delayed collision checking option.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Optimal Rapidly-exploring Random Trees.
void setDelayCC(bool delayCC)
Option that delays collision checking procedures. When it is enabled, all neighbors are sorted by cos...
void setNearestNeighbors(void)
Set a different nearest neighbors datastructure.
void updateChildCosts(Motion *m)
Updates the cost of the children of this node if the cost up to this node has changed.
base::OptimizationObjectivePtr opt_
Objective we're optimizing.
A boost shared pointer wrapper for ompl::base::StateSampler.
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
base::Cost cost
The cost up to this motion.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setRange(double distance)
Set the range the planner is supposed to use.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
boost::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
double getGoalBias(void) const
Get the goal bias the planner is using.
base::Cost bestCost_
Best cost found so far by algorithm.
double maxDistance_
The maximum length of a motion to be added to a tree.
Representation of a motion.
base::State * state
The state contained by the motion.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
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.
Base class for a planner.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
virtual bool isCostBetterThan(Cost c1, Cost c2) const
Check whether the the cost c1 is considered better than the cost c2. By default, this returns true on...
A class to store the exit status of Planner::solve()
Definition of an abstract state.
std::vector< Motion * > children
The set of motions descending from the current motion.
Abstract definition of optimization objectives.
base::StateSamplerPtr sampler_
State sampler.
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
RNG rng_
The random number generator.
unsigned int collisionChecks_
Number of collisions checks performed by the algorithm.
void freeMemory(void)
Free the memory allocated by this planner.
Motion * parent
The parent motion in the exploration tree.
bool delayCC_
Option to delay and reduce collision checking within iterations.
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...
void removeFromParent(Motion *m)
Removes the given motion from the parent's child list.
void setGoalBias(double goalBias)
Set the goal bias.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
SpaceInformationPtr si_
The space information for which planning is done.
Motion(const base::SpaceInformationPtr &si)
Constructor that allocates memory for the state. This constructor automatically allocates memory for ...
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::vector< Motion * > goalMotions_
A list of states in the tree that satisfy the goal condition.
base::Cost incCost
The incremental cost of this motion's parent to this motion (this is stored to save distance computat...
unsigned int iterations_
Number of iterations the algorithm performed.
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
DistanceDirection distanceDirection_
Directionality of distance computation for nearest neighbors. Either from neighbors to new state...