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 <boost/thread.hpp>
42 
43 ompl::base::Planner::Planner(const SpaceInformationPtr &si, const std::string &name) :
44  si_(si), pis_(this), name_(name), setup_(false)
45 {
46  if (!si_)
47  throw Exception(name_, "Invalid space information instance for planner");
48 }
49 
51 {
52  return specs_;
53 }
54 
55 const std::string& ompl::base::Planner::getName() const
56 {
57  return name_;
58 }
59 
60 void ompl::base::Planner::setName(const std::string &name)
61 {
62  name_ = name;
63 }
64 
66 {
67  return si_;
68 }
69 
71 {
72  return pdef_;
73 }
74 
76 {
77  pdef_ = pdef;
78  pis_.update();
79 }
80 
82 {
83  return pis_;
84 }
85 
87 {
88  if (!si_->isSetup())
89  {
90  OMPL_INFORM("%s: Space information setup was not yet called. Calling now.", getName().c_str());
91  si_->setup();
92  }
93 
94  if (setup_)
95  OMPL_WARN("%s: Planner setup called multiple times", getName().c_str());
96  else
97  setup_ = true;
98 }
99 
101 {
102  if (!isSetup())
103  setup();
105 }
106 
108 {
109  return setup_;
110 }
111 
113 {
114  pis_.clear();
115  pis_.update();
116 }
117 
119 {
120  for (PlannerProgressProperties::const_iterator it = plannerProgressProperties_.begin() ; it != plannerProgressProperties_.end() ; ++it)
121  data.properties[it->first] = it->second();
122 }
123 
125 {
126  return solve(PlannerTerminationCondition(ptc, checkInterval));
127 }
128 
130 {
131  if (solveTime < 1.0)
132  return solve(timedPlannerTerminationCondition(solveTime));
133  else
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 (unsigned int i = 0 ; i < params.size() ; ++i)
147  out << " " << params[i];
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_)
160  {
161  si_->freeState(tempState_);
162  tempState_ = NULL;
163  }
164  addedStartStates_ = 0;
165  sampledGoalsCount_ = 0;
166  pdef_ = NULL;
167  si_ = NULL;
168 }
169 
171 {
172  addedStartStates_ = 0;
173  sampledGoalsCount_ = 0;
174 }
175 
177 {
178  if (!planner_)
179  throw Exception("No planner set for PlannerInputStates");
180  return use(planner_->getProblemDefinition());
181 }
182 
184 {
185  std::string error;
186 
187  if (!pdef_)
188  error = "Problem definition not specified";
189  else
190  {
191  if (pdef_->getStartStateCount() <= 0)
192  error = "No start states specified";
193  else
194  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_ == NULL || si_ == NULL)
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",
253  bounds ? "state": "bounds");
254  std::stringstream ss;
255  si_->printState(st, ss);
256  OMPL_DEBUG("%s: Discarded start state %s",
257  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
258  ss.str().c_str());
259  }
260  }
261  return NULL;
262 }
263 
265 {
266  // This initialization is safe since we are in a non-const function anyway.
268  return nextGoal(ptc);
269 }
270 
272 {
273  if (pdef_ == NULL || si_ == NULL)
274  {
275  std::string error = "Missing space information or problem definition";
276  if (planner_)
277  throw Exception(planner_->getName(), error);
278  else
279  throw Exception(error);
280  }
281 
282  const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
283 
284  if (goal)
285  {
286  time::point start_wait;
287  bool first = true;
288  bool attempt = true;
289  while (attempt)
290  {
291  attempt = false;
292 
293  if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
294  {
295  if (tempState_ == NULL)
296  tempState_ = si_->allocState();
297  do
298  {
299  goal->sampleGoal(tempState_);
300  sampledGoalsCount_++;
301  bool bounds = si_->satisfiesBounds(tempState_);
302  bool valid = bounds ? si_->isValid(tempState_) : false;
303  if (bounds && valid)
304  {
305  if (!first) // if we waited, show how long
306  {
307  OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
308  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
309  time::seconds(time::now() - start_wait));
310  }
311  return tempState_;
312  }
313  else
314  {
315  OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
316  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
317  bounds ? "state": "bounds");
318  std::stringstream ss;
319  si_->printState(tempState_, ss);
320  OMPL_DEBUG("%s: Discarded goal state %s",
321  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
322  ss.str().c_str());
323  }
324  }
325  while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
326  }
327  if (goal->couldSample() && !ptc)
328  {
329  if (first)
330  {
331  first = false;
332  start_wait = time::now();
333  OMPL_DEBUG("%s: Waiting for goal region samples ...",
334  planner_ ? planner_->getName().c_str() : "PlannerInputStates");
335  }
336  boost::this_thread::sleep(time::seconds(0.01));
337  attempt = !ptc;
338  }
339  }
340  }
341 
342  return NULL;
343 }
344 
346 {
347  if (pdef_)
348  return addedStartStates_ < pdef_->getStartStateCount();
349  return false;
350 }
351 
353 {
354  if (pdef_ && pdef_->getGoal())
355  if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
356  return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
357  return false;
358 }
const SpaceInformationPtr & getSpaceInformation() const
Get the space information this planner is using.
Definition: Planner.cpp:65
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
Planner(const SpaceInformationPtr &si, const std::string &name)
Constructor.
Definition: Planner.cpp:43
Properties that planners may have.
Definition: Planner.h:199
A boost 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:81
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...
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
bool multithreaded
Flag indicating whether multiple threads are used in the computation of the planner.
Definition: Planner.h:211
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:50
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:345
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:60
ParamSet params_
A map from parameter names to parameter instances for this planner. This field is populated by the de...
Definition: Planner.h:412
bool setup_
Flag indicating whether setup() has been called.
Definition: Planner.h:418
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
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:118
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:264
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
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
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:70
A boost 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:75
Definition of an abstract state.
Definition: State.h:50
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:100
#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:403
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL 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:47
#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:352
const std::string & getName() const
Get the name of the planner.
Definition: Planner.cpp:55
bool isSetup() const
Check if setup() was called for this planner.
Definition: Planner.cpp:107
point now()
Get the current time point.
Definition: Time.h:56
std::string name_
The name of this planner.
Definition: Planner.h:406
bool optimizingPaths
Flag indicating whether the planner attempts to optimize the path and reduce its length until the max...
Definition: Planner.h:218
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:83
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.
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
PlannerProgressProperties plannerProgressProperties_
A mapping between this planner&#39;s progress property names and the functions used for querying those pr...
Definition: Planner.h:415
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:207
std::map< std::string, std::string > properties
Any extra properties (key-value pairs) the planner can set.
Definition: PlannerData.h:394
boost::function< bool()> PlannerTerminationConditionFn
Signature for functions that decide whether termination conditions have been met for a planner...
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
Definition: GoalTypes.h:55
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68