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,...
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
Definition of a geometric path.
Definition: PathGeometric.h:66
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.
Definition: SimpleSetup.h:110
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.
Definition: SimpleSetup.h:303
PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: SimpleSetup.h:294
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:300
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.
Definition: SimpleSetup.h:253
base::PlannerStatus lastStatus_
The status of the last planning request.
Definition: SimpleSetup.h:306
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...
Definition: SimpleSetup.h:141
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.
Definition: SimpleSetup.h:297
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.
Definition: SimpleSetup.cpp:85
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.
Definition: SimpleSetup.h:122
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:288
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.
Definition: SimpleSetup.h:163
const base::OptimizationObjectivePtr & getOptimizationObjective() const
Get the optimization objective to use.
Definition: SimpleSetup.h:134
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation....
Definition: SimpleSetup.cpp:77
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)....
Definition: SimpleSetup.h:148
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:282
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:216
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:247
PathSimplifierPtr & getPathSimplifier()
Get the path simplifier.
Definition: SimpleSetup.h:128
const base::GoalPtr & getGoal() const
Get the current goal definition.
Definition: SimpleSetup.h:104
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:227
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.
Definition: SimpleSetup.h:175
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...
Definition: SimpleSetup.h:186
const base::PlannerAllocator & getPlannerAllocator() const
Get the planner allocator.
Definition: SimpleSetup.h:116
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:285
void clearStartStates()
Clear the currently set starting states.
Definition: SimpleSetup.h:192
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.
Definition: SimpleSetup.h:169
base::PlannerStatus getLastPlannerStatus() const
Return the status of the last planning attempt.
Definition: SimpleSetup.h:241
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
Definition: SimpleSetup.h:198
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:291
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, double threshold=std::numeric_limits< double >::epsilon())
A simple form of setGoal(). The goal will be an instance of ompl::base::GoalState.
Definition: SimpleSetup.cpp:96
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...
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49