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
49namespace ompl
50{
51 namespace control
52 {
54 OMPL_CLASS_FORWARD(SimpleSetup);
56
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
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
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
164 {
165 si_->setStatePropagator(sp);
166 }
167
170 {
171 si_->setStatePropagator(sp);
172 }
173
175 void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
176 {
177 pdef_->setOptimizationObjective(optimizationObjective);
178 }
179
182 const double threshold = std::numeric_limits<double>::epsilon())
183 {
184 pdef_->setStartAndGoalStates(start, goal, threshold);
185 }
186
189 const double threshold = std::numeric_limits<double>::epsilon())
190 {
191 pdef_->setGoalState(goal, threshold);
192 }
193
197 {
198 pdef_->addStartState(state);
199 }
200
203 {
204 pdef_->clearStartStates();
205 }
206
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
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
289
291 double planTime_;
292
295 };
296 }
297}
298#endif
The exception type for ompl.
Definition: Exception.h:47
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition of a scoped state.
Definition: ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::control::StatePropagator.
A shared pointer wrapper for ompl::control::ControlSpace.
Definition of a control path.
Definition: PathControl.h:61
Create the set of classes typically needed to solve a control problem.
Definition: SimpleSetup.h:63
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:288
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:225
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:279
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:196
const base::StateValidityCheckerPtr & getStateValidityChecker() const
Get the current instance of the state validity checker.
Definition: SimpleSetup.h:104
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful)....
Definition: SimpleSetup.h:139
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:282
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:285
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
Definition: SimpleSetup.h:208
base::ProblemDefinitionPtr & getProblemDefinition()
Get the current instance of the problem definition.
Definition: SimpleSetup.h:86
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
void setStateValidityChecker(const base::StateValidityCheckerFn &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:157
base::PlannerStatus getLastPlannerStatus() const
Return the status of the last planning attempt.
Definition: SimpleSetup.h:250
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:54
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:188
void setStatePropagator(const StatePropagatorPtr &sp)
Set the instance of StatePropagator to perform state propagation.
Definition: SimpleSetup.h:169
const base::GoalPtr & getGoal() const
Get the current goal definition.
Definition: SimpleSetup.h:116
const StatePropagatorPtr & getStatePropagator() const
Get the instance of the state propagator being used.
Definition: SimpleSetup.h:110
PathControl & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:151
SimpleSetup(const SpaceInformationPtr &si)
Constructor needs the control space used for planning.
Definition: SimpleSetup.cpp:40
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 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:181
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:92
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:236
base::PlannerStatus last_status_
The status of the last planning request.
Definition: SimpleSetup.h:294
const ControlSpacePtr & getControlSpace() const
Get the current instance of the control space.
Definition: SimpleSetup.h:98
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:80
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
void setStatePropagator(const StatePropagatorFn &sp)
Set the function that performs state propagation.
Definition: SimpleSetup.h:163
const base::PlannerPtr & getPlanner() const
Get the current planner.
Definition: SimpleSetup.h:122
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called.
Definition: SimpleSetup.h:216
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation....
Definition: SimpleSetup.cpp:78
void clearStartStates()
Clear the currently set starting states.
Definition: SimpleSetup.h:202
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:256
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
Definition: SimpleSetup.h:175
SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:276
const SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:74
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:291
const base::PlannerAllocator & getPlannerAllocator() const
Get the planner allocator.
Definition: SimpleSetup.h:128
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:444
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
std::function< void(const base::State *, const Control *, const double, base::State *)> StatePropagatorFn
A function that achieves state propagation.
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49