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 <functional>
50 #include <boost/concept_check.hpp>
51 #include <string>
52 #include <map>
53 
54 namespace ompl
55 {
56  namespace base
57  {
59 
60  OMPL_CLASS_FORWARD(Planner);
62 
77  class PlannerInputStates
78  {
79  public:
81  PlannerInputStates(const PlannerPtr &planner) : planner_(planner.get())
82  {
83  tempState_ = nullptr;
84  update();
85  }
86 
88  PlannerInputStates(const Planner *planner) : planner_(planner)
89  {
90  tempState_ = nullptr;
91  update();
92  }
93 
98  {
99  tempState_ = nullptr;
100  clear();
101  }
102 
105  {
106  clear();
107  }
108 
110  void clear();
111 
115  void restart();
116 
122  bool update();
123 
129  bool use(const ProblemDefinitionPtr &pdef);
130 
133  void checkValidity() const;
134 
137  const State *nextStart();
138 
147  const State *nextGoal(const PlannerTerminationCondition &ptc);
148 
150  const State *nextGoal();
151 
153  bool haveMoreStartStates() const;
154 
156  bool haveMoreGoalStates() const;
157 
161  unsigned int getSeenStartStatesCount() const
162  {
163  return addedStartStates_;
164  }
165 
167  unsigned int getSampledGoalsCount() const
168  {
169  return sampledGoalsCount_;
170  }
171 
172  private:
173  const Planner *planner_{nullptr};
174 
175  unsigned int addedStartStates_;
176  unsigned int sampledGoalsCount_;
177  State *tempState_;
178 
179  ProblemDefinitionPtr pdef_;
180  const SpaceInformation *si_;
181  };
182 
184  struct PlannerSpecs
185  {
186  PlannerSpecs() = default;
187 
190 
192  bool multithreaded{false};
193 
195  bool approximateSolutions{false};
196 
199  bool optimizingPaths{false};
200 
205  bool directed{false};
206 
208  bool provingSolutionNonExistence{false};
209 
211  bool canReportIntermediateSolutions{false};
212  };
213 
215  class Planner
216  {
217  public:
218  // non-copyable
219  Planner(const Planner &) = delete;
220  Planner &operator=(const Planner &) = delete;
221 
223  Planner(SpaceInformationPtr si, std::string name);
224 
226  virtual ~Planner() = default;
227 
229  template <class T>
230  T *as()
231  {
233  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>));
234 
235  return static_cast<T *>(this);
236  }
237 
239  template <class T>
240  const T *as() const
241  {
243  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>));
244 
245  return static_cast<const T *>(this);
246  }
247 
250 
253 
256 
259 
264  virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
265 
278  virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0;
279 
282  PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
283 
287  PlannerStatus solve(double solveTime);
288 
292  virtual void clear();
293 
301  virtual void clearQuery();
302 
309  virtual void getPlannerData(PlannerData &data) const;
310 
312  const std::string &getName() const;
313 
315  void setName(const std::string &name);
316 
318  const PlannerSpecs &getSpecs() const;
319 
324  virtual void setup();
325 
330  virtual void checkValidity();
331 
333  bool isSetup() const;
334 
336  ParamSet &params()
337  {
338  return params_;
339  }
340 
342  const ParamSet &params() const
343  {
344  return params_;
345  }
346 
349  using PlannerProgressProperty = std::function<std::string()>;
350 
353  using PlannerProgressProperties = std::map<std::string, PlannerProgressProperty>;
354 
357  {
359  }
360 
362  virtual void printProperties(std::ostream &out) const;
363 
365  virtual void printSettings(std::ostream &out) const;
366 
367  protected:
370  template <typename T, typename PlannerType, typename SetterType, typename GetterType>
371  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
372  const GetterType &getter, const std::string &rangeSuggestion = "")
373  {
374  params_.declareParam<T>(name,
375  [planner, setter](T param)
376  {
377  (*planner.*setter)(param);
378  },
379  [planner, getter]
380  {
381  return (*planner.*getter)();
382  });
383  if (!rangeSuggestion.empty())
384  params_[name].setRangeSuggestion(rangeSuggestion);
385  }
386 
389  template <typename T, typename PlannerType, typename SetterType>
390  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
391  const std::string &rangeSuggestion = "")
392  {
393  params_.declareParam<T>(name, [planner, setter](T param)
394  {
395  (*planner.*setter)(param);
396  });
397  if (!rangeSuggestion.empty())
398  params_[name].setRangeSuggestion(rangeSuggestion);
399  }
400 
403  void addPlannerProgressProperty(const std::string &progressPropertyName,
404  const PlannerProgressProperty &prop)
405  {
406  plannerProgressProperties_[progressPropertyName] = prop;
407  }
408 
411 
414 
417 
419  std::string name_;
420 
423 
427 
431 
433  bool setup_;
434  };
435 
437  using PlannerAllocator = std::function<PlannerPtr(const SpaceInformationPtr &)>;
438  }
439 }
440 
441 #endif
~PlannerInputStates()
Destructor. Clear allocated memory.
Definition: Planner.h:168
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:339
Base class for a planner.
Definition: Planner.h:279
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again.
Definition: Planner.cpp:180
Maintain a set of parameters.
Definition: GenericParam.h:289
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
unsigned int getSeenStartStatesCount() const
Get the number of start states from the problem definition that were already seen,...
Definition: Planner.h:225
A shared pointer wrapper for ompl::base::SpaceInformation.
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:256
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:141
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
T * as()
Cast this instance to a desired type.
Definition: Planner.h:294
const PlannerSpecs & getSpecs() const
Return the specifications (capabilities of this planner)
Definition: Planner.cpp:51
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:417
bool provingSolutionNonExistence
Flag indicating whether the planner is able to prove that no solution path exists.
Definition: Planner.h:272
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
Definition: Planner.cpp:161
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:467
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:477
ParamSet & params()
Get the parameters for this planner.
Definition: Planner.h:400
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:92
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:490
Properties that planners may have.
Definition: Planner.h:248
A shared pointer wrapper for ompl::base::Planner.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
virtual void checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set,...
Definition: Planner.cpp:106
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:435
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:231
std::string name_
The name of this planner.
Definition: Planner.h:483
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:228
const PlannerInputStates & getPlannerInputStates() const
Get the planner input states.
Definition: Planner.cpp:87
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:486
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:269
virtual ~Planner()=default
Destructor.
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:263
std::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:413
A class to store the exit status of Planner::solve()
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:501
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition: Planner.h:275
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
A shared pointer wrapper for ompl::base::ProblemDefinition.
virtual void clearQuery()
Clears internal datastructures of any query-specific information from the previous query....
Definition: Planner.cpp:124
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:193
const PlannerProgressProperties & getPlannerProgressProperties() const
Retrieve a planner's planner progress property map.
Definition: Planner.h:420
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:497
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:216
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:118
const State * nextGoal()
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:258
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:81
@ GOAL_ANY
This bit is set if casting to generic goal regions (ompl::base::Goal) is possible....
Definition: GoalTypes.h:145
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:129
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:480
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
void clear()
Clear all stored information.
Definition: Planner.cpp:167
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
PlannerInputStates()
Default constructor. No work is performed. A call to use() needs to be made, before making any calls ...
Definition: Planner.h:161
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=[] { return T();})
This function declares a parameter name, and specifies the setter and getter functions.
Definition: GenericParam.h:295
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:346
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:259
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:147
GoalType
The type of goal.
Definition: GoalTypes.h:109
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner's progress property names and the functions used for querying those pr...
Definition: Planner.h:494
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:253
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
bool update()
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:186
Main namespace. Contains everything in this library.
Definition: AppBase.h:21