37 #ifndef OMPL_GEOMETRIC_SIMPLE_SETUP_
38 #define OMPL_GEOMETRIC_SIMPLE_SETUP_
40 #include "ompl/base/Planner.h"
41 #include "ompl/base/PlannerData.h"
42 #include "ompl/base/SpaceInformation.h"
43 #include "ompl/base/ProblemDefinition.h"
44 #include "ompl/geometric/PathGeometric.h"
45 #include "ompl/geometric/PathSimplifier.h"
47 #include "ompl/util/Exception.h"
56 OMPL_CLASS_FORWARD(SimpleSetup);
91 return si_->getStateSpace();
97 return si_->getStateValidityChecker();
103 return pdef_->getGoal();
136 return pdef_->getSolutionPath().get();
148 si_->setStateValidityChecker(svc);
154 si_->setStateValidityChecker(svc);
159 const double threshold = std::numeric_limits<double>::epsilon())
161 pdef_->setStartAndGoalStates(start, goal, threshold);
168 pdef_->addStartState(state);
174 pdef_->clearStartStates();
187 pdef_->setGoalState(goal, threshold);
194 pdef_->setGoal(goal);
203 if (planner && planner->getSpaceInformation().get() !=
si_.get())
204 throw Exception(
"Planner instance does not match space information");
253 virtual void clear(
void);
256 virtual void print(std::ostream &out = std::cout)
const;
261 virtual void setup(
void);
const base::ProblemDefinitionPtr & getProblemDefinition(void) const
Get the current instance of the problem definition.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void setStartAndGoalStates(const base::ScopedState<> &start, const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
Set the start and goal states to use.
double simplifyTime_
The amount of time the last path simplification step took.
A boost shared pointer wrapper for ompl::base::ProblemDefinition.
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
base::PlannerAllocator pa_
The optional planner allocator.
bool haveSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition of a scoped state.
const base::PlannerPtr & getPlanner(void) const
Get the current planner.
PathSimplifierPtr & getPathSimplifier(void)
Get the path simplifier.
A boost shared pointer wrapper for ompl::base::StateSpace.
base::ParamSet params_
The parameters that describe the planning context.
boost::function< PlannerPtr(const SpaceInformationPtr &)> PlannerAllocator
Definition of a function that can allocate a planner.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
void setGoal(const base::GoalPtr &goal)
Set the goal for planning. This call is not needed if setStartAndGoalStates() has been called...
void setPlanner(const base::PlannerPtr &planner)
Set the planner to use. If the planner is not set, an attempt is made to use the planner allocator...
base::SpaceInformationPtr si_
The created space information.
const base::StateSpacePtr & getStateSpace(void) const
Get the current instance of the state space.
PathSimplifierPtr psk_
The instance of the path simplifier.
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
Maintain a set of parameters.
bool haveExactSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
const PathSimplifierPtr & getPathSimplifier(void) const
Get the path simplifier.
Create the set of classes typically needed to solve a geometric problem.
const base::StateValidityCheckerPtr & getStateValidityChecker(void) const
Get the current instance of the state validity checker.
A boost shared pointer wrapper for ompl::base::Planner.
base::PlannerStatus last_status_
The status of the last planning request.
const base::ParamSet & params(void) const
Get the parameters for this planning context.
base::PlannerPtr planner_
The maintained planner instance.
base::ProblemDefinitionPtr pdef_
The created problem definition.
A class to store the exit status of Planner::solve()
const base::GoalPtr & getGoal(void) const
Get the current goal definition.
void clearStartStates(void)
Clear the currently set starting states.
void setStateValidityChecker(const base::StateValidityCheckerPtr &svc)
Set the state validity checker to use.
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
const base::PlannerAllocator & getPlannerAllocator(void) const
Get the planner allocator.
void setPlannerAllocator(const base::PlannerAllocator &pa)
Set the planner allocator to use. This is only used if no planner has been set. This is optional – a...
base::ParamSet & params(void)
Get the parameters for this planning context.
A boost shared pointer wrapper for ompl::geometric::PathSimplifier.
base::PlannerStatus getLastPlannerStatus(void) const
Return the status of the last planning attempt.
void addStartState(const base::ScopedState<> &state)
Add a starting state for planning. This call is not needed if setStartAndGoalStates() has been called...
The exception type for ompl.
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
PathGeometric & getSolutionPath(void) const
Get the solution path. Throw an exception if no solution is available.
virtual void clear(void)
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
bool configured_
Flag indicating whether the classes needed for planning are set up.
void setGoalState(const base::ScopedState<> &goal, const double threshold=std::numeric_limits< double >::epsilon())
A simple form of setGoal(). The goal will be an instance of ompl::base::GoalState.
double planTime_
The amount of time the last planning step took.
double getLastSimplificationTime(void) const
Get the amount of time (in seconds) spend during the last path simplification step.
void setStartState(const base::ScopedState<> &state)
Clear the currently set starting states and add state as the starting state.
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
double getLastPlanComputationTime(void) const
Get the amount of time (in seconds) spent during the last planning step.
A boost shared pointer wrapper for ompl::base::Goal.
Definition of a geometric path.
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
SimpleSetup(const base::StateSpacePtr &space)
Constructor needs the state space used for planning.
void setStateValidityChecker(const base::StateValidityCheckerFn &svc)
Set the state validity checker to use.
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
const base::SpaceInformationPtr & getSpaceInformation(void) const
Get the current instance of the space information.