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
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
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
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:181
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:345
Maintain a set of parameters.
Definition: GenericParam.h:234
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
const ParamSet & params() const
Get the parameters for this planner.
Definition: Planner.h:347
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
const T * as() const
Cast this instance to a desired type.
Definition: Planner.h:257
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...
Definition of an abstract state.
Definition: State.h:50
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
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
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
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:352
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 restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:170
const PlannerProgressProperties & getPlannerProgressProperties() const
Retrieve a planner&#39;s planner progress property map.
Definition: Planner.h:359
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
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:183
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