Planner.cpp
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#include "ompl/base/Planner.h"
38#include "ompl/util/Exception.h"
39#include "ompl/base/goals/GoalSampleableRegion.h"
40#include <sstream>
41#include <thread>
42#include <utility>
43
44ompl::base::Planner::Planner(SpaceInformationPtr si, std::string name)
45 : si_(std::move(si)), pis_(this), name_(std::move(name)), setup_(false)
46{
47 if (!si_)
48 throw Exception(name_, "Invalid space information instance for planner");
49}
50
52{
53 return specs_;
54}
55
56const std::string &ompl::base::Planner::getName() const
57{
58 return name_;
59}
60
61void ompl::base::Planner::setName(const std::string &name)
62{
63 name_ = name;
64}
65
67{
68 return si_;
69}
70
72{
73 return pdef_;
74}
75
77{
78 return pdef_;
79}
80
82{
83 pdef_ = pdef;
84 pis_.update();
85}
86
88{
89 return pis_;
90}
91
93{
94 if (!si_->isSetup())
95 {
96 OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str());
97 si_->setup();
98 }
99
100 if (setup_)
101 OMPL_WARN("%s: Planner setup called multiple times", getName().c_str());
102 else
103 setup_ = true;
104}
105
107{
108 if (!isSetup())
109 setup();
110 pis_.checkValidity();
111}
112
114{
115 return setup_;
116}
117
119{
120 pis_.clear();
121 pis_.update();
122}
123
125{
126 clear();
127}
128
130{
131 for (const auto &plannerProgressProperty : plannerProgressProperties_)
132 data.properties[plannerProgressProperty.first] = plannerProgressProperty.second();
133}
134
136{
137 return solve(PlannerTerminationCondition(ptc, checkInterval));
138}
139
141{
142 if (solveTime < 1.0)
143 return solve(timedPlannerTerminationCondition(solveTime));
144 return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)));
145}
146
147void ompl::base::Planner::printProperties(std::ostream &out) const
148{
149 out << "Planner " + getName() + " specs:" << std::endl;
150 out << "Multithreaded: " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl;
151 out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl;
152 out << "Can optimize solutions: " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl;
153 out << "Aware of the following parameters:";
154 std::vector<std::string> params;
155 params_.getParamNames(params);
156 for (auto &param : params)
157 out << " " << param;
158 out << std::endl;
159}
160
161void ompl::base::Planner::printSettings(std::ostream &out) const
162{
163 out << "Declared parameters for planner " << getName() << ":" << std::endl;
164 params_.print(out);
165}
166
168{
169 if (tempState_ != nullptr)
170 {
171 si_->freeState(tempState_);
172 tempState_ = nullptr;
173 }
174 addedStartStates_ = 0;
175 sampledGoalsCount_ = 0;
176 pdef_ = nullptr;
177 si_ = nullptr;
178}
179
181{
182 addedStartStates_ = 0;
183 sampledGoalsCount_ = 0;
184}
185
187{
188 if (planner_ == nullptr)
189 throw Exception("No planner set for PlannerInputStates");
190 return use(planner_->getProblemDefinition());
191}
192
194{
195 std::string error;
196
197 if (pdef_ == nullptr)
198 error = "Problem definition not specified";
199 else
200 {
201 if (pdef_->getStartStateCount() <= 0)
202 error = "No start states specified";
203 else if (!pdef_->getGoal())
204 error = "No goal specified";
205 }
206
207 if (!error.empty())
208 {
209 if (planner_ != nullptr)
210 throw Exception(planner_->getName(), error);
211 else
212 throw Exception(error);
213 }
214}
215
217{
218 if (pdef)
219 return use(pdef.get());
220
221 clear();
222 return true;
223}
224
226{
227 if (pdef_ != pdef)
228 {
229 clear();
230 pdef_ = pdef;
231 si_ = pdef->getSpaceInformation().get();
232 return true;
233 }
234 return false;
235}
236
238{
239 if (pdef_ == nullptr || si_ == nullptr)
240 {
241 std::string error = "Missing space information or problem definition";
242 if (planner_ != nullptr)
243 throw Exception(planner_->getName(), error);
244 else
245 throw Exception(error);
246 }
247
248 while (addedStartStates_ < pdef_->getStartStateCount())
249 {
250 const base::State *st = pdef_->getStartState(addedStartStates_);
251 addedStartStates_++;
252 bool bounds = si_->satisfiesBounds(st);
253 bool valid = bounds ? si_->isValid(st) : false;
254 if (bounds && valid)
255 return st;
256
257 OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
258 planner_ ? planner_->getName().c_str() : "PlannerInputStates", bounds ? "state" : "bounds");
259 std::stringstream ss;
260 si_->printState(st, ss);
261 OMPL_DEBUG("%s: Discarded start state %s", planner_ ? planner_->getName().c_str() : "PlannerInputStates",
262 ss.str().c_str());
263 }
264 return nullptr;
265}
266
268{
269 // This initialization is safe since we are in a non-const function anyway.
271 return nextGoal(ptc);
272}
273
275{
276 if (pdef_ == nullptr || si_ == nullptr)
277 {
278 std::string error = "Missing space information or problem definition";
279 if (planner_ != nullptr)
280 throw Exception(planner_->getName(), error);
281 else
282 throw Exception(error);
283 }
284
285 if (pdef_->getGoal() != nullptr)
286 {
287 const GoalSampleableRegion *goal =
288 pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;
289
290 if (goal != nullptr)
291 {
292 time::point start_wait;
293 bool first = true;
294 bool attempt = true;
295 while (attempt)
296 {
297 attempt = false;
298
299 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
300 {
301 if (tempState_ == nullptr)
302 tempState_ = si_->allocState();
303 do
304 {
305 goal->sampleGoal(tempState_);
306 sampledGoalsCount_++;
307 bool bounds = si_->satisfiesBounds(tempState_);
308 bool valid = bounds ? si_->isValid(tempState_) : false;
309 if (bounds && valid)
310 {
311 if (!first) // if we waited, show how long
312 {
313 OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
314 planner_ ? planner_->getName().c_str() : "PlannerInputStates",
315 time::seconds(time::now() - start_wait));
316 }
317 return tempState_;
318 }
319
320 OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
321 planner_ ? planner_->getName().c_str() : "PlannerInputStates",
322 bounds ? "state" : "bounds");
323 std::stringstream ss;
324 si_->printState(tempState_, ss);
325 OMPL_DEBUG("%s: Discarded goal state:\n%s",
326 planner_ ? planner_->getName().c_str() : "PlannerInputStates", ss.str().c_str());
327 } while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
328 }
329 if (goal->couldSample() && !ptc)
330 {
331 if (first)
332 {
333 first = false;
334 start_wait = time::now();
335 OMPL_DEBUG("%s: Waiting for goal region samples ...",
336 planner_ ? planner_->getName().c_str() : "PlannerInputStates");
337 }
338 std::this_thread::sleep_for(time::seconds(0.01));
339 attempt = !ptc;
340 }
341 }
342 }
343 }
344
345 return nullptr;
346}
347
349{
350 if (pdef_ != nullptr)
351 return addedStartStates_ < pdef_->getStartStateCount();
352 return false;
353}
354
356{
357 if ((pdef_ != nullptr) && pdef_->getGoal())
358 if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
359 return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
360 return false;
361}
The exception type for ompl.
Definition: Exception.h:47
Abstract definition of a goal region that can be sampled.
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future....
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
Definition: Goal.h:102
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:410
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
void clear()
Clear all stored information.
Definition: Planner.cpp:167
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
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
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
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
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
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
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
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 void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:92
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...
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:56
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:52
point now()
Get the current time point.
Definition: Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:64
STL namespace.
Properties that planners may have.
Definition: Planner.h:192
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49