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
54namespace ompl
55{
56 namespace base
57 {
59
60 OMPL_CLASS_FORWARD(Planner);
62
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
192 {
193 PlannerSpecs() = default;
194
197
199 bool multithreaded{false};
200
203
206 bool optimizingPaths{false};
207
212 bool directed{false};
213
216
219 };
220
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
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
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
Maintain a set of parameters.
Definition: GenericParam.h:226
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:231
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
~PlannerInputStates()
Destructor. Clear allocated memory.
Definition: Planner.h:104
void clear()
Clear all stored information.
Definition: Planner.cpp:167
PlannerInputStates(const PlannerPtr &planner)
Default constructor. No work is performed.
Definition: Planner.h:81
PlannerInputStates()
Default constructor. No work is performed. A call to use() needs to be made, before making any calls ...
Definition: Planner.h:97
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:193
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:348
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:237
PlannerInputStates(const Planner *planner)
Default constructor. No work is performed.
Definition: Planner.h:88
const State * nextGoal()
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:267
bool update()
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:186
unsigned int getSampledGoalsCount() const
Get the number of sampled goal states, including invalid ones.
Definition: Planner.h:174
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
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:355
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again.
Definition: Planner.cpp:180
unsigned int getSeenStartStatesCount() const
Get the number of start states from the problem definition that were already seen,...
Definition: Planner.h:168
A shared pointer wrapper for ompl::base::Planner.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition: Planner.h:223
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
const ParamSet & params() const
Get the parameters for this planner.
Definition: Planner.h:349
T * as()
Cast this instance to a desired type.
Definition: Planner.h:237
PlannerInputStates pis_
Utility class to extract valid input states
Definition: Planner.h:423
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:433
ParamSet & params()
Get the parameters for this planner.
Definition: Planner.h:343
const PlannerInputStates & getPlannerInputStates() const
Get the planner input states.
Definition: Planner.cpp:87
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
Definition: Planner.cpp:161
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:356
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
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:410
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:397
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:429
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:378
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:420
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:118
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
std::string name_
The name of this planner.
Definition: Planner.h:426
const PlannerSpecs & getSpecs() const
Return the specifications (capabilities of this planner)
Definition: Planner.cpp:51
const T * as() const
Cast this instance to a desired type.
Definition: Planner.h:247
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:417
virtual void clearQuery()
Clears internal datastructures of any query-specific information from the previous query....
Definition: Planner.cpp:124
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
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:147
virtual ~Planner()=default
Destructor.
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner's progress property names and the functions used for querying those pr...
Definition: Planner.h:437
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:92
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:440
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:360
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
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
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 PlannerProgressProperties & getPlannerProgressProperties() const
Retrieve a planner's planner progress property map.
Definition: Planner.h:363
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:50
GoalType
The type of goal.
Definition: GoalTypes.h:46
@ GOAL_ANY
This bit is set if casting to generic goal regions (ompl::base::Goal) is possible....
Definition: GoalTypes.h:49
std::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Definition: Planner.h:444
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
Main namespace. Contains everything in this library.
Properties that planners may have.
Definition: Planner.h:192
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:199
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:206
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition: Planner.h:218
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition: Planner.h:212
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition: Planner.h:196
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:202
bool provingSolutionNonExistence
Flag indicating whether the planner is able to prove that no solution path exists.
Definition: Planner.h:215
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49