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(
"%s: Space information setup was not yet called. Calling now.", getName().c_str());
95 OMPL_WARN(
"%s: Planner setup called multiple times", getName().c_str());
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);
208 return use(pdef.get());
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 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);
255 planner_ ? planner_->getName().c_str() :
"PlannerInputStates",
265 return nextGoal(ptc);
270 if (pdef_ == NULL || si_ == NULL)
272 std::string error =
"Missing space information or problem definition";
274 throw Exception(planner_->getName(), error);
290 if (sampledGoalsCount_ < goal->maxSampleCount() && goal->
canSample())
292 if (tempState_ == NULL)
293 tempState_ = si_->allocState();
297 sampledGoalsCount_++;
298 bool bounds = si_->satisfiesBounds(tempState_);
299 bool valid = bounds ? si_->isValid(tempState_) :
false;
304 OMPL_DEBUG(
"%s: Waited %lf seconds for the first goal sample.",
305 planner_ ? planner_->getName().c_str() :
"PlannerInputStates",
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);
318 planner_ ? planner_->getName().c_str() :
"PlannerInputStates",
322 while (!ptc && sampledGoalsCount_ < goal->maxSampleCount() && goal->
canSample());
330 OMPL_DEBUG(
"%s: Waiting for goal region samples ...",
331 planner_ ? planner_->getName().c_str() :
"PlannerInputStates");
345 return addedStartStates_ < pdef_->getStartStateCount();
351 if (pdef_ && pdef_->getGoal())
Planner(const SpaceInformationPtr &si, const std::string &name)
Constructor.
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Properties that planners may have.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
virtual void printProperties(std::ostream &out) const
Print properties of the motion planner.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
const std::string & getName(void) const
Get the name of the planner.
bool hasType(GoalType type) const
Check if this goal can be cast to a particular goal type.
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.
const ProblemDefinitionPtr & getProblemDefinition(void) const
Get the problem definition the planner is trying to solve.
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 setName(const std::string &name)
Set the name of the planner.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
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)
virtual void checkValidity(void)
Check to see if the planner is in a working state (setup has been called, a goal was set...
boost::posix_time::ptime point
Representation of a point in time.
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information this planner is using.
A class to store the exit status of Planner::solve()
virtual void setProblemDefinition(const ProblemDefinitionPtr &pdef)
Set the problem definition for the planner. The problem needs to be set before calling solve()...
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.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
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...
std::string name_
The name of this planner.
virtual void getPlannerData(PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
const PlannerInputStates & getPlannerInputStates(void) const
Get the planner input states.
SpaceInformationPtr si_
The space information for which planning is done.
virtual void printSettings(std::ostream &out) const
Print information about the motion planner's settings.
point now(void)
Get the current time point.
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.