All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SimpleSetup.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_SIMPLE_SETUP_
38 #define OMPL_GEOMETRIC_SIMPLE_SETUP_
39 
40 #include "ompl/base/Planner.h"
41 #include "ompl/base/PlannerData.h"
42 #include "ompl/base/SpaceInformation.h"
43 #include "ompl/base/ProblemDefinition.h"
44 #include "ompl/geometric/PathGeometric.h"
45 #include "ompl/geometric/PathSimplifier.h"
46 #include "ompl/util/Console.h"
47 #include "ompl/util/Exception.h"
48 
49 namespace ompl
50 {
51 
52  namespace geometric
53  {
54 
56  ClassForward(SimpleSetup);
58 
65  {
66  public:
67 
69  explicit
70  SimpleSetup(const base::StateSpacePtr &space);
71 
72  virtual ~SimpleSetup(void)
73  {
74  }
75 
78  {
79  return si_;
80  }
81 
84  {
85  return pdef_;
86  }
87 
90  {
91  return si_->getStateSpace();
92  }
93 
96  {
97  return si_->getStateValidityChecker();
98  }
99 
101  const base::GoalPtr& getGoal(void) const
102  {
103  return pdef_->getGoal();
104  }
105 
107  const base::PlannerPtr& getPlanner(void) const
108  {
109  return planner_;
110  }
111 
114  {
115  return pa_;
116  }
117 
120  {
121  return psk_;
122  }
123 
126  {
127  return psk_;
128  }
129 
131  bool haveExactSolutionPath(void) const;
132 
134  bool haveSolutionPath(void) const
135  {
136  return pdef_->getSolutionPath();
137  }
138 
140  PathGeometric& getSolutionPath(void) const;
141 
143  void getPlannerData(base::PlannerData &pd) const;
144 
147  {
148  si_->setStateValidityChecker(svc);
149  }
150 
153  {
154  si_->setStateValidityChecker(svc);
155  }
156 
159  const double threshold = std::numeric_limits<double>::epsilon())
160  {
161  pdef_->setStartAndGoalStates(start, goal, threshold);
162  }
163 
167  {
168  pdef_->addStartState(state);
169  }
170 
172  void clearStartStates(void)
173  {
174  pdef_->clearStartStates();
175  }
176 
179  {
181  addStartState(state);
182  }
183 
185  void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
186  {
187  pdef_->setGoalState(goal, threshold);
188  }
189 
192  void setGoal(const base::GoalPtr &goal)
193  {
194  pdef_->setGoal(goal);
195  }
196 
201  void setPlanner(const base::PlannerPtr &planner)
202  {
203  if (planner && planner->getSpaceInformation().get() != si_.get())
204  throw Exception("Planner instance does not match space information");
205  planner_ = planner;
206  configured_ = false;
207  }
208 
213  {
214  pa_ = pa;
215  planner_.reset();
216  configured_ = false;
217  }
218 
220  virtual base::PlannerStatus solve(double time = 1.0);
221 
224 
226  bool invalidLastRequest(void) const
227  {
228  return invalid_request_;
229  }
230 
232  double getLastPlanComputationTime(void) const
233  {
234  return planTime_;
235  }
236 
238  double getLastSimplificationTime(void) const
239  {
240  return simplifyTime_;
241  }
242 
245  void simplifySolution(double duration = 0.0);
246 
249 
253  virtual void clear(void);
254 
256  virtual void print(std::ostream &out = std::cout) const;
257 
261  virtual void setup(void);
262 
265  {
266  return params_;
267  }
268 
270  const base::ParamSet& params(void) const
271  {
272  return params_;
273  }
274 
275  protected:
276 
279 
282 
285 
288 
291 
294 
296  double planTime_;
297 
300 
303 
306  };
307 
310  }
311 
312 }
313 #endif