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) : base::Planner(si, "pRRT"),
00074 samplerArray_(si)
00075 {
00076 specs_.approximateSolutions = true;
00077 specs_.multithreaded = true;
00078
00079 setThreadCount(2);
00080 goalBias_ = 0.05;
00081 maxDistance_ = 0.0;
00082 }
00083
00084 virtual ~pRRT(void)
00085 {
00086 freeMemory();
00087 }
00088
00089 virtual void getPlannerData(base::PlannerData &data) const;
00090
00091 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00092
00093 virtual void clear(void);
00094
00104 void setGoalBias(double goalBias)
00105 {
00106 goalBias_ = goalBias;
00107 }
00108
00110 double getGoalBias(void) const
00111 {
00112 return goalBias_;
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
00134 unsigned int getThreadCount(void) const
00135 {
00136 return threadCount_;
00137 }
00138
00140 template<template<typename T> class NN>
00141 void setNearestNeighbors(void)
00142 {
00143 nn_.reset(new NN<Motion*>());
00144 }
00145
00146 virtual void setup(void);
00147
00148 protected:
00149
00150 class Motion
00151 {
00152 public:
00153
00154 Motion(void) : state(NULL), parent(NULL)
00155 {
00156 }
00157
00158 Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
00159 {
00160 }
00161
00162 ~Motion(void)
00163 {
00164 }
00165
00166 base::State *state;
00167 Motion *parent;
00168
00169 };
00170
00171 struct SolutionInfo
00172 {
00173 Motion *solution;
00174 Motion *approxsol;
00175 double approxdif;
00176 boost::mutex lock;
00177 };
00178
00179 void threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol);
00180 void freeMemory(void);
00181
00182 double distanceFunction(const Motion* a, const Motion* b) const
00183 {
00184 return si_->distance(a->state, b->state);
00185 }
00186
00187 base::StateSamplerArray<base::StateSampler> samplerArray_;
00188 boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
00189 boost::mutex nnLock_;
00190
00191 unsigned int threadCount_;
00192
00193 double goalBias_;
00194 double maxDistance_;
00195 };
00196
00197 }
00198 }
00199
00200 #endif