Loading...
Searching...
No Matches
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
70
75
80
82{
83 pdef_ = pdef;
84 pis_.update();
85}
86
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_.reset();
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 && pdef_ != pdef)
219 {
220 clear();
221 pdef_ = pdef;
222 si_ = pdef->getSpaceInformation().get();
223 return true;
224 }
225 return false;
226}
227
229{
230 if (pdef_ == nullptr || si_ == nullptr)
231 {
232 std::string error = "Missing space information or problem definition";
233 if (planner_ != nullptr)
234 throw Exception(planner_->getName(), error);
235 else
236 throw Exception(error);
237 }
238
239 while (addedStartStates_ < pdef_->getStartStateCount())
240 {
241 const base::State *st = pdef_->getStartState(addedStartStates_);
242 addedStartStates_++;
243 bool bounds = si_->satisfiesBounds(st);
244 bool valid = bounds ? si_->isValid(st) : false;
245 if (bounds && valid)
246 return st;
247
248 OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
249 planner_ ? planner_->getName().c_str() : "PlannerInputStates", bounds ? "state" : "bounds");
250 std::stringstream ss;
251 si_->printState(st, ss);
252 OMPL_DEBUG("%s: Discarded start state %s", planner_ ? planner_->getName().c_str() : "PlannerInputStates",
253 ss.str().c_str());
254 }
255 return nullptr;
256}
257
259{
260 // This initialization is safe since we are in a non-const function anyway.
262 return nextGoal(ptc);
263}
264
266{
267 if (pdef_ == nullptr || si_ == nullptr)
268 {
269 std::string error = "Missing space information or problem definition";
270 if (planner_ != nullptr)
271 throw Exception(planner_->getName(), error);
272 else
273 throw Exception(error);
274 }
275
276 if (pdef_->getGoal() != nullptr)
277 {
278 const GoalSampleableRegion *goal =
279 pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;
280
281 if (goal != nullptr)
282 {
283 time::point start_wait;
284 bool first = true;
285 bool attempt = true;
286 while (attempt)
287 {
288 attempt = false;
289
290 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
291 {
292 if (tempState_ == nullptr)
293 tempState_ = si_->allocState();
294 do
295 {
296 goal->sampleGoal(tempState_);
297 sampledGoalsCount_++;
298 bool bounds = si_->satisfiesBounds(tempState_);
299 bool valid = bounds ? si_->isValid(tempState_) : false;
300 if (bounds && valid)
301 {
302 if (!first) // if we waited, show how long
303 {
304 OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
305 planner_ ? planner_->getName().c_str() : "PlannerInputStates",
306 time::seconds(time::now() - start_wait));
307 }
308 return tempState_;
309 }
310
311 OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
312 planner_ ? planner_->getName().c_str() : "PlannerInputStates",
313 bounds ? "state" : "bounds");
314 std::stringstream ss;
315 si_->printState(tempState_, ss);
316 OMPL_DEBUG("%s: Discarded goal state:\n%s",
317 planner_ ? planner_->getName().c_str() : "PlannerInputStates", ss.str().c_str());
318 } while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
319 }
320 if (goal->couldSample() && !ptc)
321 {
322 if (first)
323 {
324 first = false;
325 start_wait = time::now();
326 OMPL_DEBUG("%s: Waiting for goal region samples ...",
327 planner_ ? planner_->getName().c_str() : "PlannerInputStates");
328 }
329 std::this_thread::sleep_for(time::seconds(0.01));
330 attempt = !ptc;
331 }
332 }
333 }
334 }
335
336 return nullptr;
337}
338
340{
341 if (pdef_ != nullptr)
342 return addedStartStates_ < pdef_->getStartStateCount();
343 return false;
344}
345
347{
348 if ((pdef_ != nullptr) && pdef_->getGoal())
349 if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
350 return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
351 return false;
352}
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,...
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
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:339
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition Planner.cpp:228
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
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
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:419
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: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 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.
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:185
A class to store the exit status of Planner::solve()