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  namespace control
52  {
54  OMPL_CLASS_FORWARD(SimpleSetup);
56 
62  class SimpleSetup
63  {
64  public:
66  explicit SimpleSetup(const SpaceInformationPtr &si);
67 
69  explicit SimpleSetup(const ControlSpacePtr &space);
70 
71  virtual ~SimpleSetup() = default;
72 
75  {
76  return si_;
77  }
78 
80  const base::ProblemDefinitionPtr &getProblemDefinition() const
81  {
82  return pdef_;
83  }
84 
86  base::ProblemDefinitionPtr &getProblemDefinition()
87  {
88  return pdef_;
89  }
90 
92  const base::StateSpacePtr &getStateSpace() const
93  {
94  return si_->getStateSpace();
95  }
96 
98  const ControlSpacePtr &getControlSpace() const
99  {
100  return si_->getControlSpace();
101  }
102 
104  const base::StateValidityCheckerPtr &getStateValidityChecker() const
105  {
106  return si_->getStateValidityChecker();
107  }
108 
111  {
112  return si_->getStatePropagator();
113  }
114 
116  const base::GoalPtr &getGoal() const
117  {
118  return pdef_->getGoal();
119  }
120 
122  const base::PlannerPtr &getPlanner() const
123  {
124  return planner_;
125  }
126 
129  {
130  return pa_;
131  }
132 
135  bool haveExactSolutionPath() const;
136 
139  bool haveSolutionPath() const
140  {
141  return pdef_->getSolutionPath() != nullptr;
142  }
143 
145  PathControl &getSolutionPath() const;
146 
148  void getPlannerData(base::PlannerData &pd) const;
149 
151  void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
152  {
153  si_->setStateValidityChecker(svc);
154  }
155 
158  {
159  si_->setStateValidityChecker(svc);
160  }
161 
163  void setStatePropagator(const StatePropagatorFn &sp)
164  {
165  si_->setStatePropagator(sp);
166  }
167 
169  void setStatePropagator(const StatePropagatorPtr &sp)
170  {
171  si_->setStatePropagator(sp);
172  }
173 
175  void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
176  {
177  pdef_->setOptimizationObjective(optimizationObjective);
178  }
179 
181  void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal,
182  const double threshold = std::numeric_limits<double>::epsilon())
183  {
184  pdef_->setStartAndGoalStates(start, goal, threshold);
185  }
186 
188  void setGoalState(const base::ScopedState<> &goal,
189  const double threshold = std::numeric_limits<double>::epsilon())
190  {
191  pdef_->setGoalState(goal, threshold);
192  }
193 
196  void addStartState(const base::ScopedState<> &state)
197  {
198  pdef_->addStartState(state);
199  }
200 
202  void clearStartStates()
203  {
204  pdef_->clearStartStates();
205  }
206 
208  void setStartState(const base::ScopedState<> &state)
209  {
211  addStartState(state);
212  }
213 
216  void setGoal(const base::GoalPtr &goal)
217  {
218  pdef_->setGoal(goal);
219  }
220 
225  void setPlanner(const base::PlannerPtr &planner)
226  {
227  if (planner && planner->getSpaceInformation().get() != si_.get())
228  throw Exception("Planner instance does not match space information");
229  planner_ = planner;
230  configured_ = false;
231  }
232 
237  {
238  pa_ = pa;
239  planner_.reset();
240  configured_ = false;
241  }
242 
244  virtual base::PlannerStatus solve(double time = 1.0);
245 
248 
251  {
252  return last_status_;
253  }
254 
256  double getLastPlanComputationTime() const
257  {
258  return planTime_;
259  }
260 
264  virtual void clear();
265 
267  virtual void print(std::ostream &out = std::cout) const;
268 
272  virtual void setup();
273 
274  protected:
277 
279  base::ProblemDefinitionPtr pdef_;
280 
282  base::PlannerPtr planner_;
283 
286 
288  bool configured_;
289 
291  double planTime_;
292 
295  };
296  }
297 }
298 #endif
const base::PlannerAllocator & getPlannerAllocator() const
Get the planner allocator.
Definition: SimpleSetup.h:192
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
Definition: SimpleSetup.h:239
const ControlSpacePtr & getControlSpace() const
Get the current instance of the control space.
Definition: SimpleSetup.h:162
A shared pointer wrapper for ompl::base::SpaceInformation.
const base::StateValidityCheckerPtr & getStateValidityChecker() const
Get the current instance of the state validity checker.
Definition: SimpleSetup.h:168
SimpleSetup(const SpaceInformationPtr &si)
Constructor needs the control space used for planning.
Definition: SimpleSetup.cpp:40
A shared pointer wrapper for ompl::control::ControlSpace.
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:138
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:343
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:300
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:340
const base::GoalPtr & getGoal() const
Get the current goal definition.
Definition: SimpleSetup.h:180
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:289
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
Definition: SimpleSetup.h:272
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:349
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
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:245
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:355
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:54
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:352
A class to store the exit status of Planner::solve()
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:501
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:88
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:252
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:144
void clearStartStates()
Clear the currently set starting states.
Definition: SimpleSetup.h:266
const base::PlannerPtr & getPlanner() const
Get the current planner.
Definition: SimpleSetup.h:186
base::PlannerStatus getLastPlannerStatus() const
Return the status of the last planning attempt.
Definition: SimpleSetup.h:314
PathControl & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:346
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
A shared pointer wrapper for ompl::control::StatePropagator.
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:260
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called.
Definition: SimpleSetup.h:280
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful)....
Definition: SimpleSetup.h:203
void setStatePropagator(const StatePropagatorFn &sp)
Set the function that performs state propagation.
Definition: SimpleSetup.h:227
Definition of a scoped state.
Definition: ScopedState.h:120
const StatePropagatorPtr & getStatePropagator() const
Get the instance of the state propagator being used.
Definition: SimpleSetup.h:174
The exception type for ompl.
Definition: Exception.h:78
base::PlannerStatus last_status_
The status of the last planning request.
Definition: SimpleSetup.h:358
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:215
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:320
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation....
Definition: SimpleSetup.cpp:78
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:156