All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
EST.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PLANNERS_EST_EST_
38 #define OMPL_GEOMETRIC_PLANNERS_EST_EST_
39 
40 #include "ompl/datastructures/Grid.h"
41 #include "ompl/geometric/planners/PlannerIncludes.h"
42 #include "ompl/base/ProjectionEvaluator.h"
43 #include "ompl/datastructures/PDF.h"
44 #include <vector>
45 
46 namespace ompl
47 {
48 
49  namespace geometric
50  {
51 
73  class EST : public base::Planner
74  {
75  public:
76 
78  EST(const base::SpaceInformationPtr &si);
79 
80  virtual ~EST(void);
81 
83 
84  virtual void clear(void);
85 
93  void setGoalBias(double goalBias)
94  {
95  goalBias_ = goalBias;
96  }
97 
99  double getGoalBias(void) const
100  {
101  return goalBias_;
102  }
103 
109  void setRange(double distance)
110  {
111  maxDistance_ = distance;
112  }
113 
115  double getRange(void) const
116  {
117  return maxDistance_;
118  }
119 
122  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
123  {
124  projectionEvaluator_ = projectionEvaluator;
125  }
126 
129  void setProjectionEvaluator(const std::string &name)
130  {
131  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
132  }
133 
136  {
137  return projectionEvaluator_;
138  }
139 
140  virtual void setup(void);
141 
142  virtual void getPlannerData(base::PlannerData &data) const;
143 
144  protected:
145 
147  class Motion
148  {
149  public:
150 
151  Motion(void) : state(NULL), parent(NULL)
152  {
153  }
154 
156  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL)
157  {
158  }
159 
160  ~Motion(void)
161  {
162  }
163 
166 
169  };
170 
171  struct MotionInfo;
172 
175 
178 
180  struct MotionInfo
181  {
182  Motion* operator[](unsigned int i)
183  {
184  return motions_[i];
185  }
186  const Motion* operator[](unsigned int i) const
187  {
188  return motions_[i];
189  }
190  void push_back(Motion* m)
191  {
192  motions_.push_back(m);
193  }
194  unsigned int size(void) const
195  {
196  return motions_.size();
197  }
198  bool empty(void) const
199  {
200  return motions_.empty();
201  }
202  std::vector<Motion*> motions_;
203  CellPDF::Element* elem_;
204  };
205 
206 
208  struct TreeData
209  {
210  TreeData(void) : grid(0), size(0)
211  {
212  }
213 
216 
218  unsigned int size;
219  };
220 
222  void freeMemory(void);
223 
225  void addMotion(Motion *motion);
226 
228  Motion* selectMotion(void);
229 
232 
235 
238 
240  double goalBias_;
241 
243  double maxDistance_;
244 
247 
250 
253  };
254 
255  }
256 }
257 
258 #endif