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 #ifndef OMPL_GEOMETRIC_PLANNERS_SBL_SBL_
00038 #define OMPL_GEOMETRIC_PLANNERS_SBL_SBL_
00039
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/base/ProjectionEvaluator.h"
00042 #include "ompl/datastructures/Grid.h"
00043 #include "ompl/datastructures/PDF.h"
00044 #include <vector>
00045
00046 namespace ompl
00047 {
00048
00049 namespace geometric
00050 {
00051
00087 class SBL : public base::Planner
00088 {
00089 public:
00090
00092 SBL(const base::SpaceInformationPtr &si);
00093
00094 virtual ~SBL(void);
00095
00098 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00099 {
00100 projectionEvaluator_ = projectionEvaluator;
00101 }
00102
00105 void setProjectionEvaluator(const std::string &name)
00106 {
00107 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00108 }
00109
00111 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00112 {
00113 return projectionEvaluator_;
00114 }
00115
00121 void setRange(double distance)
00122 {
00123 maxDistance_ = distance;
00124 }
00125
00127 double getRange(void) const
00128 {
00129 return maxDistance_;
00130 }
00131
00132 virtual void setup(void);
00133
00134 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00135
00136 virtual void clear(void);
00137
00138 virtual void getPlannerData(base::PlannerData &data) const;
00139
00140 protected:
00141
00142 struct MotionInfo;
00143
00145 typedef Grid<MotionInfo>::Cell GridCell;
00146
00148 typedef PDF<GridCell*> CellPDF;
00149
00151 class Motion
00152 {
00153 public:
00154
00156 Motion(void) : root(NULL), state(NULL), parent(NULL), valid(false)
00157 {
00158 }
00159
00161 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
00162 {
00163 }
00164
00166 const base::State *root;
00167
00169 base::State *state;
00170
00172 Motion *parent;
00173
00175 bool valid;
00176
00178 std::vector<Motion*> children;
00179 };
00180
00182 struct MotionInfo
00183 {
00184 Motion* operator[](unsigned int i)
00185 {
00186 return motions_[i];
00187 }
00188 std::vector<Motion*>::iterator begin (void)
00189 {
00190 return motions_.begin ();
00191 }
00192 void erase (std::vector<Motion*>::iterator iter)
00193 {
00194 motions_.erase (iter);
00195 }
00196 void push_back(Motion* m)
00197 {
00198 motions_.push_back(m);
00199 }
00200 unsigned int size(void) const
00201 {
00202 return motions_.size();
00203 }
00204 bool empty(void) const
00205 {
00206 return motions_.empty();
00207 }
00208
00209 std::vector<Motion*> motions_;
00210 CellPDF::Element* elem_;
00211 };
00212
00214 struct TreeData
00215 {
00216 TreeData(void) : grid(0), size(0)
00217 {
00218 }
00219
00221 Grid<MotionInfo> grid;
00222
00224 unsigned int size;
00225
00227 CellPDF pdf;
00228 };
00229
00231 void freeMemory(void)
00232 {
00233 freeGridMotions(tStart_.grid);
00234 freeGridMotions(tGoal_.grid);
00235 }
00236
00238 void freeGridMotions(Grid<MotionInfo> &grid);
00239
00241 void addMotion(TreeData &tree, Motion *motion);
00242
00244 Motion* selectMotion(TreeData &tree);
00245
00247 void removeMotion(TreeData &tree, Motion *motion);
00248
00254 bool isPathValid(TreeData &tree, Motion *motion);
00255
00257 bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
00258
00260 base::ValidStateSamplerPtr sampler_;
00261
00263 base::ProjectionEvaluatorPtr projectionEvaluator_;
00264
00266 TreeData tStart_;
00267
00269 TreeData tGoal_;
00270
00272 double maxDistance_;
00273
00275 RNG rng_;
00276 };
00277
00278 }
00279 }
00280
00281 #endif