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