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_GEOMETRIC_SIMPLE_SETUP_
38#define OMPL_GEOMETRIC_SIMPLE_SETUP_
39
40#include "ompl/base/Planner.h"
41#include "ompl/base/PlannerData.h"
42#include "ompl/base/SpaceInformation.h"
43#include "ompl/base/ProblemDefinition.h"
44#include "ompl/geometric/PathGeometric.h"
45#include "ompl/geometric/PathSimplifier.h"
46#include "ompl/util/Console.h"
47#include "ompl/util/Exception.h"
48
49namespace ompl
50{
51 namespace geometric
52 {
54 OMPL_CLASS_FORWARD(SimpleSetup);
56
63 {
64 public:
66 explicit SimpleSetup(const base::SpaceInformationPtr &si);
67
69 explicit SimpleSetup(const base::StateSpacePtr &space);
70
71 virtual ~SimpleSetup() = default;
72
74 const base::SpaceInformationPtr &getSpaceInformation() const
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 base::StateValidityCheckerPtr &getStateValidityChecker() const
99 {
100 return si_->getStateValidityChecker();
101 }
102
104 const base::GoalPtr &getGoal() const
105 {
106 return pdef_->getGoal();
107 }
108
110 const base::PlannerPtr &getPlanner() const
111 {
112 return planner_;
113 }
114
117 {
118 return pa_;
119 }
120
123 {
124 return psk_;
125 }
126
129 {
130 return psk_;
131 }
132
134 const base::OptimizationObjectivePtr &getOptimizationObjective() const
135 {
136 return pdef_->getOptimizationObjective();
137 }
138
142 {
143 return pdef_->hasExactSolution();
144 }
145
148 bool haveSolutionPath() const
149 {
150 return pdef_->hasSolution();
151 }
152
154 const std::string getSolutionPlannerName() const;
155
158
160 void getPlannerData(base::PlannerData &pd) const;
161
163 void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
164 {
165 si_->setStateValidityChecker(svc);
166 }
167
170 {
171 si_->setStateValidityChecker(svc);
172 }
173
175 void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
176 {
177 pdef_->setOptimizationObjective(optimizationObjective);
178 }
179
182 double threshold = std::numeric_limits<double>::epsilon());
183
187 {
188 pdef_->addStartState(state);
189 }
190
193 {
194 pdef_->clearStartStates();
195 }
196
199 {
201 addStartState(state);
202 }
203
205 void setGoalState(const base::ScopedState<> &goal,
206 double threshold = std::numeric_limits<double>::epsilon());
207
210 void setGoal(const base::GoalPtr &goal);
211
216 void setPlanner(const base::PlannerPtr &planner)
217 {
218 if (planner && planner->getSpaceInformation().get() != si_.get())
219 throw Exception("Planner instance does not match space information");
220 planner_ = planner;
221 configured_ = false;
222 }
223
228 {
229 pa_ = pa;
230 planner_.reset();
231 configured_ = false;
232 }
233
235 virtual base::PlannerStatus solve(double time = 1.0);
236
239
242 {
243 return lastStatus_;
244 }
245
248 {
249 return planTime_;
250 }
251
254 {
255 return simplifyTime_;
256 }
257
261 void simplifySolution(double duration = 0.0);
262
266
270 virtual void clear();
271
273 virtual void print(std::ostream &out = std::cout) const;
274
278 virtual void setup();
279
280 protected:
282 base::SpaceInformationPtr si_;
283
285 base::ProblemDefinitionPtr pdef_;
286
288 base::PlannerPtr planner_;
289
292
295
298
300 double planTime_;
301
304
307 };
308 }
309}
310#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
Definition of a geometric path.
A shared pointer wrapper for ompl::geometric::PathSimplifier.
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
const base::PlannerPtr & getPlanner() const
Get the current planner.
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
double simplifyTime_
The amount of time the last path simplification step took.
PathSimplifierPtr psk_
The instance of the path simplifier.
double planTime_
The amount of time the last planning step took.
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition SimpleSetup.h:80
double getLastSimplificationTime() const
Get the amount of time (in seconds) spend during the last path simplification step.
base::PlannerStatus lastStatus_
The status of the last planning request.
const base::StateValidityCheckerPtr & getStateValidityChecker() const
Get the current instance of the state validity checker.
Definition SimpleSetup.h:98
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
const std::string getSolutionPlannerName() const
Get the best solution's planer name. Throw an exception if no solution is available.
bool configured_
Flag indicating whether the classes needed for planning are set up.
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
const PathSimplifierPtr & getPathSimplifier() const
Get the path simplifier.
base::PlannerPtr planner_
The maintained planner instance.
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
const base::OptimizationObjectivePtr & getOptimizationObjective() const
Get the optimization objective to use.
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation....
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called.
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful)....
base::SpaceInformationPtr si_
The created space information.
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....
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
PathSimplifierPtr & getPathSimplifier()
Get the path simplifier.
const base::GoalPtr & getGoal() const
Get the current goal definition.
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 ...
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition SimpleSetup.h:74
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition SimpleSetup.h:92
void addStartState(const base::ScopedState<> &state)
Add a starting state for planning. This call is not needed if setStartAndGoalStates() has been called...
const base::PlannerAllocator & getPlannerAllocator() const
Get the planner allocator.
base::ProblemDefinitionPtr pdef_
The created problem definition.
void clearStartStates()
Clear the currently set starting states.
base::ProblemDefinitionPtr & getProblemDefinition()
Get the current instance of the problem definition.
Definition SimpleSetup.h:86
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.
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
base::PlannerAllocator pa_
The optional planner allocator.
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, double threshold=std::numeric_limits< double >::epsilon())
A simple form of setGoal(). The goal will be an instance of ompl::base::GoalState.
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...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()