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
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.
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: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()