37 #include "ompl/geometric/SimpleSetup.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/geometric/planners/rrt/RRTConnect.h"
40 #include "ompl/geometric/planners/rrt/RRT.h"
41 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
42 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
48 throw Exception(
"Unable to allocate default planner for unspecified goal definition");
51 if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
54 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
63 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
70 throw Exception(
"Unable to allocate default planner");
76 configured_(false), planTime_(0.0), simplifyTime_(0.0), last_status_(base::PlannerStatus::UNKNOWN)
86 if (!configured_ || !si_->isSetup() || !planner_->isSetup())
96 OMPL_INFORM(
"No planner specified. Using default.");
100 planner_->setProblemDefinition(pdef_);
101 if (!planner_->isSetup())
105 params_.include(si_->params());
106 params_.include(planner_->params());
116 pdef_->clearSolutionPaths();
125 last_status_ = planner_->solve(time);
128 OMPL_INFORM(
"Solution found in %f seconds", planTime_);
130 OMPL_INFORM(
"No solution found after %f seconds", planTime_);
139 last_status_ = planner_->solve(ptc);
142 OMPL_INFORM(
"Solution found in %f seconds", planTime_);
144 OMPL_INFORM(
"No solution found after %f seconds", planTime_);
156 psk_->simplify(static_cast<PathGeometric&>(*p), ptc);
158 OMPL_INFORM(
"Path simplification took %f seconds", simplifyTime_);
173 if (duration < std::numeric_limits<double>::epsilon())
174 psk_->simplifyMax(static_cast<PathGeometric&>(*p));
176 psk_->simplify(static_cast<PathGeometric&>(*p), duration);
178 OMPL_INFORM(
"Path simplification took %f seconds", simplifyTime_);
198 return haveSolutionPath() && (!pdef_->hasApproximateSolution() || pdef_->getSolutionDifference() < std::numeric_limits<double>::epsilon());
205 planner_->getPlannerData(pd);
212 si_->printProperties(out);
213 si_->printSettings(out);
217 planner_->printProperties(out);
218 planner_->printSettings(out);
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void getPlannerData(base::PlannerData &pd) const
Get information about the exploration data structure the motion planner used.
void include(const ParamSet &other, const std::string &prefix="")
Include the params of a different ParamSet into this one. Optionally include a prefix for each of the...
A boost shared pointer wrapper for ompl::base::StateSpace.
base::ParamSet params_
The parameters that describe the planning context.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
base::SpaceInformationPtr si_
The created space information.
PathSimplifierPtr psk_
The instance of the path simplifier.
bool haveExactSolutionPath(void) const
Return true if a solution path is available (previous call to solve() was successful) and the solutio...
duration seconds(double sec)
Return the time duration representing a given number of seconds.
A boost shared pointer wrapper for ompl::base::Planner.
Kinematic Planning by Interior-Exterior Cell Exploration.
boost::posix_time::ptime point
Representation of a point in time.
base::ProblemDefinitionPtr pdef_
The created problem definition.
A class to store the exit status of Planner::solve()
Rapidly-exploring Random Trees.
This class contains routines that attempt to simplify geometric paths.
virtual void setup(void)
This method will create the necessary classes for planning. The solve() method will call this functio...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Lazy Bi-directional KPIECE with one level of discretization.
The exception type for ompl.
Definition of a problem to be solved. This includes the start state(s) for the system and a goal spec...
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.
void simplifySolution(double duration=0.0)
Attempt to simplify the current solution path. Spent at most duration seconds in the simplification p...
A boost shared pointer wrapper for ompl::base::Goal.
virtual void clear(void)
Clears the entire data structure.
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.
base::PlannerPtr getDefaultPlanner(const base::GoalPtr &goal)
Given a goal specification, decide on a planner for that goal.
point now(void)
Get the current time point.
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.