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_CONTROL_SIMPLE_SETUP_
38 #define OMPL_CONTROL_SIMPLE_SETUP_
39 
40 #include "ompl/base/Planner.h"
41 #include "ompl/control/SpaceInformation.h"
42 #include "ompl/control/PlannerData.h"
43 #include "ompl/base/ProblemDefinition.h"
44 #include "ompl/control/PathControl.h"
45 #include "ompl/geometric/PathGeometric.h"
46 #include "ompl/util/Console.h"
47 #include "ompl/util/Exception.h"
48 
49 namespace ompl
50 {
51 
52  namespace control
53  {
55  ClassForward(SimpleSetup);
57 
64  {
65  public:
66 
68  explicit
69  SimpleSetup(const ControlSpacePtr &space);
70 
71  virtual ~SimpleSetup(void)
72  {
73  }
74 
77  {
78  return si_;
79  }
80 
83  {
84  return pdef_;
85  }
86 
89  {
90  return si_->getStateSpace();
91  }
92 
94  const ControlSpacePtr& getControlSpace(void) const
95  {
96  return si_->getControlSpace();
97  }
98 
101  {
102  return si_->getStateValidityChecker();
103  }
104 
105  const StatePropagatorPtr& getStatePropagator(void) const
106  {
107  return si_->getStatePropagator();
108  }
109 
111  const base::GoalPtr& getGoal(void) const
112  {
113  return pdef_->getGoal();
114  }
115 
117  const base::PlannerPtr& getPlanner(void) const
118  {
119  return planner_;
120  }
121 
124  {
125  return pa_;
126  }
127 
129  bool haveExactSolutionPath(void) const;
130 
131 
133  bool haveSolutionPath(void) const
134  {
135  return pdef_->getSolutionPath();
136  }
137 
139  PathControl& getSolutionPath(void) const;
140 
142  void getPlannerData(base::PlannerData &pd) const;
143 
146  {
147  si_->setStateValidityChecker(svc);
148  }
149 
152  {
153  si_->setStateValidityChecker(svc);
154  }
155 
158  {
159  si_->setStatePropagator(sp);
160  }
161 
163  void setStatePropagator(const StatePropagatorPtr &sp)
164  {
165  si_->setStatePropagator(sp);
166  }
167 
169  void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
170  {
171  pdef_->setStartAndGoalStates(start, goal, threshold);
172  }
173 
175  void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
176  {
177  pdef_->setGoalState(goal, threshold);
178  }
179 
183  {
184  pdef_->addStartState(state);
185  }
186 
188  void clearStartStates(void)
189  {
190  pdef_->clearStartStates();
191  }
192 
195  {
197  addStartState(state);
198  }
199 
202  void setGoal(const base::GoalPtr &goal)
203  {
204  pdef_->setGoal(goal);
205  }
206 
211  void setPlanner(const base::PlannerPtr &planner)
212  {
213  if (planner && planner->getSpaceInformation().get() != si_.get())
214  throw Exception("Planner instance does not match space information");
215  planner_ = planner;
216  configured_ = false;
217  }
218 
223  {
224  pa_ = pa;
225  planner_.reset();
226  configured_ = false;
227  }
228 
230  virtual base::PlannerStatus solve(double time = 1.0);
231 
234 
236  bool invalidLastRequest(void) const
237  {
238  return invalid_request_;
239  }
240 
242  double getLastPlanComputationTime(void) const
243  {
244  return planTime_;
245  }
246 
250  virtual void clear(void);
251 
253  virtual void print(std::ostream &out = std::cout) const;
254 
258  virtual void setup(void);
259 
262  {
263  return params_;
264  }
265 
267  const base::ParamSet& params(void) const
268  {
269  return params_;
270  }
271 
272  protected:
273 
276 
279 
282 
285 
288 
290  double planTime_;
291 
294 
297  };
298 
301  }
302 
303 }
304 #endif