All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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(void) 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();
104  pis_.checkValidity();
105 }
106 
108 {
109  return setup_;
110 }
111 
113 {
114  pis_.clear();
115  pis_.update();
116 }
117 
119 {
120 }
121 
123 {
124  return solve(PlannerTerminationCondition(ptc, checkInterval));
125 }
126 
128 {
129  if (solveTime < 1.0)
130  return solve(timedPlannerTerminationCondition(solveTime));
131  else
132  return solve(timedPlannerTerminationCondition(solveTime, std::min(solveTime / 100.0, 0.1)));
133 }
134 
135 void ompl::base::Planner::printProperties(std::ostream &out) const
136 {
137  out << "Planner " + getName() + " specs:" << std::endl;
138  out << "Multithreaded: " << (getSpecs().multithreaded ? "Yes" : "No") << std::endl;
139  out << "Reports approximate solutions: " << (getSpecs().approximateSolutions ? "Yes" : "No") << std::endl;
140  out << "Can optimize solutions: " << (getSpecs().optimizingPaths ? "Yes" : "No") << std::endl;
141  out << "Aware of the following parameters:";
142  std::vector<std::string> params;
143  params_.getParamNames(params);
144  for (unsigned int i = 0 ; i < params.size() ; ++i)
145  out << " " << params[i];
146  out << std::endl;
147 }
148 
149 void ompl::base::Planner::printSettings(std::ostream &out) const
150 {
151  out << "Declared parameters for planner " << getName() << ":" << std::endl;
152  params_.print(out);
153 }
154 
156 {
157  if (tempState_)
158  {
159  si_->freeState(tempState_);
160  tempState_ = NULL;
161  }
162  addedStartStates_ = 0;
163  sampledGoalsCount_ = 0;
164  pdef_ = NULL;
165  si_ = NULL;
166 }
167 
169 {
170  addedStartStates_ = 0;
171  sampledGoalsCount_ = 0;
172 }
173 
175 {
176  if (!planner_)
177  throw Exception("No planner set for PlannerInputStates");
178  return use(planner_->getProblemDefinition());
179 }
180 
182 {
183  std::string error;
184 
185  if (!pdef_)
186  error = "Problem definition not specified";
187  else
188  {
189  if (pdef_->getStartStateCount() <= 0)
190  error = "No start states specified";
191  else
192  if (!pdef_->getGoal())
193  error = "No goal specified";
194  }
195 
196  if (!error.empty())
197  {
198  if (planner_)
199  throw Exception(planner_->getName(), error);
200  else
201  throw Exception(error);
202  }
203 }
204 
206 {
207  if (pdef)
208  return use(pdef.get());
209  else
210  {
211  clear();
212  return true;
213  }
214 }
215 
217 {
218  if (pdef_ != pdef)
219  {
220  clear();
221  pdef_ = pdef;
222  si_ = pdef->getSpaceInformation().get();
223  return true;
224  }
225  return false;
226 }
227 
229 {
230  if (pdef_ == NULL || si_ == NULL)
231  {
232  std::string error = "Missing space information or problem definition";
233  if (planner_)
234  throw Exception(planner_->getName(), error);
235  else
236  throw Exception(error);
237  }
238 
239  while (addedStartStates_ < pdef_->getStartStateCount())
240  {
241  const base::State *st = pdef_->getStartState(addedStartStates_);
242  addedStartStates_++;
243  bool bounds = si_->satisfiesBounds(st);
244  bool valid = bounds ? si_->isValid(st) : false;
245  if (bounds && valid)
246  return st;
247  else
248  {
249  OMPL_WARN("%s: Skipping invalid start state (invalid %s)",
250  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
251  bounds ? "state": "bounds");
252  std::stringstream ss;
253  si_->printState(st, ss);
254  OMPL_DEBUG("%s: Discarded start state %s",
255  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
256  ss.str().c_str());
257  }
258  }
259  return NULL;
260 }
261 
263 {
265  return nextGoal(ptc);
266 }
267 
269 {
270  if (pdef_ == NULL || si_ == NULL)
271  {
272  std::string error = "Missing space information or problem definition";
273  if (planner_)
274  throw Exception(planner_->getName(), error);
275  else
276  throw Exception(error);
277  }
278 
279  const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
280 
281  if (goal)
282  {
283  time::point start_wait;
284  bool first = true;
285  bool attempt = true;
286  while (attempt)
287  {
288  attempt = false;
289 
290  if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
291  {
292  if (tempState_ == NULL)
293  tempState_ = si_->allocState();
294  do
295  {
296  goal->sampleGoal(tempState_);
297  sampledGoalsCount_++;
298  bool bounds = si_->satisfiesBounds(tempState_);
299  bool valid = bounds ? si_->isValid(tempState_) : false;
300  if (bounds && valid)
301  {
302  if (!first) // if we waited, show how long
303  {
304  OMPL_DEBUG("%s: Waited %lf seconds for the first goal sample.",
305  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
306  time::seconds(time::now() - start_wait));
307  }
308  return tempState_;
309  }
310  else
311  {
312  OMPL_WARN("%s: Skipping invalid goal state (invalid %s)",
313  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
314  bounds ? "state": "bounds");
315  std::stringstream ss;
316  si_->printState(tempState_, ss);
317  OMPL_DEBUG("%s: Discarded goal state %s",
318  planner_ ? planner_->getName().c_str() : "PlannerInputStates",
319  ss.str().c_str());
320  }
321  }
322  while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
323  }
324  if (goal->couldSample() && !ptc)
325  {
326  if (first)
327  {
328  first = false;
329  start_wait = time::now();
330  OMPL_DEBUG("%s: Waiting for goal region samples ...",
331  planner_ ? planner_->getName().c_str() : "PlannerInputStates");
332  }
333  boost::this_thread::sleep(time::seconds(0.01));
334  attempt = !ptc;
335  }
336  }
337  }
338 
339  return NULL;
340 }
341 
343 {
344  if (pdef_)
345  return addedStartStates_ < pdef_->getStartStateCount();
346  return false;
347 }
348 
350 {
351  if (pdef_ && pdef_->getGoal())
352  if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
353  return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
354  return false;
355 }
Planner(const SpaceInformationPtr &si, const std::string &name)
Constructor.
Definition: Planner.cpp:43
const State * nextStart(void)
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:228
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Properties that planners may have.
Definition: Planner.h:199
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
Definition: Planner.cpp:135
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
const State * nextGoal(void)
Same as above but only one attempt is made to find a valid goal.
Definition: Planner.cpp:262
const std::string & getName(void) const
Get the name of the planner.
Definition: Planner.cpp:55
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
Definition: Goal.h:102
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
bool isSetup(void) const
Check if setup() was called for this planner.
Definition: Planner.cpp:107
const ProblemDefinitionPtr & getProblemDefinition(void) const
Get the problem definition the planner is trying to solve.
Definition: Planner.cpp:70
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
PlannerTerminationCondition plannerAlwaysTerminatingCondition()
Simple termination condition that always returns true. The termination condition will always be met...
void checkValidity(void) const
Check if the problem definition was set, start state are available and goal was set.
Definition: Planner.cpp:181
void setName(const std::string &name)
Set the name of the planner.
Definition: Planner.cpp:60
bool update(void)
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:174
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.
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information this problem definition is for.
const PlannerSpecs & getSpecs(void) const
Return the specifications (capabilities of this planner)
Definition: Planner.cpp:50
virtual void checkValidity(void)
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
void restart(void)
Forget how many states were returned by nextStart() and nextGoal() and return all states again...
Definition: Planner.cpp:168
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information this planner is using.
Definition: Planner.cpp:65
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
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
virtual bool couldSample(void) 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.
Definition of an abstract state.
Definition: State.h:50
bool haveMoreStartStates(void) const
Check if there are more potential start states.
Definition: Planner.cpp:342
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
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...
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
std::string name_
The name of this planner.
Definition: Planner.h:401
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
Helper class to extract valid start & goal states. Usually used internally by planners.
Definition: Planner.h:83
const PlannerInputStates & getPlannerInputStates(void) const
Get the planner input states.
Definition: Planner.cpp:81
bool haveMoreGoalStates(void) const
Check if there are more potential goal states.
Definition: Planner.cpp:349
void clear(void)
Clear all stored information.
Definition: Planner.cpp:155
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:392
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
Definition: Planner.cpp:149
point now(void)
Get the current time point.
Definition: Time.h:56
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:205
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