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
00061 class RRTConnect : public base::Planner
00062 {
00063 public:
00064
00066 RRTConnect(const base::SpaceInformationPtr &si);
00067
00068 virtual ~RRTConnect(void);
00069
00070 virtual void getPlannerData(base::PlannerData &data) const;
00071
00072 virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc);
00073
00074 virtual void clear(void);
00075
00081 void setRange(double distance)
00082 {
00083 maxDistance_ = distance;
00084 }
00085
00087 double getRange(void) const
00088 {
00089 return maxDistance_;
00090 }
00091
00093 template<template<typename T> class NN>
00094 void setNearestNeighbors(void)
00095 {
00096 tStart_.reset(new NN<Motion*>());
00097 tGoal_.reset(new NN<Motion*>());
00098 }
00099
00100 virtual void setup(void);
00101
00102 protected:
00103
00105 class Motion
00106 {
00107 public:
00108
00109 Motion(void) : root(NULL), state(NULL), parent(NULL)
00110 {
00111 parent = NULL;
00112 state = NULL;
00113 }
00114
00115 Motion(const base::SpaceInformationPtr &si) : root(NULL), state(si->allocState()), parent(NULL)
00116 {
00117 }
00118
00119 ~Motion(void)
00120 {
00121 }
00122
00123 const base::State *root;
00124 base::State *state;
00125 Motion *parent;
00126
00127 };
00128
00130 typedef boost::shared_ptr< NearestNeighbors<Motion*> > TreeData;
00131
00133 struct TreeGrowingInfo
00134 {
00135 base::State *xstate;
00136 Motion *xmotion;
00137 bool start;
00138 };
00139
00141 enum GrowState
00142 {
00144 TRAPPED,
00146 ADVANCED,
00148 REACHED
00149 };
00150
00152 void freeMemory(void);
00153
00155 double distanceFunction(const Motion* a, const Motion* b) const
00156 {
00157 return si_->distance(a->state, b->state);
00158 }
00159
00161 GrowState growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion);
00162
00164 base::StateSamplerPtr sampler_;
00165
00167 TreeData tStart_;
00168
00170 TreeData tGoal_;
00171
00173 double maxDistance_;
00174
00176 RNG rng_;
00177
00179 std::pair<base::State*, base::State*> connectionPoint_;
00180 };
00181
00182 }
00183 }
00184
00185 #endif