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 <functional>
51 #include <boost/concept_check.hpp>
52 #include <string>
53 #include <map>
54 
55 namespace ompl
56 {
57  namespace base
58  {
60 
61  OMPL_CLASS_FORWARD(Planner);
63 
79  {
80  public:
82  PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
83  {
84  tempState_ = nullptr;
85  update();
86  }
87 
89  PlannerInputStates(const Planner *planner) : planner_(planner)
90  {
91  tempState_ = nullptr;
92  update();
93  }
94 
99  {
100  tempState_ = nullptr;
101  clear();
102  }
103 
106  {
107  clear();
108  }
109 
111  void clear();
112 
116  void restart();
117 
123  bool update();
124 
130  bool use(const ProblemDefinitionPtr &pdef);
131 
137  bool use(const ProblemDefinition *pdef);
138 
141  void checkValidity() const;
142 
145  const State *nextStart();
146 
155  const State *nextGoal(const PlannerTerminationCondition &ptc);
156 
158  const State *nextGoal();
159 
161  bool haveMoreStartStates() const;
162 
164  bool haveMoreGoalStates() const;
165 
169  unsigned int getSeenStartStatesCount() const
170  {
171  return addedStartStates_;
172  }
173 
175  unsigned int getSampledGoalsCount() const
176  {
177  return sampledGoalsCount_;
178  }
179 
180  private:
181  const Planner *planner_{nullptr};
182 
183  unsigned int addedStartStates_;
184  unsigned int sampledGoalsCount_;
185  State *tempState_;
186 
187  const ProblemDefinition *pdef_;
188  const SpaceInformation *si_;
189  };
190 
193  {
194  PlannerSpecs() = default;
195 
197  GoalType recognizedGoal{GOAL_ANY};
198 
200  bool multithreaded{false};
201 
203  bool approximateSolutions{false};
204 
207  bool optimizingPaths{false};
208 
213  bool directed{false};
214 
216  bool provingSolutionNonExistence{false};
217 
219  bool canReportIntermediateSolutions{false};
220  };
221 
223  class Planner
224  {
225  public:
226  // non-copyable
227  Planner(const Planner &) = delete;
228  Planner &operator=(const Planner &) = delete;
229 
231  Planner(SpaceInformationPtr si, std::string name);
232 
234  virtual ~Planner() = default;
235 
237  template <class T>
238  T *as()
239  {
241  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>));
242 
243  return static_cast<T *>(this);
244  }
245 
247  template <class T>
248  const T *as() const
249  {
251  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>));
252 
253  return static_cast<const T *>(this);
254  }
255 
257  const SpaceInformationPtr &getSpaceInformation() const;
258 
260  const ProblemDefinitionPtr &getProblemDefinition() const;
261 
263  const PlannerInputStates &getPlannerInputStates() const;
264 
269  virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
270 
283  virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0;
284 
287  PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
288 
292  PlannerStatus solve(double solveTime);
293 
297  virtual void clear();
298 
305  virtual void getPlannerData(PlannerData &data) const;
306 
308  const std::string &getName() const;
309 
311  void setName(const std::string &name);
312 
314  const PlannerSpecs &getSpecs() const;
315 
320  virtual void setup();
321 
326  virtual void checkValidity();
327 
329  bool isSetup() const;
330 
333  {
334  return params_;
335  }
336 
338  const ParamSet &params() const
339  {
340  return params_;
341  }
342 
345  typedef std::function<std::string()> PlannerProgressProperty;
346 
349  typedef std::map<std::string, PlannerProgressProperty> PlannerProgressProperties;
350 
352  const PlannerProgressProperties &getPlannerProgressProperties() const
353  {
354  return plannerProgressProperties_;
355  }
356 
358  virtual void printProperties(std::ostream &out) const;
359 
361  virtual void printSettings(std::ostream &out) const;
362 
363  protected:
366  template <typename T, typename PlannerType, typename SetterType, typename GetterType>
367  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
368  const GetterType &getter, const std::string &rangeSuggestion = "")
369  {
370  params_.declareParam<T>(name,
371  [planner, setter](T param)
372  {
373  (*planner.*setter)(param);
374  },
375  [planner, getter]
376  {
377  return (*planner.*getter)();
378  });
379  if (!rangeSuggestion.empty())
380  params_[name].setRangeSuggestion(rangeSuggestion);
381  }
382 
385  template <typename T, typename PlannerType, typename SetterType>
386  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
387  const std::string &rangeSuggestion = "")
388  {
389  params_.declareParam<T>(name, [planner, setter](T param)
390  {
391  (*planner.*setter)(param);
392  });
393  if (!rangeSuggestion.empty())
394  params_[name].setRangeSuggestion(rangeSuggestion);
395  }
396 
399  void addPlannerProgressProperty(const std::string &progressPropertyName,
400  const PlannerProgressProperty &prop)
401  {
402  plannerProgressProperties_[progressPropertyName] = prop;
403  }
404 
407 
410 
413 
415  std::string name_;
416 
419 
423 
426  PlannerProgressProperties plannerProgressProperties_;
427 
429  bool setup_;
430  };
431 
433  typedef std::function<PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator;
434  }
435 }
436 
437 #endif
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:399
Properties that planners may have.
Definition: Planner.h:192
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:367
A shared pointer wrapper for ompl::base::ProblemDefinition.
ParamSet & params()
Get the parameters for this planner.
Definition: Planner.h:332
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:409
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:175
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:335
Maintain a set of parameters.
Definition: GenericParam.h:226
PlannerInputStates(const Planner *planner)
Default constructor. No work is performed.
Definition: Planner.h:89
~PlannerInputStates()
Destructor. Clear allocated memory.
Definition: Planner.h:105
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:422
const ParamSet & params() const
Get the parameters for this planner.
Definition: Planner.h:338
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
PlannerInputStates(const PlannerPtr &planner)
Default constructor. No work is performed.
Definition: Planner.h:82
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A 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:257
Base class for a planner.
Definition: Planner.h:223
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:386
const T * as() const
Cast this instance to a desired type.
Definition: Planner.h:248
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 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:49
unsigned int getSeenStartStatesCount() const
Get the number of start states from the problem definition that were already seen, including invalid ones.
Definition: Planner.h:169
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
std::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:345
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
GoalType
The type of goal.
Definition: GoalTypes.h:45
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:342
std::string name_
The name of this planner.
Definition: Planner.h:415
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
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:352
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:433
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
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:349
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner&#39;s progress property names and the functions used for querying those pr...
Definition: Planner.h:426
PlannerInputStates()
Default constructor. No work is performed. A call to use() needs to be made, before making any calls ...
Definition: Planner.h:98
T * as()
Cast this instance to a desired type.
Definition: Planner.h:238
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:206
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