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  pdef_ = pdef;
79  pis_.update();
80 }
81 
83 {
84  return pis_;
85 }
86 
88 {
89  if (!si_->isSetup())
90  {
91  OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str());
92  si_->setup();
93  }
94 
95  if (setup_)
96  OMPL_WARN("%s: Planner setup called multiple times", getName().c_str());
97  else
98  setup_ = true;
99 }
100 
102 {
103  if (!isSetup())
104  setup();
106 }
107 
109 {
110  return setup_;
111 }
112 
114 {
115  pis_.clear();
116  pis_.update();
117 }
118 
120 {
121  for (const auto &plannerProgressProperty : plannerProgressProperties_)
122  data.properties[plannerProgressProperty.first] = plannerProgressProperty.second();
123 }
124 
126 {
127  return solve(PlannerTerminationCondition(ptc, checkInterval));
128 }
129 
131 {
132  if (solveTime < 1.0)
133  return solve(timedPlannerTerminationCondition(solveTime));
134  else
135  return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)));
136 }
137 
138 void ompl::base::Planner::printProperties(std::ostream &out) const
139 {
140  out << "Planner " + getName() + " specs:" << std::endl;
141  out << "Multithreaded: " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl;
142  out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl;
143  out << "Can optimize solutions: " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl;
144  out << "Aware of the following parameters:";
145  std::vector<std::string> params;
146  params_.getParamNames(params);
147  for (auto &param : params)
148  out << " " << param;
149  out << std::endl;
150 }
151 
152 void ompl::base::Planner::printSettings(std::ostream &out) const
153 {
154  out << "Declared parameters for planner " << getName() << ":" << std::endl;
155  params_.print(out);
156 }
157 
159 {
160  if (tempState_)
161  {
162  si_->freeState(tempState_);
163  tempState_ = nullptr;
164  }
165  addedStartStates_ = 0;
166  sampledGoalsCount_ = 0;
167  pdef_ = nullptr;
168  si_ = nullptr;
169 }
170 
172 {
173  addedStartStates_ = 0;
174  sampledGoalsCount_ = 0;
175 }
176 
178 {
179  if (!planner_)
180  throw Exception("No planner set for PlannerInputStates");
181  return use(planner_->getProblemDefinition());
182 }
183 
185 {
186  std::string error;
187 
188  if (!pdef_)
189  error = "Problem definition not specified";
190  else
191  {
192  if (pdef_->getStartStateCount() <= 0)
193  error = "No start states specified";
194  else if (!pdef_->getGoal())
195  error = "No goal specified";
196  }
197 
198  if (!error.empty())
199  {
200  if (planner_)
201  throw Exception(planner_->getName(), error);
202  else
203  throw Exception(error);
204  }
205 }
206 
208 {
209  if (pdef)
210  return use(pdef.get());
211  else
212  {
213  clear();
214  return true;
215  }
216 }
217 
219 {
220  if (pdef_ != pdef)
221  {
222  clear();
223  pdef_ = pdef;
224  si_ = pdef->getSpaceInformation().get();
225  return true;
226  }
227  return false;
228 }
229 
231 {
232  if (pdef_ == nullptr || si_ == nullptr)
233  {
234  std::string error = "Missing space information or problem definition";
235  if (planner_)
236  throw Exception(planner_->getName(), error);
237  else
238  throw Exception(error);
239  }
240 
241  while (addedStartStates_ < pdef_->getStartStateCount())
242  {
243  const base::State *st = pdef_->getStartState(addedStartStates_);
244  addedStartStates_++;
245  bool bounds = si_->satisfiesBounds(st);
246  bool valid = bounds ? si_->isValid(st) : false;
247  if (bounds && valid)
248  return st;
249  else
250  {
251  OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
252  planner_ ? planner_->getName().c_str() : "PlannerInputStates", bounds ? "state" : "bounds");
253  std::stringstream ss;
254  si_->printState(st, ss);
255  OMPL_DEBUG("%s: Discarded start state %s", planner_ ? planner_->getName().c_str() : "PlannerInputStates",
256  ss.str().c_str());
257  }
258  }
259  return nullptr;
260 }
261 
263 {
264  // This initialization is safe since we are in a non-const function anyway.
266  return nextGoal(ptc);
267 }
268 
270 {
271  if (pdef_ == nullptr || si_ == nullptr)
272  {
273  std::string error = "Missing space information or problem definition";
274  if (planner_)
275  throw Exception(planner_->getName(), error);
276  else
277  throw Exception(error);
278  }
279 
280  const GoalSampleableRegion *goal =
281  pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;
282 
283  if (goal)
284  {
285  time::point start_wait;
286  bool first = true;
287  bool attempt = true;
288  while (attempt)
289  {
290  attempt = false;
291 
292  if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
293  {
294  if (tempState_ == nullptr)
295  tempState_ = si_->allocState();
296  do
297  {
298  goal->sampleGoal(tempState_);
299  sampledGoalsCount_++;
300  bool bounds = si_->satisfiesBounds(tempState_);
301  bool valid = bounds ? si_->isValid(tempState_) : false;
302  if (bounds && valid)
303  {
304  if (!first) // if we waited, show how long
305  {
306  OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
307  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
308  time::seconds(time::now() - start_wait));
309  }
310  return tempState_;
311  }
312  else
313  {
314  OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
315  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
316  bounds ? "state" : "bounds");
317  std::stringstream ss;
318  si_->printState(tempState_, ss);
319  OMPL_DEBUG("%s: Discarded goal state:\n%s",
320  planner_ ? planner_->getName().c_str() : "PlannerInputStates", ss.str().c_str());
321  }
322  } while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
323  }
324  if (goal->couldSample() && !ptc)
325  {
326  if (first)
327  {
328  first = false;
329  start_wait = time::now();
330  OMPL_DEBUG("%s: Waiting for goal region samples ...",
331  planner_ ? planner_->getName().c_str() : "PlannerInputStates");
332  }
333  std::this_thread::sleep_for(time::seconds(0.01));
334  attempt = !ptc;
335  }
336  }
337  }
338 
339  return nullptr;
340 }
341 
343 {
344  if (pdef_)
345  return addedStartStates_ < pdef_->getStartStateCount();
346  return false;
347 }
348 
350 {
351  if (pdef_ && pdef_->getGoal())
352  if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
353  return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
354  return false;
355 }
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:212
Properties that planners may have.
Definition: Planner.h:192
A shared pointer wrapper for ompl::base::ProblemDefinition.
virtual bool couldSample() const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
const PlannerInputStates & getPlannerInputStates() const
Get the planner input states.
Definition: Planner.cpp:82
ParamSet & params()
Get the parameters for this planner.
Definition: Planner.h:341
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
STL namespace.
std::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:113
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:418
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:209
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
const PlannerSpecs & getSpecs() const
Return the specifications (capabilities of this planner)
Definition: Planner.cpp:51
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:342
void getParamNames(std::vector< std::string > &params) const
List the names of the known parameters.
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met...
virtual void printSettings(std::ostream &out) const
Print information about the motion planner&#39;s settings.
Definition: Planner.cpp:152
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:431
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:438
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Abstract definition of a goal region that can be sampled.
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:119
void print(std::ostream &out) const
Print the parameters to a stream.
const State * nextGoal()
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:262
bool canSample() const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
void clear()
Clear all stored information.
Definition: Planner.cpp:158
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:87
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
const ProblemDefinitionPtr & getProblemDefinition() const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:71
A shared pointer wrapper for ompl::base::SpaceInformation.
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:76
Definition of an abstract state.
Definition: State.h:49
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:101
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:421
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:427
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:230
virtual PlannerStatus solve(const PlannerTerminationCondition &ptc)=0
Function that can solve the motion planning problem. This function can be called multiple times on th...
The exception type for ompl.
Definition: Exception.h:46
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
bool update()
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:177
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:349
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:56
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:108
point now()
Get the current time point.
Definition: Time.h:70
std::string name_
The name of this planner.
Definition: Planner.h:424
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:216
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:78
void restart()
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:171
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:138
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this problem definition is for.
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner&#39;s progress property names and the functions used for querying those pr...
Definition: Planner.h:435
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:184
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:207
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:406
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:56
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68