37 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
38 #define OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/OptimizationObjective.h"
42 #include "ompl/datastructures/NearestNeighbors.h"
43 #include "ompl/datastructures/DynamicSSSP.h"
71 class LBTRRT :
public base::Planner
75 LBTRRT(
const base::SpaceInformationPtr &si);
81 base::PlannerStatus
solve(
const base::PlannerTerminationCondition &ptc)
override;
83 void clear()
override;
122 template <
template <
typename T>
class NN>
125 if (
nn_ &&
nn_->size() != 0)
126 OMPL_WARN(
"Calling setNearestNeighbors will clear all states.");
128 nn_ = std::make_shared<NN<Motion *>>();
132 void setup()
override;
148 std::string getIterationCount()
const
152 std::string getBestCost()
const
168 Motion(
const base::SpaceInformationPtr &si)
169 :
state_(si->allocState())
176 base::State *
state_{
nullptr};
200 bool operator()(
const Motion *motionA,
const Motion *motionB)
202 double costA = motionA->costLb_;
203 double costB = motionB->costLb_;
208 return costA + distA < costB + distB;
217 IsLessThanLB(
LBTRRT *plannerPtr) : plannerPtr_(plannerPtr)
221 bool operator()(
const Motion *motionA,
const Motion *motionB)
const
223 return motionA->costLb_ < motionB->costLb_;
228 typedef std::set<Motion *, IsLessThanLB> Lb_queue;
229 typedef Lb_queue::iterator Lb_queue_iter;
232 void considerEdge(Motion *parent, Motion *child,
double c);
249 return si_->distance(a->state_, b->state_);
260 return si_->checkMotion(a, b);
273 std::shared_ptr<NearestNeighbors<Motion *>>
nn_;
307 #endif // OMPL_GEOMETRIC_PLANNERS_RRT_LBT_RRT_
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double lazilyUpdateApxParent(Motion *child, Motion *parent)
lazily update the parent in the approximation tree without updating costs to cildren
double getGoalBias() const
Get the goal bias the planner is using.
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
void setRange(double distance)
Set the range the planner is supposed to use.
Motion * parentApx_
The parent motion in the approximation tree.
Representation of a motion.
RNG rng_
The random number generator.
Definition of an abstract state.
std::shared_ptr< NearestNeighbors< Motion * > > nn_
A nearest-neighbors datastructure containing the tree of motions.
Motion * getMotion(std::size_t i)
get motion from id
double getRange() const
Get the range the planner is using.
std::vector< Motion * > idToMotionMap_
mapping between a motion id and the motion
std::size_t id_
unique id of the motion
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)
double bestCost_
Best cost found so far by algorithm.
double getApproximationFactor() const
Get the apprimation factor.
void freeMemory()
Free the memory allocated by this planner.
void setGoalBias(double goalBias)
Set the goal bias.
comparator - metric is the cost to reach state via a specific state
double epsilon_
approximation factor
unsigned int iterations_
Number of iterations the algorithm performed.
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
void updateChildCostsApx(Motion *m, double delta)
update the child cost of the approximation tree
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double maxDistance_
The maximum length of a motion to be added to a tree.
bool checkMotion(const Motion *a, const Motion *b)
local planner
void removeFromParentApx(Motion *m)
remove motion from its parent in the approximation tree
Lower Bound Tree Rapidly-exploring Random Trees.
double distanceFunction(const Motion *a, const Motion *b) const
Compute distance between motions (actually distance between contained states)
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
SpaceInformationPtr si_
The space information for which planning is done.
void setNearestNeighbors()
Set a different nearest neighbors datastructure.
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...
DynamicSSSP lowerBoundGraph_
A graph of motions Glb.
double costLb_
The lower bound cost of the motion while it is stored in the lowerBoundGraph_ and this may seem redun...
base::State * state_
The state contained by the motion.
std::vector< Motion * > childrenApx_
The children in the approximation tree.
double costApx_
The approximation cost.
base::StateSamplerPtr sampler_
State sampler.
void setApproximationFactor(double epsilon)
Set the apprimation factor.
LBTRRT(const base::SpaceInformationPtr &si)
Constructor.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Main namespace. Contains everything in this library.
void considerEdge(Motion *parent, Motion *child, double c)
consider an edge for addition to the roadmap