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_RRT_CONNECT_
00038 #define OMPL_GEOMETRIC_PLANNERS_RRT_RRT_CONNECT_
00039
00040 #include "ompl/geometric/planners/PlannerIncludes.h"
00041 #include "ompl/datastructures/NearestNeighbors.h"
00042
00043 namespace ompl
00044 {
00045
00046 namespace geometric
00047 {
00048
00063 class RRTConnect : public base::Planner
00064 {
00065 public:
00066
00068 RRTConnect(const base::SpaceInformationPtr &si);
00069
00070 virtual ~RRTConnect(void);
00071
00072 virtual void getPlannerData(base::PlannerData &data) const;
00073
00074 virtual bool solve(const base::PlannerTerminationCondition &ptc);
00075
00076 virtual void clear(void);
00077
00083 void setRange(double distance)
00084 {
00085 maxDistance_ = distance;
00086 }
00087
00089 double getRange(void) const
00090 {
00091 return maxDistance_;
00092 }
00093
00095 template<template<typename T> class NN>
00096 void setNearestNeighbors(void)
00097 {
00098 tStart_.reset(new NN<Motion*>());
00099 tGoal_.reset(new NN<Motion*>());
00100 }
00101
00102 virtual void setup(void);
00103
00104 protected:
00105
00107 class Motion
00108 {
00109 public:
00110
00111 Motion(void) : root(NULL), state(NULL), parent(NULL)
00112 {
00113 parent = NULL;
00114 state = NULL;
00115 }
00116
00117 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL)
00118 {
00119 }
00120
00121 ~Motion(void)
00122 {
00123 }
00124
00125 const base::State *root;
00126 base::State *state;
00127 Motion *parent;
00128
00129 };
00130
00132 typedef boost::shared_ptr< NearestNeighbors<Motion*> > TreeData;
00133
00135 struct TreeGrowingInfo
00136 {
00137 base::State *xstate;
00138 Motion *xmotion;
00139 };
00140
00142 enum GrowState
00143 {
00145 TRAPPED,
00147 ADVANCED,
00149 REACHED
00150 };
00151
00153 void freeMemory(void);
00154
00156 double distanceFunction(const Motion* a, const Motion* b) const
00157 {
00158 return si_->distance(a->state, b->state);
00159 }
00160
00162 GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion);
00163
00165 base::StateSamplerPtr sampler_;
00166
00168 TreeData tStart_;
00169
00171 TreeData tGoal_;
00172
00174 double maxDistance_;
00175
00177 RNG rng_;
00178 };
00179
00180 }
00181 }
00182
00183 #endif