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 
98  PlannerInputStates() : planner_(nullptr)
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_;
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()
195  : recognizedGoal(GOAL_ANY)
196  , multithreaded(false)
197  , approximateSolutions(false)
198  , optimizingPaths(false)
199  , directed(false)
200  , provingSolutionNonExistence(false)
201  , canReportIntermediateSolutions(false)
202  {
203  }
204 
207 
210 
213 
217 
222  bool directed;
223 
226 
229  };
230 
232  class Planner
233  {
234  public:
235  // non-copyable
236  Planner(const Planner &) = delete;
237  Planner &operator=(const Planner &) = delete;
238 
240  Planner(SpaceInformationPtr si, std::string name);
241 
243  virtual ~Planner() = default;
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 
354  typedef std::function<std::string()> PlannerProgressProperty;
355 
358  typedef std::map<std::string, PlannerProgressProperty> PlannerProgressProperties;
359 
361  const PlannerProgressProperties &getPlannerProgressProperties() const
362  {
363  return plannerProgressProperties_;
364  }
365 
367  virtual void printProperties(std::ostream &out) const;
368 
370  virtual void printSettings(std::ostream &out) const;
371 
372  protected:
375  template <typename T, typename PlannerType, typename SetterType, typename GetterType>
376  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
377  const GetterType &getter, const std::string &rangeSuggestion = "")
378  {
379  params_.declareParam<T>(name,
380  [planner, setter](T param)
381  {
382  (*planner.*setter)(param);
383  },
384  [planner, getter]
385  {
386  return (*planner.*getter)();
387  });
388  if (!rangeSuggestion.empty())
389  params_[name].setRangeSuggestion(rangeSuggestion);
390  }
391 
394  template <typename T, typename PlannerType, typename SetterType>
395  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
396  const std::string &rangeSuggestion = "")
397  {
398  params_.declareParam<T>(name, [planner, setter](T param)
399  {
400  (*planner.*setter)(param);
401  });
402  if (!rangeSuggestion.empty())
403  params_[name].setRangeSuggestion(rangeSuggestion);
404  }
405 
408  void addPlannerProgressProperty(const std::string &progressPropertyName,
409  const PlannerProgressProperty &prop)
410  {
411  plannerProgressProperties_[progressPropertyName] = prop;
412  }
413 
416 
419 
422 
424  std::string name_;
425 
428 
432 
435  PlannerProgressProperties plannerProgressProperties_;
436 
438  bool setup_;
439  };
440 
442  typedef std::function<PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator;
443  }
444 }
445 
446 #endif
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
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:408
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:376
A shared pointer wrapper for ompl::base::ProblemDefinition.
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:206
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...
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths...
Definition: Planner.h:228
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
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:222
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:342
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:431
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:438
PlannerInputStates(const PlannerPtr &planner)
Default constructor. No work is performed.
Definition: Planner.h:82
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: 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:262
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:395
const T * as() const
Cast this instance to a desired type.
Definition: Planner.h:257
void clear()
Clear all stored information.
Definition: Planner.cpp:158
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:421
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:354
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
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:177
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:349
std::string name_
The name of this planner.
Definition: Planner.h:424
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:78
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:171
const PlannerProgressProperties & getPlannerProgressProperties() const
Retrieve a planner&#39;s planner progress property map.
Definition: Planner.h:361
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:442
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
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:358
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner&#39;s progress property names and the functions used for querying those pr...
Definition: Planner.h:435
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:247
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:184
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
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