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 #include "ompl/util/Deprecation.h"
49 
50 namespace ompl
51 {
52 
53  namespace geometric
54  {
55 
57  OMPL_CLASS_FORWARD(SimpleSetup);
59 
66  {
67  public:
68 
70  explicit
72 
74  explicit
75  SimpleSetup(const base::StateSpacePtr &space);
76 
77  virtual ~SimpleSetup()
78  {
79  }
80 
83  {
84  return si_;
85  }
86 
89  {
90  return pdef_;
91  }
92 
95  {
96  return si_->getStateSpace();
97  }
98 
101  {
102  return si_->getStateValidityChecker();
103  }
104 
106  const base::GoalPtr& getGoal() const
107  {
108  return pdef_->getGoal();
109  }
110 
113  {
114  return planner_;
115  }
116 
119  {
120  return pa_;
121  }
122 
125  {
126  return psk_;
127  }
128 
131  {
132  return psk_;
133  }
134 
137  {
138  return pdef_->getOptimizationObjective();
139  }
140 
142  bool haveExactSolutionPath() const;
143 
145  bool haveSolutionPath() const
146  {
147  return pdef_->getSolutionPath().get();
148  }
149 
151  const std::string getSolutionPlannerName(void) const;
152 
155 
157  void getPlannerData(base::PlannerData &pd) const;
158 
161  {
162  si_->setStateValidityChecker(svc);
163  }
164 
167  {
168  si_->setStateValidityChecker(svc);
169  }
170 
172  void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
173  {
174  pdef_->setOptimizationObjective(optimizationObjective);
175  }
176 
179  const double threshold = std::numeric_limits<double>::epsilon())
180  {
181  pdef_->setStartAndGoalStates(start, goal, threshold);
182 
183  // Clear any past solutions since they no longer correspond to our start and goal states
184  pdef_->clearSolutionPaths();
185  }
186 
190  {
191  pdef_->addStartState(state);
192  }
193 
196  {
197  pdef_->clearStartStates();
198  }
199 
202  {
204  addStartState(state);
205  }
206 
208  void setGoalState(const base::ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
209  {
210  pdef_->setGoalState(goal, threshold);
211  }
212 
215  void setGoal(const base::GoalPtr &goal)
216  {
217  pdef_->setGoal(goal);
218  }
219 
224  void setPlanner(const base::PlannerPtr &planner)
225  {
226  if (planner && planner->getSpaceInformation().get() != si_.get())
227  throw Exception("Planner instance does not match space information");
228  planner_ = planner;
229  configured_ = false;
230  }
231 
236  {
237  pa_ = pa;
238  planner_.reset();
239  configured_ = false;
240  }
241 
243  virtual base::PlannerStatus solve(double time = 1.0);
244 
247 
250  {
251  return lastStatus_;
252  }
253 
256  {
257  return planTime_;
258  }
259 
262  {
263  return simplifyTime_;
264  }
265 
268  void simplifySolution(double duration = 0.0);
269 
272 
276  virtual void clear();
277 
279  virtual void print(std::ostream &out = std::cout) const;
280 
284  virtual void setup();
285 
286  protected:
287 
290 
293 
296 
299 
302 
305 
307  double planTime_;
308 
311 
314  };
315 
318  OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal);
319  }
320 
321 }
322 #endif
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
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:178
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:145
void setOptimizationObjective(const base::OptimizationObjectivePtr &optimizationObjective)
Set the optimization objective to use.
Definition: SimpleSetup.h:172
const base::StateValidityCheckerPtr & getStateValidityChecker() const
Get the current instance of the state validity checker.
Definition: SimpleSetup.h:100
double simplifyTime_
The amount of time the last path simplification step took.
Definition: SimpleSetup.h:310
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
base::PlannerAllocator pa_
The optional planner allocator.
Definition: SimpleSetup.h:298
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
const base::StateSpacePtr & getStateSpace() const
Get the current instance of the state space.
Definition: SimpleSetup.h:94
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:422
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:215
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:224
const base::SpaceInformationPtr & getSpaceInformation() const
Get the current instance of the space information.
Definition: SimpleSetup.h:82
double getLastPlanComputationTime() const
Get the amount of time (in seconds) spent during the last planning step.
Definition: SimpleSetup.h:255
const base::PlannerAllocator & getPlannerAllocator() const
Get the planner allocator.
Definition: SimpleSetup.h:118
base::SpaceInformationPtr si_
The created space information.
Definition: SimpleSetup.h:289
PathSimplifierPtr psk_
The instance of the path simplifier.
Definition: SimpleSetup.h:301
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
void clearStartStates()
Clear the currently set starting states.
Definition: SimpleSetup.h:195
double getLastSimplificationTime() const
Get the amount of time (in seconds) spend during the last path simplification step.
Definition: SimpleSetup.h:261
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
A boost shared pointer wrapper for ompl::base::Planner.
base::PlannerStatus getLastPlannerStatus() const
Return the status of the last planning attempt.
Definition: SimpleSetup.h:249
SimpleSetup(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: SimpleSetup.cpp:45
base::PlannerPtr planner_
The maintained planner instance.
Definition: SimpleSetup.h:295
const base::ProblemDefinitionPtr & getProblemDefinition() const
Get the current instance of the problem definition.
Definition: SimpleSetup.h:88
base::ProblemDefinitionPtr pdef_
The created problem definition.
Definition: SimpleSetup.h:292
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
A boost shared pointer wrapper for ompl::base::SpaceInformation.
PathSimplifierPtr & getPathSimplifier()
Get the path simplifier.
Definition: SimpleSetup.h:130
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:160
const base::OptimizationObjectivePtr & getOptimizationObjective() const
Get the optimization objective to use.
Definition: SimpleSetup.h:136
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 – ...
Definition: SimpleSetup.h:235
A boost shared pointer wrapper for ompl::geometric::PathSimplifier.
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:61
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:189
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
The exception type for ompl.
Definition: Exception.h:47
base::PlannerStatus lastStatus_
The status of the last planning request.
Definition: SimpleSetup.h:313
const base::GoalPtr & getGoal() const
Get the current goal definition.
Definition: SimpleSetup.h:106
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:93
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:304
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
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:208
double planTime_
The amount of time the last planning step took.
Definition: SimpleSetup.h:307
const PathSimplifierPtr & getPathSimplifier() const
Get the path simplifier.
Definition: SimpleSetup.h:124
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
Definition: SimpleSetup.h:201
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
A boost shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
Definition: PathGeometric.h:60
const std::string getSolutionPlannerName(void) const
Get the best solution&#39;s planer name. Throw an exception if no solution is available.
const base::PlannerPtr & getPlanner() const
Get the current planner.
Definition: SimpleSetup.h:112
void setStateValidityChecker(const base::StateValidityCheckerFn &svc)
Set the state validity checker to use.
Definition: SimpleSetup.h:166
OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:40
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: SimpleSetup.cpp:84