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