37 #include "ompl/base/Planner.h"
38 #include "ompl/util/Exception.h"
39 #include "ompl/base/goals/GoalSampleableRegion.h"
41 #include <boost/thread.hpp>
44 si_(si), pis_(this), name_(name), setup_(false)
47 throw Exception(
name_,
"Invalid space information instance for planner");
90 OMPL_INFORM(
"Space information setup was not yet called. Calling now.");
95 OMPL_WARN(
"Planner setup called multiple times");
104 pis_.checkValidity();
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];
151 out <<
"Declared parameters for planner " << getName() <<
":" << std::endl;
159 si_->freeState(tempState_);
162 addedStartStates_ = 0;
163 sampledGoalsCount_ = 0;
170 addedStartStates_ = 0;
171 sampledGoalsCount_ = 0;
177 throw Exception(
"No planner set for PlannerInputStates");
178 return use(planner_->getProblemDefinition());
186 error =
"Problem definition not specified";
189 if (pdef_->getStartStateCount() <= 0)
190 error =
"No start states specified";
192 if (!pdef_->getGoal())
193 error =
"No goal specified";
199 throw Exception(planner_->getName(), error);
213 return use(pdef.get());
240 if (pdef_ == NULL || si_ == NULL)
242 std::string error =
"Missing space information or problem definition";
244 throw Exception(planner_->getName(), error);
249 while (addedStartStates_ < pdef_->getStartStateCount())
251 const base::State *st = pdef_->getStartState(addedStartStates_);
253 bool bounds = si_->satisfiesBounds(st);
254 bool valid = bounds ? si_->isValid(st) :
false;
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());
271 return nextGoal(ptc);
276 if (pdef_ == NULL || si_ == NULL)
278 std::string error =
"Missing space information or problem definition";
280 throw Exception(planner_->getName(), error);
296 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->
canSample())
298 if (tempState_ == NULL)
299 tempState_ = si_->allocState();
303 sampledGoalsCount_++;
304 bool bounds = si_->satisfiesBounds(tempState_);
305 bool valid = bounds ? si_->isValid(tempState_) :
false;
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());
322 while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->
canSample());
330 OMPL_DEBUG(
"Waiting for goal region samples ...");
344 return addedStartStates_ < pdef_->getStartStateCount();
350 if (pdef_ && pdef_->getGoal())