Loading...
Searching...
No Matches
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
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
180 const SpaceInformation *si_;
181 };
182
185 {
186 PlannerSpecs() = default;
187
190
192 bool multithreaded{false};
193
196
199 bool optimizingPaths{false};
200
205 bool directed{false};
206
209
212 };
213
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
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
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
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
Maintain a set of parameters.
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.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
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:339
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:228
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:258
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:167
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:346
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:161
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:216
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:342
T * as()
Cast this instance to a desired type.
Definition Planner.h:230
PlannerInputStates pis_
Utility class to extract valid input states
Definition Planner.h:416
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition Planner.h:426
ParamSet & params()
Get the parameters for this planner.
Definition Planner.h:336
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:349
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:403
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:390
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition Planner.h:422
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:371
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition Planner.h:413
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:419
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:240
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:410
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:430
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:433
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:353
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:356
A shared pointer wrapper for ompl::base::ProblemDefinition.
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:437
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:185
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition Planner.h:192
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition Planner.h:199
bool canReportIntermediateSolutions
Flag indicating whether the planner is able to report the computation of intermediate paths.
Definition Planner.h:211
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
Definition Planner.h:205
GoalType recognizedGoal
The type of goal specification the planner can use.
Definition Planner.h:189
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition Planner.h:195
bool provingSolutionNonExistence
Flag indicating whether the planner is able to prove that no solution path exists.
Definition Planner.h:208
A class to store the exit status of Planner::solve()