All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 
49 namespace ompl
50 {
51 
52  namespace geometric
53  {
54 
56  OMPL_CLASS_FORWARD(SimpleSetup);
58 
65  {
66  public:
67 
69  explicit
70  SimpleSetup(const base::StateSpacePtr &space);
71 
72  virtual ~SimpleSetup(void)
73  {
74  }
75 
78  {
79  return si_;
80  }
81 
84  {
85  return pdef_;
86  }
87 
90  {
91  return si_->getStateSpace();
92  }
93 
96  {
97  return si_->getStateValidityChecker();
98  }
99 
101  const base::GoalPtr& getGoal(void) const
102  {
103  return pdef_->getGoal();
104  }
105 
107  const base::PlannerPtr& getPlanner(void) const
108  {
109  return planner_;
110  }
111 
114  {
115  return pa_;
116  }
117 
120  {
121  return psk_;
122  }
123 
126  {
127  return psk_;
128  }
129 
131  bool haveExactSolutionPath(void) const;
132 
134  bool haveSolutionPath(void) const
135  {
136  return pdef_->getSolutionPath().get();
137  }
138 
140  PathGeometric& getSolutionPath(void) const;
141 
143  void getPlannerData(base::PlannerData &pd) const;
144 
147  {
148  si_->setStateValidityChecker(svc);
149  }
150 
153  {
154  si_->setStateValidityChecker(svc);
155  }
156 
159  const double threshold = std::numeric_limits<double>::epsilon())
160  {
161  pdef_->setStartAndGoalStates(start, goal, threshold);
162  }
163 
167  {
168  pdef_->addStartState(state);
169  }
170 
172  void clearStartStates(void)
173  {
174  pdef_->clearStartStates();
175  }
176 
179  {
181  addStartState(state);
182  }
183 
185  void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
186  {
187  pdef_->setGoalState(goal, threshold);
188  }
189 
192  void setGoal(const base::GoalPtr &goal)
193  {
194  pdef_->setGoal(goal);
195  }
196 
201  void setPlanner(const base::PlannerPtr &planner)
202  {
203  if (planner && planner->getSpaceInformation().get() != si_.get())
204  throw Exception("Planner instance does not match space information");
205  planner_ = planner;
206  configured_ = false;
207  }
208 
213  {
214  pa_ = pa;
215  planner_.reset();
216  configured_ = false;
217  }
218 
220  virtual base::PlannerStatus solve(double time = 1.0);
221 
224 
227  {
228  return last_status_;
229  }
230 
232  double getLastPlanComputationTime(void) const
233  {
234  return planTime_;
235  }
236 
238  double getLastSimplificationTime(void) const
239  {
240  return simplifyTime_;
241  }
242 
245  void simplifySolution(double duration = 0.0);
246 
249 
253  virtual void clear(void);
254 
256  virtual void print(std::ostream &out = std::cout) const;
257 
261  virtual void setup(void);
262 
265  {
266  return params_;
267  }
268 
270  const base::ParamSet& params(void) const
271  {
272  return params_;
273  }
274 
275  protected:
276 
279 
282 
285 
288 
291 
294 
296  double planTime_;
297 
300 
303 
306  };
307 
310  }
311 
312 }
313 #endif
const base::ProblemDefinitionPtr & getProblemDefinition(void) const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:83
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
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:158
double simplifyTime_
The amount of time the last path simplification step took.
Definition: SimpleSetup.h:299
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:287
bool haveSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:134
Definition of a scoped state.
Definition: ScopedState.h:56
const base::PlannerPtr & getPlanner(void) const
Get the current planner.
Definition: SimpleSetup.h:107
PathSimplifierPtr & getPathSimplifier(void)
Get the path simplifier.
Definition: SimpleSetup.h:125
A boost shared pointer wrapper for ompl::base::StateSpace.
base::ParamSet params_
The parameters that describe the planning context.
Definition: SimpleSetup.h:305
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:417
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called...
Definition: SimpleSetup.h:192
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:201
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:278
const base::StateSpacePtr & getStateSpace(void) const
Get the current instance of the state space.
Definition: SimpleSetup.h:89
PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: SimpleSetup.h:290
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Maintain a set of parameters.
Definition: GenericParam.h:216
bool haveExactSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
const PathSimplifierPtr & getPathSimplifier(void) const
Get the path simplifier.
Definition: SimpleSetup.h:119
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:64
const base::StateValidityCheckerPtr & getStateValidityChecker(void) const
Get the current instance of the state validity checker.
Definition: SimpleSetup.h:95
A boost shared pointer wrapper for ompl::base::Planner.
base::PlannerStatus last_status_
The status of the last planning request.
Definition: SimpleSetup.h:302
const base::ParamSet & params(void) const
Get the parameters for this planning context.
Definition: SimpleSetup.h:270
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:284
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:281
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
const base::GoalPtr & getGoal(void) const
Get the current goal definition.
Definition: SimpleSetup.h:101
void clearStartStates(void)
Clear the currently set starting states.
Definition: SimpleSetup.h:172
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:146
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:84
const base::PlannerAllocator & getPlannerAllocator(void) const
Get the planner allocator.
Definition: SimpleSetup.h:113
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:212
base::ParamSet & params(void)
Get the parameters for this planning context.
Definition: SimpleSetup.h:264
A boost shared pointer wrapper for ompl::geometric::PathSimplifier.
base::PlannerStatus getLastPlannerStatus(void) const
Return the status of the last planning attempt.
Definition: SimpleSetup.h:226
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:166
The exception type for ompl.
Definition: Exception.h:47
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
PathGeometric & getSolutionPath(void) const
Get the solution path. Throw an exception if no solution is available.
virtual void clear(void)
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:293
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:185
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:296
double getLastSimplificationTime(void) const
Get the amount of time (in seconds) spend during the last path simplification step.
Definition: SimpleSetup.h:238
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
Definition: SimpleSetup.h:178
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
double getLastPlanComputationTime(void) const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:232
A boost shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
Definition: PathGeometric.h:55
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
SimpleSetup(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
Definition: SimpleSetup.cpp:75
void setStateValidityChecker(const base::StateValidityCheckerFn &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:152
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:44
const base::SpaceInformationPtr & getSpaceInformation(void) const
Get the current instance of the space information.
Definition: SimpleSetup.h:77