00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/SimpleSetup.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/geometric/planners/rrt/RRTConnect.h"
00040 #include "ompl/geometric/planners/rrt/RRT.h"
00041 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
00042 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
00043
00044 ompl::base::PlannerPtr ompl::geometric::getDefaultPlanner(const base::GoalPtr &goal)
00045 {
00046 base::PlannerPtr planner;
00047 if (!goal)
00048 throw Exception("Unable to allocate default planner for unspecified goal definition");
00049
00050
00051 if (goal->hasType(base::GOAL_SAMPLEABLE_REGION))
00052 {
00053
00054 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
00055 planner = base::PlannerPtr(new LBKPIECE1(goal->getSpaceInformation()));
00056 else
00057 planner = base::PlannerPtr(new RRTConnect(goal->getSpaceInformation()));
00058 }
00059
00060 else
00061 {
00062
00063 if (goal->getSpaceInformation()->getStateSpace()->hasDefaultProjection())
00064 planner = base::PlannerPtr(new KPIECE1(goal->getSpaceInformation()));
00065 else
00066 planner = base::PlannerPtr(new RRT(goal->getSpaceInformation()));
00067 }
00068
00069 if (!planner)
00070 throw Exception("Unable to allocate default planner");
00071
00072 return planner;
00073 }
00074
00075 void ompl::geometric::SimpleSetup::setup(void)
00076 {
00077 if (!configured_)
00078 {
00079 if (!si_)
00080 throw Exception("No space information defined");
00081
00082 if (!si_->isSetup())
00083 si_->setup();
00084 if (!planner_)
00085 {
00086 if (pa_)
00087 planner_ = pa_(si_);
00088 else
00089 {
00090 msg_.inform("No planner specified. Using default.");
00091 planner_ = getDefaultPlanner(getGoal());
00092 }
00093 }
00094 planner_->setProblemDefinition(pdef_);
00095 if (!planner_->isSetup())
00096 planner_->setup();
00097 configured_ = true;
00098 }
00099 }
00100
00101 void ompl::geometric::SimpleSetup::clear(void)
00102 {
00103 if (planner_)
00104 planner_->clear();
00105 if (pdef_ && pdef_->getGoal())
00106 pdef_->getGoal()->clearSolutionPath();
00107 }
00108
00109 bool ompl::geometric::SimpleSetup::solve(double time)
00110 {
00111 setup();
00112 time::point start = time::now();
00113 bool result = planner_->solve(time);
00114 planTime_ = time::seconds(time::now() - start);
00115 if (result)
00116 msg_.inform("Solution found in %f seconds", planTime_);
00117 else
00118 msg_.inform("No solution found after %f seconds", planTime_);
00119 return result;
00120 }
00121
00122 void ompl::geometric::SimpleSetup::simplifySolution(void)
00123 {
00124 if (pdef_ && pdef_->getGoal())
00125 {
00126 const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00127 if (p)
00128 {
00129 time::point start = time::now();
00130 psk_->simplifyMax(static_cast<PathGeometric&>(*p));
00131 simplifyTime_ = time::seconds(time::now() - start);
00132 msg_.inform("Path simplification took %f seconds", simplifyTime_);
00133 }
00134 else
00135 msg_.warn("No solution to simplify");
00136 }
00137 else
00138 msg_.warn("No solution to simplify");
00139 }
00140
00141 ompl::geometric::PathGeometric& ompl::geometric::SimpleSetup::getSolutionPath(void) const
00142 {
00143 if (pdef_ && pdef_->getGoal())
00144 {
00145 const base::PathPtr &p = pdef_->getGoal()->getSolutionPath();
00146 if (p)
00147 return static_cast<PathGeometric&>(*p);
00148 }
00149 throw Exception("No solution path");
00150 }
00151
00152 ompl::base::PlannerData ompl::geometric::SimpleSetup::getPlannerData(void) const
00153 {
00154 base::PlannerData pd;
00155 if (planner_)
00156 planner_->getPlannerData(pd);
00157 return pd;
00158 }
00159
00160 void ompl::geometric::SimpleSetup::print(std::ostream &out) const
00161 {
00162 if (si_)
00163 {
00164 si_->printSettings(out);
00165 si_->printProperties(out);
00166 }
00167 if (pdef_)
00168 pdef_->print(out);
00169 }