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.
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