37 #ifndef OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
38 #define OMPL_GEOMETRIC_PLANNERS_SBL_pSBL_
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/base/ProjectionEvaluator.h"
42 #include "ompl/base/StateSamplerArray.h"
43 #include "ompl/datastructures/Grid.h"
44 #include "ompl/datastructures/PDF.h"
45 #include <boost/thread/mutex.hpp>
99 projectionEvaluator_ = projectionEvaluator;
106 projectionEvaluator_ =
si_->getStateSpace()->getProjection(name);
112 return projectionEvaluator_;
122 maxDistance_ = distance;
140 virtual void setup(
void);
144 virtual void clear(
void);
163 Motion(
void) : root(NULL), state(NULL), parent(NULL), valid(
false)
179 std::vector<Motion*> children;
186 Motion* operator[](
unsigned int i)
190 std::vector<Motion*>::iterator begin (
void)
192 return motions_.begin ();
194 void erase (std::vector<Motion*>::iterator iter)
196 motions_.erase (iter);
200 motions_.push_back(m);
202 unsigned int size(
void)
const
204 return motions_.size();
206 bool empty(
void)
const
208 return motions_.empty();
210 std::vector<Motion*> motions_;
211 CellPDF::Element* elem_;
228 std::vector<Motion*> solution;
241 std::vector<PendingRemoveMotion> motions;
247 void freeMemory(
void)
249 freeGridMotions(tStart_.grid);
250 freeGridMotions(tGoal_.grid);
255 void addMotion(TreeData &tree, Motion *motion);
256 Motion* selectMotion(
RNG &rng, TreeData &tree);
257 void removeMotion(TreeData &tree, Motion *motion, std::map<Motion*, bool> &seen);
258 bool isPathValid(TreeData &tree, Motion *motion);
259 bool checkSolution(
RNG &rng,
bool start, TreeData &tree, TreeData &otherTree, Motion *motion, std::vector<Motion*> &solution);
268 MotionsToBeRemoved removeList_;
269 boost::mutex loopLock_;
270 boost::mutex loopLockCounter_;
271 unsigned int loopCounter_;
275 unsigned int threadCount_;