00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef OMPL_GEOMETRIC_PLANNERS_RRT_LAZY_RRT_
00039 #define OMPL_GEOMETRIC_PLANNERS_RRT_LAZY_RRT_
00040
00041 #include "ompl/geometric/planners/PlannerIncludes.h"
00042 #include "ompl/datastructures/NearestNeighbors.h"
00043 #include <vector>
00044
00045 namespace ompl
00046 {
00047
00048 namespace geometric
00049 {
00050
00081 class LazyRRT : public base::Planner
00082 {
00083 public:
00084
00086 LazyRRT(const base::SpaceInformationPtr &si);
00087
00088 virtual ~LazyRRT(void);
00089
00090 virtual void getPlannerData(base::PlannerData &data) const;
00091
00092 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00093
00094 virtual void clear(void);
00095
00105 void setGoalBias(double goalBias)
00106 {
00107 goalBias_ = goalBias;
00108 }
00109
00111 double getGoalBias(void) const
00112 {
00113 return goalBias_;
00114 }
00115
00121 void setRange(double distance)
00122 {
00123 maxDistance_ = distance;
00124 }
00125
00127 double getRange(void) const
00128 {
00129 return maxDistance_;
00130 }
00131
00133 template<template<typename T> class NN>
00134 void setNearestNeighbors(void)
00135 {
00136 nn_.reset(new NN<Motion*>());
00137 }
00138
00139 virtual void setup(void);
00140
00141 protected:
00142
00144 class Motion
00145 {
00146 public:
00147
00148 Motion(void) : state(NULL), parent(NULL), valid(false)
00149 {
00150 }
00151
00153 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL), valid(false)
00154 {
00155 }
00156
00157 ~Motion(void)
00158 {
00159 }
00160
00162 base::State *state;
00163
00165 Motion *parent;
00166
00168 bool valid;
00169
00171 std::vector<Motion*> children;
00172 };
00173
00175 void freeMemory(void);
00176
00178 void removeMotion(Motion *motion);
00179
00181 double distanceFunction(const Motion* a, const Motion* b) const
00182 {
00183 return si_->distance(a->state, b->state);
00184 }
00185
00187 base::StateSamplerPtr sampler_;
00188
00190 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00191
00193 double goalBias_;
00194
00196 double maxDistance_;
00197
00199 RNG rng_;
00200
00201 };
00202
00203 }
00204 }
00205
00206 #endif