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 
136  bool use(const ProblemDefinition *pdef);
137 
140  void checkValidity() const;
141 
144  const State *nextStart();
145 
154  const State *nextGoal(const PlannerTerminationCondition &ptc);
155 
157  const State *nextGoal();
158 
160  bool haveMoreStartStates() const;
161 
163  bool haveMoreGoalStates() const;
164 
168  unsigned int getSeenStartStatesCount() const
169  {
170  return addedStartStates_;
171  }
172 
174  unsigned int getSampledGoalsCount() const
175  {
176  return sampledGoalsCount_;
177  }
178 
179  private:
180  const Planner *planner_{nullptr};
181 
182  unsigned int addedStartStates_;
183  unsigned int sampledGoalsCount_;
184  State *tempState_;
185 
186  const ProblemDefinition *pdef_;
187  const SpaceInformation *si_;
188  };
189 
191  struct PlannerSpecs
192  {
193  PlannerSpecs() = default;
194 
197 
199  bool multithreaded{false};
200 
202  bool approximateSolutions{false};
203 
206  bool optimizingPaths{false};
207 
212  bool directed{false};
213 
215  bool provingSolutionNonExistence{false};
216 
218  bool canReportIntermediateSolutions{false};
219  };
220 
222  class Planner
223  {
224  public:
225  // non-copyable
226  Planner(const Planner &) = delete;
227  Planner &operator=(const Planner &) = delete;
228 
230  Planner(SpaceInformationPtr si, std::string name);
231 
233  virtual ~Planner() = default;
234 
236  template <class T>
237  T *as()
238  {
240  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>));
241 
242  return static_cast<T *>(this);
243  }
244 
246  template <class T>
247  const T *as() const
248  {
250  BOOST_CONCEPT_ASSERT((boost::Convertible<T *, Planner *>));
251 
252  return static_cast<const T *>(this);
253  }
254 
257 
260 
263 
266 
271  virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef);
272 
285  virtual PlannerStatus solve(const PlannerTerminationCondition &ptc) = 0;
286 
289  PlannerStatus solve(const PlannerTerminationConditionFn &ptc, double checkInterval);
290 
294  PlannerStatus solve(double solveTime);
295 
299  virtual void clear();
300 
308  virtual void clearQuery();
309 
316  virtual void getPlannerData(PlannerData &data) const;
317 
319  const std::string &getName() const;
320 
322  void setName(const std::string &name);
323 
325  const PlannerSpecs &getSpecs() const;
326 
331  virtual void setup();
332 
337  virtual void checkValidity();
338 
340  bool isSetup() const;
341 
343  ParamSet &params()
344  {
345  return params_;
346  }
347 
349  const ParamSet &params() const
350  {
351  return params_;
352  }
353 
356  using PlannerProgressProperty = std::function<std::string()>;
357 
360  using PlannerProgressProperties = std::map<std::string, PlannerProgressProperty>;
361 
364  {
366  }
367 
369  virtual void printProperties(std::ostream &out) const;
370 
372  virtual void printSettings(std::ostream &out) const;
373 
374  protected:
377  template <typename T, typename PlannerType, typename SetterType, typename GetterType>
378  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
379  const GetterType &getter, const std::string &rangeSuggestion = "")
380  {
381  params_.declareParam<T>(name,
382  [planner, setter](T param)
383  {
384  (*planner.*setter)(param);
385  },
386  [planner, getter]
387  {
388  return (*planner.*getter)();
389  });
390  if (!rangeSuggestion.empty())
391  params_[name].setRangeSuggestion(rangeSuggestion);
392  }
393 
396  template <typename T, typename PlannerType, typename SetterType>
397  void declareParam(const std::string &name, const PlannerType &planner, const SetterType &setter,
398  const std::string &rangeSuggestion = "")
399  {
400  params_.declareParam<T>(name, [planner, setter](T param)
401  {
402  (*planner.*setter)(param);
403  });
404  if (!rangeSuggestion.empty())
405  params_[name].setRangeSuggestion(rangeSuggestion);
406  }
407 
410  void addPlannerProgressProperty(const std::string &progressPropertyName,
411  const PlannerProgressProperty &prop)
412  {
413  plannerProgressProperties_[progressPropertyName] = prop;
414  }
415 
418 
421 
424 
426  std::string name_;
427 
430 
434 
438 
440  bool setup_;
441  };
442 
444  using PlannerAllocator = std::function<PlannerPtr(const SpaceInformationPtr &)>;
445  }
446 }
447 
448 #endif
~PlannerInputStates()
Destructor. Clear allocated memory.
Definition: Planner.h:168
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
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:290
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:232
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:263
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:142
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:301
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:424
bool provingSolutionNonExistence
Flag indicating whether the planner is able to prove that no solution path exists.
Definition: Planner.h:279
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
Definition: Planner.cpp:161
Definition of an abstract state.
Definition: State.h:114
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:474
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:484
ParamSet & params()
Get the parameters for this planner.
Definition: Planner.h:407
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:497
Properties that planners may have.
Definition: Planner.h:256
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:239
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:442
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:238
std::string name_
The name of this planner.
Definition: Planner.h:490
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:237
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:493
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:276
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:270
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:420
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:508
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition: Planner.h:282
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
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:427
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:504
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:267
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:487
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:481
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:355
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:266
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:110
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:501
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:260
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:22