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);