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();
105  pis_.checkValidity();
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  return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)));
135 }
136 
137 void ompl::base::Planner::printProperties(std::ostream &out) const
138 {
139  out << "Planner " + getName() + " specs:" << std::endl;
140  out << "Multithreaded: " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl;
141  out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl;
142  out << "Can optimize solutions: " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl;
143  out << "Aware of the following parameters:";
144  std::vector<std::string> params;
145  params_.getParamNames(params);
146  for (auto &param : params)
147  out << " " << param;
148  out << std::endl;
149 }
150 
151 void ompl::base::Planner::printSettings(std::ostream &out) const
152 {
153  out << "Declared parameters for planner " << getName() << ":" << std::endl;
154  params_.print(out);
155 }
156 
158 {
159  if (tempState_ != nullptr)
160  {
161  si_->freeState(tempState_);
162  tempState_ = nullptr;
163  }
164  addedStartStates_ = 0;
165  sampledGoalsCount_ = 0;
166  pdef_ = nullptr;
167  si_ = nullptr;
168 }
169 
171 {
172  addedStartStates_ = 0;
173  sampledGoalsCount_ = 0;
174 }
175 
177 {
178  if (planner_ == nullptr)
179  throw Exception("No planner set for PlannerInputStates");
180  return use(planner_->getProblemDefinition());
181 }
182 
184 {
185  std::string error;
186 
187  if (pdef_ == nullptr)
188  error = "Problem definition not specified";
189  else
190  {
191  if (pdef_->getStartStateCount() <= 0)
192  error = "No start states specified";
193  else if (!pdef_->getGoal())
194  error = "No goal specified";
195  }
196 
197  if (!error.empty())
198  {
199  if (planner_ != nullptr)
200  throw Exception(planner_->getName(), error);
201  else
202  throw Exception(error);
203  }
204 }
205 
207 {
208  if (pdef)
209  return use(pdef.get());
210 
211  clear();
212  return true;
213 }
214 
216 {
217  if (pdef_ != pdef)
218  {
219  clear();
220  pdef_ = pdef;
221  si_ = pdef->getSpaceInformation().get();
222  return true;
223  }
224  return false;
225 }
226 
228 {
229  if (pdef_ == nullptr || si_ == nullptr)
230  {
231  std::string error = "Missing space information or problem definition";
232  if (planner_ != nullptr)
233  throw Exception(planner_->getName(), error);
234  else
235  throw Exception(error);
236  }
237 
238  while (addedStartStates_ < pdef_->getStartStateCount())
239  {
240  const base::State *st = pdef_->getStartState(addedStartStates_);
241  addedStartStates_++;
242  bool bounds = si_->satisfiesBounds(st);
243  bool valid = bounds ? si_->isValid(st) : false;
244  if (bounds && valid)
245  return st;
246 
247  OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
248  planner_ ? planner_->getName().c_str() : "PlannerInputStates", bounds ? "state" : "bounds");
249  std::stringstream ss;
250  si_->printState(st, ss);
251  OMPL_DEBUG("%s: Discarded start state %s", planner_ ? planner_->getName().c_str() : "PlannerInputStates",
252  ss.str().c_str());
253  }
254  return nullptr;
255 }
256 
258 {
259  // This initialization is safe since we are in a non-const function anyway.
261  return nextGoal(ptc);
262 }
263 
265 {
266  if (pdef_ == nullptr || si_ == nullptr)
267  {
268  std::string error = "Missing space information or problem definition";
269  if (planner_ != nullptr)
270  throw Exception(planner_->getName(), error);
271  else
272  throw Exception(error);
273  }
274 
275  const GoalSampleableRegion *goal =
276  pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : nullptr;
277 
278  if (goal != nullptr)
279  {
280  time::point start_wait;
281  bool first = true;
282  bool attempt = true;
283  while (attempt)
284  {
285  attempt = false;
286 
287  if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
288  {
289  if (tempState_ == nullptr)
290  tempState_ = si_->allocState();
291  do
292  {
293  goal->sampleGoal(tempState_);
294  sampledGoalsCount_++;
295  bool bounds = si_->satisfiesBounds(tempState_);
296  bool valid = bounds ? si_->isValid(tempState_) : false;
297  if (bounds && valid)
298  {
299  if (!first) // if we waited, show how long
300  {
301  OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
302  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
303  time::seconds(time::now() - start_wait));
304  }
305  return tempState_;
306  }
307 
308  OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
309  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
310  bounds ? "state" : "bounds");
311  std::stringstream ss;
312  si_->printState(tempState_, ss);
313  OMPL_DEBUG("%s: Discarded goal state:\n%s",
314  planner_ ? planner_->getName().c_str() : "PlannerInputStates", ss.str().c_str());
315  } while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
316  }
317  if (goal->couldSample() && !ptc)
318  {
319  if (first)
320  {
321  first = false;
322  start_wait = time::now();
323  OMPL_DEBUG("%s: Waiting for goal region samples ...",
324  planner_ ? planner_->getName().c_str() : "PlannerInputStates");
325  }
326  std::this_thread::sleep_for(time::seconds(0.01));
327  attempt = !ptc;
328  }
329  }
330  }
331 
332  return nullptr;
333 }
334 
336 {
337  if (pdef_ != nullptr)
338  return addedStartStates_ < pdef_->getStartStateCount();
339  return false;
340 }
341 
343 {
344  if ((pdef_ != nullptr) && pdef_->getGoal())
345  if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
346  return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
347  return false;
348 }
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:66
Properties that planners may have.
Definition: Planner.h:192
A shared pointer wrapper for ompl::base::ProblemDefinition.
const PlannerInputStates & getPlannerInputStates() const
Get the planner input states.
Definition: Planner.cpp:82
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
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:335
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:151
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:61
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
const State * nextGoal()
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:257
void clear()
Clear all stored information.
Definition: Planner.cpp:157
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
const State * nextStart()
Return the next valid start state or nullptr if no more valid start states are available.
Definition: Planner.cpp:227
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:176
bool haveMoreGoalStates() const
Check if there are more potential goal states.
Definition: Planner.cpp:342
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:415
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:170
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:137
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:406
void checkValidity() const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:183
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:206
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:410
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