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/tools/multiplan/OptimizePlan.h"
00038 #include "ompl/geometric/PathSimplifier.h"
00039
00040 void ompl::tools::OptimizePlan::addPlanner(const base::PlannerPtr &planner)
00041 {
00042 if (planner && planner->getSpaceInformation().get() != getProblemDefinition()->getSpaceInformation().get())
00043 throw Exception("Planner instance does not match space information");
00044 planners_.push_back(planner);
00045 }
00046
00047 void ompl::tools::OptimizePlan::addPlannerAllocator(const base::PlannerAllocator &pa)
00048 {
00049 planners_.push_back(pa(getProblemDefinition()->getSpaceInformation()));
00050 }
00051
00052 void ompl::tools::OptimizePlan::clearPlanners(void)
00053 {
00054 planners_.clear();
00055 }
00056
00057 bool ompl::tools::OptimizePlan::solve(double solveTime, unsigned int maxSol, unsigned int nthreads)
00058 {
00059 time::point end = time::now() + time::seconds(solveTime);
00060 unsigned int nt = std::min(nthreads, (unsigned int)planners_.size());
00061 msg_.debug("Using %u threads", nt);
00062
00063 bool result = false;
00064 unsigned int np = 0;
00065 const base::GoalPtr &goal = getProblemDefinition()->getGoal();
00066 pp_.clearHybridizationPaths();
00067
00068 while (time::now() < end)
00069 {
00070 pp_.clearPlanners();
00071 for (unsigned int i = 0 ; i < nt ; ++i)
00072 {
00073 planners_[np]->clear();
00074 pp_.addPlanner(planners_[np]);
00075 np = (np + 1) % planners_.size();
00076 }
00077 if (pp_.solve(std::max(time::seconds(end - time::now()), 0.0), true))
00078 {
00079 result = true;
00080 if (goal->getSolutionPath()->length() <= goal->getMaximumPathLength())
00081 {
00082 msg_.debug("Terminating early since solution path is shorted than the maximum path length");
00083 break;
00084 }
00085 if (goal->getSolutionCount() >= maxSol)
00086 {
00087 msg_.debug("Terminating early since %u solutions were generated", maxSol);
00088 break;
00089 }
00090 }
00091 }
00092
00093
00094 if (time::now() < end && result)
00095 {
00096 geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric*>(goal->getSolutionPath().get());
00097 if (p)
00098 {
00099 geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation());
00100 ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0));
00101 }
00102 }
00103
00104 return result;
00105 }