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_pSBL_
00038 #define OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
00039
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/base/ProjectionEvaluator.h"
00042 #include "ompl/base/StateSamplerArray.h"
00043 #include "ompl/datastructures/Grid.h"
00044 #include "ompl/datastructures/PDF.h"
00045 #include <boost/thread/mutex.hpp>
00046 #include <vector>
00047
00048 namespace ompl
00049 {
00050
00051 namespace geometric
00052 {
00053
00089 class pSBL : public base::Planner
00090 {
00091 public:
00092
00093 pSBL(const base::SpaceInformationPtr &si);
00094
00095 virtual ~pSBL(void);
00096
00099 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00100 {
00101 projectionEvaluator_ = projectionEvaluator;
00102 }
00103
00106 void setProjectionEvaluator(const std::string &name)
00107 {
00108 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00109 }
00110
00112 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00113 {
00114 return projectionEvaluator_;
00115 }
00116
00122 void setRange(double distance)
00123 {
00124 maxDistance_ = distance;
00125 }
00126
00128 double getRange(void) const
00129 {
00130 return maxDistance_;
00131 }
00132
00134 void setThreadCount(unsigned int nthreads);
00135
00137 unsigned int getThreadCount(void) const
00138 {
00139 return threadCount_;
00140 }
00141
00142 virtual void setup(void);
00143
00144 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00145
00146 virtual void clear(void);
00147
00148 virtual void getPlannerData(base::PlannerData &data) const;
00149
00150 protected:
00151
00152 class Motion;
00153 struct MotionInfo;
00154
00156 typedef Grid<MotionInfo>::Cell GridCell;
00157
00159 typedef PDF<GridCell*> CellPDF;
00160
00161 class Motion
00162 {
00163 public:
00164
00165 Motion(void) : root(NULL), state(NULL), parent(NULL), valid(false)
00166 {
00167 }
00168
00169 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
00170 {
00171 }
00172
00173 ~Motion(void)
00174 {
00175 }
00176
00177 const base::State *root;
00178 base::State *state;
00179 Motion *parent;
00180 bool valid;
00181 std::vector<Motion*> children;
00182 boost::mutex lock;
00183 };
00184
00186 struct MotionInfo
00187 {
00188 Motion* operator[](unsigned int i)
00189 {
00190 return motions_[i];
00191 }
00192 std::vector<Motion*>::iterator begin (void)
00193 {
00194 return motions_.begin ();
00195 }
00196 void erase (std::vector<Motion*>::iterator iter)
00197 {
00198 motions_.erase (iter);
00199 }
00200 void push_back(Motion* m)
00201 {
00202 motions_.push_back(m);
00203 }
00204 unsigned int size(void) const
00205 {
00206 return motions_.size();
00207 }
00208 bool empty(void) const
00209 {
00210 return motions_.empty();
00211 }
00212 std::vector<Motion*> motions_;
00213 CellPDF::Element* elem_;
00214 };
00215
00216 struct TreeData
00217 {
00218 TreeData(void) : grid(0), size(0)
00219 {
00220 }
00221
00222 Grid<MotionInfo> grid;
00223 unsigned int size;
00224 CellPDF pdf;
00225 boost::mutex lock;
00226 };
00227
00228 struct SolutionInfo
00229 {
00230 std::vector<Motion*> solution;
00231 bool found;
00232 boost::mutex lock;
00233 };
00234
00235 struct PendingRemoveMotion
00236 {
00237 TreeData *tree;
00238 Motion *motion;
00239 };
00240
00241 struct MotionsToBeRemoved
00242 {
00243 std::vector<PendingRemoveMotion> motions;
00244 boost::mutex lock;
00245 };
00246
00247 void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
00248
00249 void freeMemory(void)
00250 {
00251 freeGridMotions(tStart_.grid);
00252 freeGridMotions(tGoal_.grid);
00253 }
00254
00255 void freeGridMotions(Grid<MotionInfo> &grid);
00256
00257 void addMotion(TreeData &tree, Motion *motion);
00258 Motion* selectMotion(RNG &rng, TreeData &tree);
00259 void removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen);
00260 bool isPathValid(TreeData &tree, Motion *motion);
00261 bool checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
00262
00263
00264 base::StateSamplerArray<base::ValidStateSampler> samplerArray_;
00265 base::ProjectionEvaluatorPtr projectionEvaluator_;
00266
00267 TreeData tStart_;
00268 TreeData tGoal_;
00269
00270 MotionsToBeRemoved removeList_;
00271 boost::mutex loopLock_;
00272 boost::mutex loopLockCounter_;
00273 unsigned int loopCounter_;
00274
00275 double maxDistance_;
00276
00277 unsigned int threadCount_;
00278 };
00279
00280 }
00281 }
00282
00283 #endif