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  OMPL_CLASS_FORWARD(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 
106  const StatePropagatorPtr& getStatePropagator(void) const
107  {
108  return si_->getStatePropagator();
109  }
110 
112  const base::GoalPtr& getGoal(void) const
113  {
114  return pdef_->getGoal();
115  }
116 
118  const base::PlannerPtr& getPlanner(void) const
119  {
120  return planner_;
121  }
122 
125  {
126  return pa_;
127  }
128 
130  bool haveExactSolutionPath(void) const;
131 
132 
134  bool haveSolutionPath(void) const
135  {
136  return pdef_->getSolutionPath().get();
137  }
138 
140  PathControl& 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  {
160  si_->setStatePropagator(sp);
161  }
162 
164  void setStatePropagator(const StatePropagatorPtr &sp)
165  {
166  si_->setStatePropagator(sp);
167  }
168 
170  void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
171  {
172  pdef_->setStartAndGoalStates(start, goal, threshold);
173  }
174 
176  void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
177  {
178  pdef_->setGoalState(goal, threshold);
179  }
180 
184  {
185  pdef_->addStartState(state);
186  }
187 
189  void clearStartStates(void)
190  {
191  pdef_->clearStartStates();
192  }
193 
196  {
198  addStartState(state);
199  }
200 
203  void setGoal(const base::GoalPtr &goal)
204  {
205  pdef_->setGoal(goal);
206  }
207 
212  void setPlanner(const base::PlannerPtr &planner)
213  {
214  if (planner && planner->getSpaceInformation().get() != si_.get())
215  throw Exception("Planner instance does not match space information");
216  planner_ = planner;
217  configured_ = false;
218  }
219 
224  {
225  pa_ = pa;
226  planner_.reset();
227  configured_ = false;
228  }
229 
231  virtual base::PlannerStatus solve(double time = 1.0);
232 
235 
238  {
239  return last_status_;
240  }
241 
243  double getLastPlanComputationTime(void) const
244  {
245  return planTime_;
246  }
247 
251  virtual void clear(void);
252 
254  virtual void print(std::ostream &out = std::cout) const;
255 
259  virtual void setup(void);
260 
263  {
264  return params_;
265  }
266 
268  const base::ParamSet& params(void) const
269  {
270  return params_;
271  }
272 
273  protected:
274 
277 
280 
283 
286 
289 
291  double planTime_;
292 
295 
298  };
299 
302  }
303 
304 }
305 #endif
const ControlSpacePtr & getControlSpace(void) const
Get the current instance of the control space.
Definition: SimpleSetup.h:94
const SpaceInformationPtr & getSpaceInformation(void) const
Get the current instance of the space information.
Definition: SimpleSetup.h:76
void clearStartStates(void)
Clear the currently set starting states.
Definition: SimpleSetup.h:189
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:285
const base::PlannerAllocator & getPlannerAllocator(void) const
Get the planner allocator.
Definition: SimpleSetup.h:124
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called...
Definition: SimpleSetup.h:203
boost::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:279
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:41
void setStatePropagator(const StatePropagatorFn &sp)
Set the function that performs state propagation.
Definition: SimpleSetup.h:158
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
virtual void clear(void)
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: SimpleSetup.cpp:91
void setPlannerAllocator(const base::PlannerAllocator &pa)
Set the planner allocator to use. This is only used if no planner has been set. This is optional – a...
Definition: SimpleSetup.h:223
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:417
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const base::PlannerPtr & getPlanner(void) const
Get the current planner.
Definition: SimpleSetup.h:118
const base::StateValidityCheckerPtr & getStateValidityChecker(void) const
Get the current instance of the state validity checker.
Definition: SimpleSetup.h:100
Definition of a control path.
Definition: PathControl.h:54
SimpleSetup(const ControlSpacePtr &space)
Constructor needs the control space used for planning.
Definition: SimpleSetup.cpp:56
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:282
Maintain a set of parameters.
Definition: GenericParam.h:216
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
Definition: SimpleSetup.h:195
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
const base::StateSpacePtr & getStateSpace(void) const
Get the current instance of the state space.
Definition: SimpleSetup.h:88
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
A boost shared pointer wrapper for ompl::control::ControlSpace.
A boost shared pointer wrapper for ompl::base::Planner.
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:64
void setStatePropagator(const StatePropagatorPtr &sp)
Set the instance of StatePropagator to perform state propagation.
Definition: SimpleSetup.h:164
void setGoalState(const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
A simple form of setGoal(). The goal will be an instance of ompl::base::GoalState.
Definition: SimpleSetup.h:176
base::ParamSet & params(void)
Get the parameters for this planning context.
Definition: SimpleSetup.h:262
base::ParamSet params_
The parameters that describe the planning context.
Definition: SimpleSetup.h:297
const base::ParamSet & params(void) const
Get the parameters for this planning context.
Definition: SimpleSetup.h:268
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
Definition: SimpleSetup.h:170
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
PathControl & getSolutionPath(void) const
Get the solution path. Throw an exception if no solution is available.
base::PlannerStatus last_status_
The status of the last planning request.
Definition: SimpleSetup.h:294
const base::ProblemDefinitionPtr & getProblemDefinition(void) const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:82
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
base::PlannerStatus getLastPlannerStatus(void) const
Return the status of the last planning attempt.
Definition: SimpleSetup.h:237
A boost shared pointer wrapper for ompl::control::SpaceInformation.
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:276
The exception type for ompl.
Definition: Exception.h:47
const StatePropagatorPtr & getStatePropagator(void) const
Get the instance of the state propagator being used.
Definition: SimpleSetup.h:106
bool haveExactSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
double getLastPlanComputationTime(void) const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:243
const base::GoalPtr & getGoal(void) const
Get the current goal definition.
Definition: SimpleSetup.h:112
void addStartState(const base::ScopedState<> &state)
Add a starting state for planning. This call is not needed if setStartAndGoalStates() has been called...
Definition: SimpleSetup.h:183
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:146
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:291
void setPlanner(const base::PlannerPtr &planner)
Set the planner to use. If the planner is not set, an attempt is made to use the planner allocator...
Definition: SimpleSetup.h:212
A boost shared pointer wrapper for ompl::base::Goal.
bool haveSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:134
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:288
void setStateValidityChecker(const base::StateValidityCheckerFn &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:152