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
00069 class pRRT : public base::Planner
00070 {
00071 public:
00072
00073 pRRT(const base::SpaceInformationPtr &si);
00074
00075 virtual ~pRRT(void);
00076
00077 virtual void getPlannerData(base::PlannerData &data) const;
00078
00079 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00080
00081 virtual void clear(void);
00082
00092 void setGoalBias(double goalBias)
00093 {
00094 goalBias_ = goalBias;
00095 }
00096
00098 double getGoalBias(void) const
00099 {
00100 return goalBias_;
00101 }
00102
00108 void setRange(double distance)
00109 {
00110 maxDistance_ = distance;
00111 }
00112
00114 double getRange(void) const
00115 {
00116 return maxDistance_;
00117 }
00118
00120 void setThreadCount(unsigned int nthreads);
00121
00122 unsigned int getThreadCount(void) const
00123 {
00124 return threadCount_;
00125 }
00126
00128 template<template<typename T> class NN>
00129 void setNearestNeighbors(void)
00130 {
00131 nn_.reset(new NN<Motion*>());
00132 }
00133
00134 virtual void setup(void);
00135
00136 protected:
00137
00138 class Motion
00139 {
00140 public:
00141
00142 Motion(void) : state(NULL), parent(NULL)
00143 {
00144 }
00145
00146 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
00147 {
00148 }
00149
00150 ~Motion(void)
00151 {
00152 }
00153
00154 base::State *state;
00155 Motion *parent;
00156
00157 };
00158
00159 struct SolutionInfo
00160 {
00161 Motion *solution;
00162 Motion *approxsol;
00163 double approxdif;
00164 boost::mutex lock;
00165 };
00166
00167 void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
00168 void freeMemory(void);
00169
00170 double distanceFunction(const Motion* a, const Motion* b) const
00171 {
00172 return si_->distance(a->state, b->state);
00173 }
00174
00175 base::StateSamplerArray<base::StateSampler> samplerArray_;
00176 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00177 boost::mutex nnLock_;
00178
00179 unsigned int threadCount_;
00180
00181 double goalBias_;
00182 double maxDistance_;
00183 };
00184
00185 }
00186 }
00187
00188 #endif