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("Space information setup was not yet called. Calling now.");
91  si_->setup();
92  }
93 
94  if (setup_)
95  OMPL_WARN("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_->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  return use(pdef);
208 }
209 
211 {
212  if (pdef)
213  return use(pdef.get());
214  else
215  {
216  clear();
217  return true;
218  }
219 }
220 
222 {
223  return use(pdef);
224 }
225 
227 {
228  if (pdef_ != pdef)
229  {
230  clear();
231  pdef_ = pdef;
232  si_ = pdef->getSpaceInformation().get();
233  return true;
234  }
235  return false;
236 }
237 
239 {
240  if (pdef_ == NULL || si_ == NULL)
241  {
242  std::string error = "Missing space information or problem definition";
243  if (planner_)
244  throw Exception(planner_->getName(), error);
245  else
246  throw Exception(error);
247  }
248 
249  while (addedStartStates_ < pdef_->getStartStateCount())
250  {
251  const base::State *st = pdef_->getStartState(addedStartStates_);
252  addedStartStates_++;
253  bool bounds = si_->satisfiesBounds(st);
254  bool valid = bounds ? si_->isValid(st) : false;
255  if (bounds && valid)
256  return st;
257  else
258  {
259  OMPL_WARN("Skipping invalid start state (invalid %s)", bounds ? "state": "bounds");
260  std::stringstream ss;
261  si_->printState(st, ss);
262  OMPL_DEBUG("Discarded start state %s", ss.str().c_str());
263  }
264  }
265  return NULL;
266 }
267 
269 {
271  return nextGoal(ptc);
272 }
273 
275 {
276  if (pdef_ == NULL || si_ == NULL)
277  {
278  std::string error = "Missing space information or problem definition";
279  if (planner_)
280  throw Exception(planner_->getName(), error);
281  else
282  throw Exception(error);
283  }
284 
285  const GoalSampleableRegion *goal = pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION) ? pdef_->getGoal()->as<GoalSampleableRegion>() : NULL;
286 
287  if (goal)
288  {
289  time::point start_wait;
290  bool first = true;
291  bool attempt = true;
292  while (attempt)
293  {
294  attempt = false;
295 
296  if (sampledGoalsCount_ < goal->maxSampleCount() && goal->canSample())
297  {
298  if (tempState_ == NULL)
299  tempState_ = si_->allocState();
300  do
301  {
302  goal->sampleGoal(tempState_);
303  sampledGoalsCount_++;
304  bool bounds = si_->satisfiesBounds(tempState_);
305  bool valid = bounds ? si_->isValid(tempState_) : false;
306  if (bounds && valid)
307  {
308  if (!first) // if we waited, show how long
309  {
310  OMPL_DEBUG("Waited %lf seconds for the first goal sample.", time::seconds(time::now() - start_wait));
311  }
312  return tempState_;
313  }
314  else
315  {
316  OMPL_WARN("Skipping invalid goal state (invalid %s)", bounds ? "state": "bounds");
317  std::stringstream ss;
318  si_->printState(tempState_, ss);
319  OMPL_DEBUG("Discarded goal state %s", 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("Waiting for goal region samples ...");
331  }
332  boost::this_thread::sleep(time::seconds(0.01));
333  attempt = !ptc;
334  }
335  }
336  }
337 
338  return NULL;
339 }
340 
342 {
343  if (pdef_)
344  return addedStartStates_ < pdef_->getStartStateCount();
345  return false;
346 }
347 
349 {
350  if (pdef_ && pdef_->getGoal())
351  if (pdef_->getGoal()->hasType(GOAL_SAMPLEABLE_REGION))
352  return sampledGoalsCount_ < pdef_->getGoal()->as<GoalSampleableRegion>()->maxSampleCount();
353  return false;
354 }