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 logInform(
"Space information setup was not yet called. Calling now.");
95 logWarn(
"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_->getSpaceInformation(), 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);
208 return use(si.get(), pdef.get());
218 if (pdef_ != pdef || si_ != si)
230 if (pdef_ == NULL || si_ == NULL)
232 std::string error =
"Missing space information or problem definition";
234 throw Exception(planner_->getName(), error);
239 while (addedStartStates_ < pdef_->getStartStateCount())
241 const base::State *st = pdef_->getStartState(addedStartStates_);
243 bool bounds = si_->satisfiesBounds(st);
244 bool valid = bounds ? si_->isValid(st) :
false;
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());
261 return nextGoal(ptc);
266 if (pdef_ == NULL || si_ == NULL)
268 std::string error =
"Missing space information or problem definition";
270 throw Exception(planner_->getName(), error);
286 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->
canSample())
288 if (tempState_ == NULL)
289 tempState_ = si_->allocState();
293 sampledGoalsCount_++;
294 bool bounds = si_->satisfiesBounds(tempState_);
295 bool valid = bounds ? si_->isValid(tempState_) :
false;
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());
320 logDebug(
"Waiting for goal region samples ...");
334 return addedStartStates_ < pdef_->getStartStateCount();
340 if (pdef_ && pdef_->getGoal())