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_RRT_pRRT_
00038 #define OMPL_GEOMETRIC_PLANNERS_RRT_pRRT_
00039
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/base/StateSamplerArray.h"
00042 #include "ompl/datastructures/NearestNeighbors.h"
00043 #include <boost/thread/mutex.hpp>
00044
00045 namespace ompl
00046 {
00047
00048 namespace geometric
00049 {
00050
00067 class pRRT : public base::Planner
00068 {
00069 public:
00070
00071 pRRT(const base::SpaceInformationPtr &si);
00072
00073 virtual ~pRRT(void);
00074
00075 virtual void getPlannerData(base::PlannerData &data) const;
00076
00077 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00078
00079 virtual void clear(void);
00080
00090 void setGoalBias(double goalBias)
00091 {
00092 goalBias_ = goalBias;
00093 }
00094
00096 double getGoalBias(void) const
00097 {
00098 return goalBias_;
00099 }
00100
00106 void setRange(double distance)
00107 {
00108 maxDistance_ = distance;
00109 }
00110
00112 double getRange(void) const
00113 {
00114 return maxDistance_;
00115 }
00116
00118 void setThreadCount(unsigned int nthreads);
00119
00120 unsigned int getThreadCount(void) const
00121 {
00122 return threadCount_;
00123 }
00124
00126 template<template<typename T> class NN>
00127 void setNearestNeighbors(void)
00128 {
00129 nn_.reset(new NN<Motion*>());
00130 }
00131
00132 virtual void setup(void);
00133
00134 protected:
00135
00136 class Motion
00137 {
00138 public:
00139
00140 Motion(void) : state(NULL), parent(NULL)
00141 {
00142 }
00143
00144 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
00145 {
00146 }
00147
00148 ~Motion(void)
00149 {
00150 }
00151
00152 base::State *state;
00153 Motion *parent;
00154
00155 };
00156
00157 struct SolutionInfo
00158 {
00159 Motion *solution;
00160 Motion *approxsol;
00161 double approxdif;
00162 boost::mutex lock;
00163 };
00164
00165 void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
00166 void freeMemory(void);
00167
00168 double distanceFunction(const Motion* a, const Motion* b) const
00169 {
00170 return si_->distance(a->state, b->state);
00171 }
00172
00173 base::StateSamplerArray<base::StateSampler> samplerArray_;
00174 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00175 boost::mutex nnLock_;
00176
00177 unsigned int threadCount_;
00178
00179 double goalBias_;
00180 double maxDistance_;
00181
00183 Motion *lastGoalMotion_;
00184 };
00185
00186 }
00187 }
00188
00189 #endif