All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
Planner.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_BASE_PLANNER_
38 #define OMPL_BASE_PLANNER_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/base/ProblemDefinition.h"
42 #include "ompl/base/PlannerData.h"
43 #include "ompl/base/PlannerStatus.h"
44 #include "ompl/base/PlannerTerminationCondition.h"
45 #include "ompl/base/GenericParam.h"
46 #include "ompl/util/Console.h"
47 #include "ompl/util/Time.h"
48 #include "ompl/util/ClassForward.h"
49 #include "ompl/util/Deprecation.h"
50 #include <boost/function.hpp>
51 #include <boost/concept_check.hpp>
52 #include <boost/noncopyable.hpp>
53 #include <boost/lexical_cast.hpp>
54 #include <string>
55 #include <map>
56 
57 namespace ompl
58 {
59 
60  namespace base
61  {
62 
64 
65  OMPL_CLASS_FORWARD(Planner);
67 
84  {
85  public:
86 
88  PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
89  {
90  tempState_ = NULL;
91  update();
92  }
93 
95  PlannerInputStates(const Planner *planner) : planner_(planner)
96  {
97  tempState_ = NULL;
98  update();
99  }
100 
104  PlannerInputStates(void) : planner_(NULL)
105  {
106  tempState_ = NULL;
107  clear();
108  }
109 
112  {
113  clear();
114  }
115 
117  void clear(void);
118 
122  void restart(void);
123 
129  bool update(void);
130 
136  bool use(const ProblemDefinitionPtr &pdef);
137 
143  bool use(const ProblemDefinition *pdef);
144 
147  void checkValidity(void) const;
148 
151  const State* nextStart(void);
152 
161  const State* nextGoal(const PlannerTerminationCondition &ptc);
162 
164  const State* nextGoal(void);
165 
167  bool haveMoreStartStates(void) const;
168 
170  bool haveMoreGoalStates(void) const;
171 
175  unsigned int getSeenStartStatesCount(void) const
176  {
177  return addedStartStates_;
178  }
179 
181  unsigned int getSampledGoalsCount(void) const
182  {
183  return sampledGoalsCount_;
184  }
185 
186  private:
187 
188  const Planner *planner_;
189 
190  unsigned int addedStartStates_;
191  unsigned int sampledGoalsCount_;
192  State *tempState_;
193 
194  const ProblemDefinition *pdef_;
195  const SpaceInformation *si_;
196  };
197 
200  {
202  {
203  }
204 
207 
210 
213 
217 
220  bool directed;
221 
224  };
225 
227  class Planner : private boost::noncopyable
228  {
229 
230  public:
231 
233  Planner(const SpaceInformationPtr &si, const std::string &name);
234 
236  virtual ~Planner(void)
237  {
238  }
239 
241  template<class T>
242  T* as(void)
243  {
245  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
246 
247  return static_cast<T*>(this);
248  }
249 
251  template<class T>
252  const T* as(void) const
253  {
255  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
256 
257  return static_cast<const T*>(this);
258  }
259 
261  const SpaceInformationPtr& getSpaceInformation(void) const;
262 
264  const ProblemDefinitionPtr& getProblemDefinition(void) const;
265 
267  const PlannerInputStates& getPlannerInputStates(void) const;
268 
273  virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
274 
287  virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0;
288 
291  PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
292 
296  PlannerStatus solve(double solveTime);
297 
301  virtual void clear(void);
302 
309  virtual void getPlannerData(PlannerData &data) const;
310 
312  const std::string& getName(void) const;
313 
315  void setName(const std::string &name);
316 
318  const PlannerSpecs& getSpecs(void) const;
319 
324  virtual void setup(void);
325 
330  virtual void checkValidity(void);
331 
333  bool isSetup(void) const;
334 
337  {
338  return params_;
339  }
340 
342  const ParamSet& params(void) const
343  {
344  return params_;
345  }
346 
348  typedef boost::function<std::string ()> PlannerProgressProperty;
349 
351  typedef std::map<std::string, PlannerProgressProperty> PlannerProgressProperties;
352 
355  {
357  }
358 
360  virtual void printProperties(std::ostream &out) const;
361 
363  virtual void printSettings(std::ostream &out) const;
364 
365  protected:
366 
368  template<typename T, typename PlannerType, typename SetterType, typename GetterType>
369  void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const GetterType& getter, const std::string &rangeSuggestion = "")
370  {
371  params_.declareParam<T>(name, boost::bind(setter, planner, _1), boost::bind(getter, planner));
372  if (!rangeSuggestion.empty())
373  params_[name].setRangeSuggestion(rangeSuggestion);
374  }
375 
377  template<typename T, typename PlannerType, typename SetterType>
378  void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const std::string &rangeSuggestion = "")
379  {
380  params_.declareParam<T>(name, boost::bind(setter, planner, _1));
381  if (!rangeSuggestion.empty())
382  params_[name].setRangeSuggestion(rangeSuggestion);
383  }
384 
386  void addPlannerProgressProperty(const std::string& progressPropertyName, const PlannerProgressProperty& prop)
387  {
388  plannerProgressProperties_[progressPropertyName] = prop;
389  }
390 
393 
396 
399 
401  std::string name_;
402 
405 
408 
411 
413  bool setup_;
414  };
415 
417  typedef boost::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator;
418  }
419 }
420 
421 #endif
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
Planner(const SpaceInformationPtr &si, const std::string &name)
Constructor.
Definition: Planner.cpp:43
void addPlannerProgressProperty(const std::string &progressPropertyName, const PlannerProgressProperty &prop)
Add a planner progress property called progressPropertyName with a property querying function prop to...
Definition: Planner.h:386
const State * nextStart(void)
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:228
Properties that planners may have.
Definition: Planner.h:199
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
virtual ~Planner(void)
Destructor.
Definition: Planner.h:236
const ParamSet & params(void) const
Get the parameters for this planner.
Definition: Planner.h:342
void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter, const GetterType &getter, const std::string &rangeSuggestion="")
This function declares a parameter for this planner instance, and specifies the setter and getter fun...
Definition: Planner.h:369
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:135
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
const State * nextGoal(void)
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:262
const std::string & getName(void) const
Get the name of the planner.
Definition: Planner.cpp:55
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...
bool isSetup(void) const
Check if setup() was called for this planner.
Definition: Planner.cpp:107
const ProblemDefinitionPtr & getProblemDefinition(void) const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:70
PlannerInputStates(void)
Default constructor. No work is performed. A call to use() needs to be made, before making any calls ...
Definition: Planner.h:104
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:395
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:209
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:220
Maintain a set of parameters.
Definition: GenericParam.h:216
void checkValidity(void) const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:181
const T * as(void) const
Cast this instance to a desired type.
Definition: Planner.h:252
PlannerInputStates(const Planner *planner)
Default constructor. No work is performed.
Definition: Planner.h:95
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:60
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:407
bool update(void)
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:174
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:413
PlannerInputStates(const PlannerPtr &planner)
Default constructor. No work is performed.
Definition: Planner.h:88
bool provingSolutionNonExistence
Flag indicating whether the planner is able to prove that no solution path exists.
Definition: Planner.h:223
const PlannerSpecs & getSpecs(void) const
Return the specifications (capabilities of this planner)
Definition: Planner.cpp:50
A boost shared pointer wrapper for ompl::base::Planner.
T * as(void)
Cast this instance to a desired type.
Definition: Planner.h:242
virtual void checkValidity(void)
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
Base class for a planner.
Definition: Planner.h:227
void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter, const std::string &rangeSuggestion="")
This function declares a parameter for this planner instance, and specifies the setter function...
Definition: Planner.h:378
void restart(void)
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:168
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information this planner is using.
Definition: Planner.cpp:65
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
unsigned int getSeenStartStatesCount(void) const
Get the number of start states from the problem definition that were already seen, including invalid ones.
Definition: Planner.h:175
A boost shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
Definition: Planner.cpp:75
unsigned int getSampledGoalsCount(void) const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:181
Definition of an abstract state.
Definition: State.h:50
bool haveMoreStartStates(void) const
Check if there are more potential start states.
Definition: Planner.cpp:342
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:398
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:404
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
GoalType
The type of goal.
Definition: GoalTypes.h:46
ParamSet & params(void)
Get the parameters for this planner.
Definition: Planner.h:336
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=typename SpecificParam< T >::GetterFn())
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:222
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
const PlannerProgressProperties & getPlannerProgressProperties() const
Retrieve a planner's planner progress property map.
Definition: Planner.h:354
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
std::string name_
The name of this planner.
Definition: Planner.h:401
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: Planner.cpp:118
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:216
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:83
const PlannerInputStates & getPlannerInputStates(void) const
Get the planner input states.
Definition: Planner.cpp:81
~PlannerInputStates(void)
Destructor. Clear allocated memory.
Definition: Planner.h:111
bool haveMoreGoalStates(void) const
Check if there are more potential goal states.
Definition: Planner.cpp:349
void clear(void)
Clear all stored information.
Definition: Planner.cpp:155
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:392
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
Definition: Planner.cpp:149
std::map< std::string, PlannerProgressProperty > PlannerProgressProperties
A dictionary which maps the name of a progress property to the function to be used for querying that ...
Definition: Planner.h:351
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner's progress property names and the functions used for querying those pr...
Definition: Planner.h:410
bool use(const ProblemDefinitionPtr &pdef)
Set the problem definition this class operates on. If a planner is not set in the constructor argumen...
Definition: Planner.cpp:205
boost::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
This bit is set if casting to generic goal regions (ompl::base::Goal) is possible. This bit shold always be set.
Definition: GoalTypes.h:49
boost::function< std::string()> PlannerProgressProperty
Definition of a function which returns a property about the planner's progress that can be queried by...
Definition: Planner.h:348