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 <vector>
00044
00045 namespace ompl
00046 {
00047
00048 namespace geometric
00049 {
00050
00086 class SBL : public base::Planner
00087 {
00088 public:
00089
00091 SBL(const base::SpaceInformationPtr &si) : base::Planner(si, "SBL")
00092 {
00093 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00094 maxDistance_ = 0.0;
00095 }
00096
00097 virtual ~SBL(void)
00098 {
00099 freeMemory();
00100 }
00101
00104 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00105 {
00106 projectionEvaluator_ = projectionEvaluator;
00107 }
00108
00111 void setProjectionEvaluator(const std::string &name)
00112 {
00113 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00114 }
00115
00117 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00118 {
00119 return projectionEvaluator_;
00120 }
00121
00127 void setRange(double distance)
00128 {
00129 maxDistance_ = distance;
00130 }
00131
00133 double getRange(void) const
00134 {
00135 return maxDistance_;
00136 }
00137
00138 virtual void setup(void);
00139
00140 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00141
00142 virtual void clear(void);
00143
00144 virtual void getPlannerData(base::PlannerData &data) const;
00145
00146 protected:
00147
00148 class Motion;
00149
00151 typedef std::vector<Motion*> MotionSet;
00152
00154 class Motion
00155 {
00156 public:
00157
00159 Motion(void) : root(NULL), state(NULL), parent(NULL), valid(false)
00160 {
00161 }
00162
00164 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
00165 {
00166 }
00167
00168 ~Motion(void)
00169 {
00170 }
00171
00173 const base::State *root;
00174
00176 base::State *state;
00177
00179 Motion *parent;
00180
00182 bool valid;
00183
00185 MotionSet children;
00186 };
00187
00189 struct TreeData
00190 {
00191 TreeData(void) : grid(0), size(0)
00192 {
00193 }
00194
00196 Grid<MotionSet> grid;
00197
00199 unsigned int size;
00200 };
00201
00203 void freeMemory(void)
00204 {
00205 freeGridMotions(tStart_.grid);
00206 freeGridMotions(tGoal_.grid);
00207 }
00208
00210 void freeGridMotions(Grid<MotionSet> &grid);
00211
00213 void addMotion(TreeData &tree, Motion *motion);
00214
00216 Motion* selectMotion(TreeData &tree);
00217
00219 void removeMotion(TreeData &tree, Motion *motion);
00220
00226 bool isPathValid(TreeData &tree, Motion *motion);
00227
00229 bool checkSolution(bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
00230
00232 base::ValidStateSamplerPtr sampler_;
00233
00235 base::ProjectionEvaluatorPtr projectionEvaluator_;
00236
00238 TreeData tStart_;
00239
00241 TreeData tGoal_;
00242
00244 double maxDistance_;
00245
00247 RNG rng_;
00248 };
00249
00250 }
00251 }
00252
00253 #endif