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 <boost/thread/mutex.hpp>
00045 #include <vector>
00046
00047 namespace ompl
00048 {
00049
00050 namespace geometric
00051 {
00052
00088 class pSBL : public base::Planner
00089 {
00090 public:
00091
00092 pSBL(const base::SpaceInformationPtr &si) : base::Planner(si, "pSBL"),
00093 samplerArray_(si)
00094 {
00095 specs_.recognizedGoal = base::GOAL_STATE;
00096 specs_.multithreaded = true;
00097 maxDistance_ = 0.0;
00098 setThreadCount(2);
00099 }
00100
00101 virtual ~pSBL(void)
00102 {
00103 freeMemory();
00104 }
00105
00108 void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
00109 {
00110 projectionEvaluator_ = projectionEvaluator;
00111 }
00112
00115 void setProjectionEvaluator(const std::string &name)
00116 {
00117 projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
00118 }
00119
00121 const base::ProjectionEvaluatorPtr& getProjectionEvaluator(void) const
00122 {
00123 return projectionEvaluator_;
00124 }
00125
00131 void setRange(double distance)
00132 {
00133 maxDistance_ = distance;
00134 }
00135
00137 double getRange(void) const
00138 {
00139 return maxDistance_;
00140 }
00141
00143 void setThreadCount(unsigned int nthreads);
00144
00146 unsigned int getThreadCount(void) const
00147 {
00148 return threadCount_;
00149 }
00150
00151 virtual void setup(void);
00152
00153 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00154
00155 virtual void clear(void);
00156
00157 virtual void getPlannerData(base::PlannerData &data) const;
00158
00159 protected:
00160
00161 class Motion;
00162 typedef std::vector<Motion*> MotionSet;
00163
00164 class Motion
00165 {
00166 public:
00167
00168 Motion(void) : root(NULL), state(NULL), parent(NULL), valid(false)
00169 {
00170 }
00171
00172 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL), valid(false)
00173 {
00174 }
00175
00176 ~Motion(void)
00177 {
00178 }
00179
00180 const base::State *root;
00181 base::State *state;
00182 Motion *parent;
00183 bool valid;
00184 MotionSet children;
00185 boost::mutex lock;
00186 };
00187
00188 struct TreeData
00189 {
00190 TreeData(void) : grid(0), size(0)
00191 {
00192 }
00193
00194 Grid<MotionSet> grid;
00195 unsigned int size;
00196 boost::mutex lock;
00197 };
00198
00199 struct SolutionInfo
00200 {
00201 std::vector<Motion*> solution;
00202 bool found;
00203 boost::mutex lock;
00204 };
00205
00206 struct PendingRemoveMotion
00207 {
00208 TreeData *tree;
00209 Motion *motion;
00210 };
00211
00212 struct MotionsToBeRemoved
00213 {
00214 std::vector<PendingRemoveMotion> motions;
00215 boost::mutex lock;
00216 };
00217
00218 void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
00219
00220 void freeMemory(void)
00221 {
00222 freeGridMotions(tStart_.grid);
00223 freeGridMotions(tGoal_.grid);
00224 }
00225
00226 void freeGridMotions(Grid<MotionSet> &grid);
00227
00228 void addMotion(TreeData &tree, Motion *motion);
00229 Motion* selectMotion(RNG &rng, TreeData &tree);
00230 void removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen);
00231 bool isPathValid(TreeData &tree, Motion *motion);
00232 bool checkSolution(RNG &rng, bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
00233
00234
00235 base::StateSamplerArray<base::ValidStateSampler> samplerArray_;
00236 base::ProjectionEvaluatorPtr projectionEvaluator_;
00237
00238 TreeData tStart_;
00239 TreeData tGoal_;
00240
00241 MotionsToBeRemoved removeList_;
00242 boost::mutex loopLock_;
00243 boost::mutex loopLockCounter_;
00244 unsigned int loopCounter_;
00245
00246 double maxDistance_;
00247
00248 unsigned int threadCount_;
00249 };
00250
00251 }
00252 }
00253
00254 #endif