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() : planner_(NULL)
105  {
106  tempState_ = NULL;
107  clear();
108  }
109 
112  {
113  clear();
114  }
115 
117  void clear();
118 
122  void restart();
123 
129  bool update();
130 
136  bool use(const ProblemDefinitionPtr &pdef);
137 
143  bool use(const ProblemDefinition *pdef);
144 
147  void checkValidity() const;
148 
151  const State* nextStart();
152 
161  const State* nextGoal(const PlannerTerminationCondition &ptc);
162 
164  const State* nextGoal();
165 
167  bool haveMoreStartStates() const;
168 
170  bool haveMoreGoalStates() const;
171 
175  unsigned int getSeenStartStatesCount() const
176  {
177  return addedStartStates_;
178  }
179 
181  unsigned int getSampledGoalsCount() 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  {
201  PlannerSpecs() : recognizedGoal(GOAL_ANY), multithreaded(false), approximateSolutions(false),
202  optimizingPaths(false), directed(false), provingSolutionNonExistence(false),
203  canReportIntermediateSolutions(false)
204  {
205  }
206 
209 
212 
215 
219 
222  bool directed;
223 
226 
229  };
230 
232  class Planner : private boost::noncopyable
233  {
234 
235  public:
236 
238  Planner(const SpaceInformationPtr &si, const std::string &name);
239 
241  virtual ~Planner()
242  {
243  }
244 
246  template<class T>
247  T* as()
248  {
250  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
251 
252  return static_cast<T*>(this);
253  }
254 
256  template<class T>
257  const T* as() const
258  {
260  BOOST_CONCEPT_ASSERT((boost::Convertible<T*, Planner*>));
261 
262  return static_cast<const T*>(this);
263  }
264 
266  const SpaceInformationPtr& getSpaceInformation() const;
267 
269  const ProblemDefinitionPtr& getProblemDefinition() const;
270 
272  const PlannerInputStates& getPlannerInputStates() const;
273 
278  virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
279 
292  virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0;
293 
296  PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
297 
301  PlannerStatus solve(double solveTime);
302 
306  virtual void clear();
307 
314  virtual void getPlannerData(PlannerData &data) const;
315 
317  const std::string& getName() const;
318 
320  void setName(const std::string &name);
321 
323  const PlannerSpecs& getSpecs() const;
324 
329  virtual void setup();
330 
335  virtual void checkValidity();
336 
338  bool isSetup() const;
339 
342  {
343  return params_;
344  }
345 
347  const ParamSet& params() const
348  {
349  return params_;
350  }
351 
353  typedef boost::function<std::string ()> PlannerProgressProperty;
354 
356  typedef std::map<std::string, PlannerProgressProperty> PlannerProgressProperties;
357 
359  const PlannerProgressProperties& getPlannerProgressProperties() const
360  {
361  return plannerProgressProperties_;
362  }
363 
365  virtual void printProperties(std::ostream &out) const;
366 
368  virtual void printSettings(std::ostream &out) const;
369 
370  protected:
371 
373  template<typename T, typename PlannerType, typename SetterType, typename GetterType>
374  void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const GetterType& getter, const std::string &rangeSuggestion = "")
375  {
376  params_.declareParam<T>(name, boost::bind(setter, planner, _1), boost::bind(getter, planner));
377  if (!rangeSuggestion.empty())
378  params_[name].setRangeSuggestion(rangeSuggestion);
379  }
380 
382  template<typename T, typename PlannerType, typename SetterType>
383  void declareParam(const std::string &name, const PlannerType &planner, const SetterType& setter, const std::string &rangeSuggestion = "")
384  {
385  params_.declareParam<T>(name, boost::bind(setter, planner, _1));
386  if (!rangeSuggestion.empty())
387  params_[name].setRangeSuggestion(rangeSuggestion);
388  }
389 
391  void addPlannerProgressProperty(const std::string& progressPropertyName, const PlannerProgressProperty& prop)
392  {
393  plannerProgressProperties_[progressPropertyName] = prop;
394  }
395 
398 
401 
404 
406  std::string name_;
407 
410 
413 
415  PlannerProgressProperties plannerProgressProperties_;
416 
418  bool setup_;
419  };
420 
422  typedef boost::function<PlannerPtr(const SpaceInformationPtr&)> PlannerAllocator;
423  }
424 }
425 
426 #endif
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
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:391
Properties that planners may have.
Definition: Planner.h:199
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:374
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:352
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:208
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:422
ParamSet & params()
Get the parameters for this planner.
Definition: Planner.h:341
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths...
Definition: Planner.h:228
virtual ~Planner()
Destructor.
Definition: Planner.h:241
unsigned int getSeenStartStatesCount() const
Get the number of start states from the problem definition that were already seen, including invalid ones.
Definition: Planner.h:175
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:211
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:222
Maintain a set of parameters.
Definition: GenericParam.h:234
const ParamSet & params() const
Get the parameters for this planner.
Definition: Planner.h:347
PlannerInputStates(const Planner *planner)
Default constructor. No work is performed.
Definition: Planner.h:95
~PlannerInputStates()
Destructor. Clear allocated memory.
Definition: Planner.h:111
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:412
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:418
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:225
Main namespace. Contains everything in this library.
Definition: Cost.h:42
A boost shared pointer wrapper for ompl::base::Planner.
const State * nextGoal()
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:264
Base class for a planner.
Definition: Planner.h:232
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:383
void clear()
Clear all stored information.
Definition: Planner.cpp:157
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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...
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:181
Definition of an abstract state.
Definition: State.h:50
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:345
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
GoalType
The type of goal.
Definition: GoalTypes.h:46
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
bool update()
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:176
const PlannerProgressProperties & getPlannerProgressProperties() const
Retrieve a planner&#39;s planner progress property map.
Definition: Planner.h:359
std::string name_
The name of this planner.
Definition: Planner.h:406
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:83
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:183
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
const T * as() const
Cast this instance to a desired type.
Definition: Planner.h:257
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
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:356
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner&#39;s progress property names and the functions used for querying those pr...
Definition: Planner.h:415
PlannerInputStates()
Default constructor. No work is performed. A call to use() needs to be made, before making any calls ...
Definition: Planner.h:104
T * as()
Cast this instance to a desired type.
Definition: Planner.h:247
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:207
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&#39;s progress that can be queried by...
Definition: Planner.h:353