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  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
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:203
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:332
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:409
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:200
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:335
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:151
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:422
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:429
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:257
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: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
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:412
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:418
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
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:207
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
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner&#39;s progress property names and the functions used for querying those pr...
Definition: Planner.h:426
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: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