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
00085 class SBL : public base::Planner
00086 {
00087 public:
00088
00090 SBL(const base::SpaceInformationPtr &si);
00091
00092 virtual ~SBL(void);
00093
00096 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00097 {
00098 projectionEvaluator_ = projectionEvaluator;
00099 }
00100
00103 void setProjectionEvaluator(const std::string &name)
00104 {
00105 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00106 }
00107
00109 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00110 {
00111 return projectionEvaluator_;
00112 }
00113
00119 void setRange(double distance)
00120 {
00121 maxDistance_ = distance;
00122 }
00123
00125 double getRange(void) const
00126 {
00127 return maxDistance_;
00128 }
00129
00130 virtual void setup(void);
00131
00132 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00133
00134 virtual void clear(void);
00135
00136 virtual void getPlannerData(base::PlannerData &data) const;
00137
00138 protected:
00139
00140 struct MotionInfo;
00141
00143 typedef Grid<MotionInfo>::Cell GridCell;
00144
00146 typedef PDF<GridCell*> CellPDF;
00147
00149 class Motion
00150 {
00151 public:
00152
00154 Motion(void) : root(NULL), state(NULL), parent(NULL), valid(false)
00155 {
00156 }
00157
00159 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
00160 {
00161 }
00162
00164 const base::State *root;
00165
00167 base::State *state;
00168
00170 Motion *parent;
00171
00173 bool valid;
00174
00176 std::vector<Motion*> children;
00177 };
00178
00180 struct MotionInfo
00181 {
00182 Motion* operator[](unsigned int i)
00183 {
00184 return motions_[i];
00185 }
00186 std::vector<Motion*>::iterator begin (void)
00187 {
00188 return motions_.begin ();
00189 }
00190 void erase (std::vector<Motion*>::iterator iter)
00191 {
00192 motions_.erase (iter);
00193 }
00194 void push_back(Motion* m)
00195 {
00196 motions_.push_back(m);
00197 }
00198 unsigned int size(void) const
00199 {
00200 return motions_.size();
00201 }
00202 bool empty(void) const
00203 {
00204 return motions_.empty();
00205 }
00206
00207 std::vector<Motion*> motions_;
00208 CellPDF::Element* elem_;
00209 };
00210
00212 struct TreeData
00213 {
00214 TreeData(void) : grid(0), size(0)
00215 {
00216 }
00217
00219 Grid<MotionInfo> grid;
00220
00222 unsigned int size;
00223
00225 CellPDF pdf;
00226 };
00227
00229 void freeMemory(void)
00230 {
00231 freeGridMotions(tStart_.grid);
00232 freeGridMotions(tGoal_.grid);
00233 }
00234
00236 void freeGridMotions(Grid<MotionInfo> &grid);
00237
00239 void addMotion(TreeData &tree, Motion *motion);
00240
00242 Motion* selectMotion(TreeData &tree);
00243
00245 void removeMotion(TreeData &tree, Motion *motion);
00246
00252 bool isPathValid(TreeData &tree, Motion *motion);
00253
00255 bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
00256
00258 base::ValidStateSamplerPtr sampler_;
00259
00261 base::ProjectionEvaluatorPtr projectionEvaluator_;
00262
00264 TreeData tStart_;
00265
00267 TreeData tGoal_;
00268
00270 double maxDistance_;
00271
00273 RNG rng_;
00274
00276 std::pair<base::State*, base::State*> connectionPoint_;
00277 };
00278
00279 }
00280 }
00281
00282 #endif