SimpleSetup.cpp
46 configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
54 configured_(false), planTime_(0.0), simplifyTime_(0.0), lastStatus_(base::PlannerStatus::UNKNOWN)
92 // we provide a duplicate implementation here to allow the planner to choose how the time is turned into a planner termination condition
107 ompl::base::PlannerStatus ompl::geometric::SimpleSetup::solve(const base::PlannerTerminationCondition &ptc)
121 void ompl::geometric::SimpleSetup::simplifySolution(const base::PlannerTerminationCondition &ptc)
133 OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
156 OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
191 return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
bool haveExactSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
Definition: SimpleSetup.cpp:189
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
bool haveSolutionPath() const
Return true if a solution path is available (previous call to solve() was successful). The solution may be approximate.
Definition: SimpleSetup.h:145
double simplifyTime_
The amount of time the last path simplification step took.
Definition: SimpleSetup.h:310
PathGeometric & getSolutionPath() const
Get the solution path. Throw an exception if no solution is available.
Definition: SimpleSetup.cpp:178
Representation of a solution to a planning problem.
Definition: ProblemDefinition.h:72
A boost shared pointer wrapper for ompl::base::StateSpace.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:237
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Definition: PlannerTerminationCondition.h:64
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
A boost shared pointer wrapper for ompl::base::Planner.
SimpleSetup(const base::SpaceInformationPtr &si)
Constructor needs the state space used for planning.
Definition: SimpleSetup.cpp:45
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
This class contains routines that attempt to simplify geometric paths.
Definition: PathSimplifier.h:66
A boost shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
Definition: SpaceInformation.h:86
virtual void setup()
This method will create the necessary classes for planning. The solve() method will call this functio...
Definition: SimpleSetup.cpp:61
static base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SelfConfig.cpp:250
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
Definition: SimpleSetup.cpp:194
base::PlannerStatus lastStatus_
The status of the last planning request.
Definition: SimpleSetup.h:313
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
Definition: ProblemDefinition.h:150
virtual base::PlannerStatus solve(double time=1.0)
Run the planner for up to a specified amount of time (default is 1 second)
Definition: SimpleSetup.cpp:93
std::string plannerName_
Name of planner type that generated this solution, as received from Planner::getName() ...
Definition: ProblemDefinition.h:136
bool configured_
Flag indicating whether the classes needed for planning are set up.
Definition: SimpleSetup.h:304
virtual void print(std::ostream &out=std::cout) const
Print information about the current setup.
Definition: SimpleSetup.cpp:201
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
Definition: SimpleSetup.cpp:141
A boost shared pointer wrapper for ompl::base::Goal.
const std::string getSolutionPlannerName(void) const
Get the best solution's planer name. Throw an exception if no solution is available.
Definition: SimpleSetup.cpp:164
OMPL_DEPRECATED base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
Definition: SimpleSetup.cpp:40
virtual void clear()
Clear all planning data. This only includes data generated by motion plan computation. Planner settings, start & goal states are not affected.
Definition: SimpleSetup.cpp:84
A boost shared pointer wrapper for ompl::base::Path.