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_ = 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
Properties that planners may have.
Definition: Planner.h:192
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49