All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
RRTstar.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Rice University
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 Rice University 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 /* Authors: Alejandro Perez, Sertac Karaman, Ioan Sucan */
36 
37 #ifndef OMPL_CONTRIB_RRT_STAR_RRTSTAR_
38 #define OMPL_CONTRIB_RRT_STAR_RRTSTAR_
39 
40 #include "ompl/geometric/planners/PlannerIncludes.h"
41 #include "ompl/datastructures/NearestNeighbors.h"
42 #include "ompl/base/spaces/RealVectorStateSpace.h"
43 #include <limits>
44 #include <vector>
45 
46 
47 namespace ompl
48 {
49 
50  namespace geometric
51  {
52 
74  class RRTstar : public base::Planner
75  {
76  public:
77 
79 
80  virtual ~RRTstar(void);
81 
82  virtual void getPlannerData(base::PlannerData &data) const;
83 
85 
86  virtual void clear(void);
87 
97  void setGoalBias(double goalBias)
98  {
99  goalBias_ = goalBias;
100  }
101 
103  double getGoalBias(void) const
104  {
105  return goalBias_;
106  }
107 
113  void setRange(double distance)
114  {
115  maxDistance_ = distance;
116  }
117 
119  double getRange(void) const
120  {
121  return maxDistance_;
122  }
123 
133  void setBallRadiusConstant(double ballRadiusConstant)
134  {
135  ballRadiusConst_ = ballRadiusConstant;
136  }
137 
141  double getBallRadiusConstant(void) const
142  {
143  return ballRadiusConst_;
144  }
145 
154  void setMaxBallRadius(double maxBallRadius)
155  {
156  ballRadiusMax_ = maxBallRadius;
157  }
158 
161  double getMaxBallRadius(void) const
162  {
163  return ballRadiusMax_;
164  }
165 
167  template<template<typename T> class NN>
169  {
170  nn_.reset(new NN<Motion*>());
171  }
172 
180  void setDelayCC(bool delayCC)
181  {
182  delayCC_ = delayCC;
183  }
184 
186  bool getDelayCC(void) const
187  {
188  return delayCC_;
189  }
190 
191  virtual void setup(void);
192 
193  protected:
194 
195 
197  class Motion
198  {
199  public:
200 
201  Motion(void) : state(NULL), parent(NULL), cost(0.0)
202  {
203  }
204 
206  Motion(const base::SpaceInformationPtr &si) : state(si->allocState()), parent(NULL), cost(0.0)
207  {
208  }
209 
210  ~Motion(void)
211  {
212  }
213 
216 
219 
221  double cost;
222 
224  std::vector<Motion*> children;
225  };
226 
228  void freeMemory(void);
229 
231  static bool compareMotion(const Motion* a, const Motion* b)
232  {
233  return (a->cost < b->cost);
234  }
235 
237  double distanceFunction(const Motion* a, const Motion* b) const
238  {
239  return si_->distance(a->state, b->state);
240  }
241 
243  void removeFromParent(Motion *m);
244 
246  void updateChildCosts(Motion *m, double delta);
247 
250 
252  boost::shared_ptr< NearestNeighbors<Motion*> > nn_;
253 
255  double goalBias_;
256 
258  double maxDistance_;
259 
262 
265 
268 
270  bool delayCC_;
271 
272  };
273 
274  }
275 }
276 
277 #endif