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 
44 ompl::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 
56 const std::string &ompl::base::Planner::getName() const
57 {
58  return name_;
59 }
60 
61 void 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 
147 void 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 
161 void 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 }
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met.
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:339
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again.
Definition: Planner.cpp:180
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time)
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner,...
A shared pointer wrapper for ompl::base::SpaceInformation.
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:141
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
const PlannerSpecs & getSpecs() const
Return the specifications (capabilities of this planner)
Definition: Planner.cpp:51
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
Definition: Planner.cpp:161
Definition of an abstract state.
Definition: State.h:113
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future....
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
point now()
Get the current time point.
Definition: Time.h:122
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:92
Properties that planners may have.
Definition: Planner.h:248
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
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
std::string name_
The name of this planner.
Definition: Planner.h:483
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:228
const PlannerInputStates & getPlannerInputStates() const
Get the planner input states.
Definition: Planner.cpp:87
A class to store the exit status of Planner::solve()
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.
virtual void clearQuery()
Clears internal datastructures of any query-specific information from the previous query....
Definition: Planner.cpp:124
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
Definition: Goal.h:166
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:193
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:118
const State * nextGoal()
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:258
@ GOAL_SAMPLEABLE_REGION
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible.
Definition: GoalTypes.h:152
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:474
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
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:113
void clear()
Clear all stored information.
Definition: Planner.cpp:167
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:346
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:147
Abstract definition of a goal region that can be sampled.
The exception type for ompl.
Definition: Exception.h:78
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
bool update()
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:186