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
65
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
184 struct PlannerSpecs
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
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
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.
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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()