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  logInform("Space information setup was not yet called. Calling now.");
91  si_->setup();
92  }
93 
94  if (setup_)
95  logWarn("Planner setup called multiple times");
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(PlannerThreadedTerminationCondition(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_->getSpaceInformation(), 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 (si && pdef)
208  return use(si.get(), pdef.get());
209  else
210  {
211  clear();
212  return true;
213  }
214 }
215 
217 {
218  if (pdef_ != pdef || si_ != si)
219  {
220  clear();
221  pdef_ = pdef;
222  si_ = si;
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  logWarn("Skipping invalid start state (invalid %s)", bounds ? "state": "bounds");
250  std::stringstream ss;
251  si_->printState(st, ss);
252  logDebug("Discarded start state %s", ss.str().c_str());
253  }
254  }
255  return NULL;
256 }
257 
259 {
261  return nextGoal(ptc);
262 }
263 
265 {
266  if (pdef_ == NULL || si_ == NULL)
267  {
268  std::string error = "Missing space information or problem definition";
269  if (planner_)
270  throw Exception(planner_->getName(), error);
271  else
272  throw Exception(error);
273  }
274 
275  const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
276 
277  if (goal)
278  {
279  time::point start_wait;
280  bool first = true;
281  bool attempt = true;
282  while (attempt)
283  {
284  attempt = false;
285 
286  if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
287  {
288  if (tempState_ == NULL)
289  tempState_ = si_->allocState();
290  do
291  {
292  goal->sampleGoal(tempState_);
293  sampledGoalsCount_++;
294  bool bounds = si_->satisfiesBounds(tempState_);
295  bool valid = bounds ? si_->isValid(tempState_) : false;
296  if (bounds && valid)
297  {
298  if (!first) // if we waited, show how long
299  {
300  logDebug("Waited %lf seconds for the first goal sample.", time::seconds(time::now() - start_wait));
301  }
302  return tempState_;
303  }
304  else
305  {
306  logWarn("Skipping invalid goal state (invalid %s)", bounds ? "state": "bounds");
307  std::stringstream ss;
308  si_->printState(tempState_, ss);
309  logDebug("Discarded goal state %s", ss.str().c_str());
310  }
311  }
312  while (!ptc() && sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample());
313  }
314  if (goal->couldSample() && !ptc())
315  {
316  if (first)
317  {
318  first = false;
319  start_wait = time::now();
320  logDebug("Waiting for goal region samples ...");
321  }
322  boost::this_thread::sleep(time::seconds(0.01));
323  attempt = !ptc();
324  }
325  }
326  }
327 
328  return NULL;
329 }
330 
332 {
333  if (pdef_)
334  return addedStartStates_ < pdef_->getStartStateCount();
335  return false;
336 }
337 
339 {
340  if (pdef_ && pdef_->getGoal())
341  if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
342  return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
343  return false;
344 }