Loading...
Searching...
No Matches
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
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,...
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.
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....
base::ProblemDefinitionPtr pdef_
The created problem definition.
void addStartState(const base::ScopedState<> &state)
Add a starting state for planning. This call is not needed if setStartAndGoalStates() has been called...
const base::StateValidityCheckerPtr & getStateValidityChecker() const
Get the current instance of the state validity checker.
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful)....
base::PlannerPtr planner_
The maintained planner instance.
base::PlannerAllocator pa_
The optional planner allocator.
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
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.
base::PlannerStatus getLastPlannerStatus() const
Return the status of the last planning attempt.
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
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.
void setStatePropagator(const StatePropagatorPtr &sp)
Set the instance of StatePropagator to perform state propagation.
const base::GoalPtr & getGoal() const
Get the current goal definition.
const StatePropagatorPtr & getStatePropagator() const
Get the instance of the state propagator being used.
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.
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for a specified amount of time (default is 1 second)
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.
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 ...
base::PlannerStatus last_status_
The status of the last planning request.
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.
const base::PlannerPtr & getPlanner() const
Get the current planner.
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called.
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation....
void clearStartStates()
Clear the currently set starting states.
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
SpaceInformationPtr si_
The created space information.
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.
const base::PlannerAllocator & getPlannerAllocator() const
Get the planner allocator.
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition Planner.h:437
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()