ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc) { using solveFunctionType = void (ompl::geometric::CForest::*)(base::Planner *, const base::PlannerTerminationCondition &); checkValidity(); time::point start = time::now(); std::vector<std::thread*> threads(planners_.size()); const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback(); if (prevSolutionCallback) OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str()); pdef_->setIntermediateSolutionCallback(std::bind(&CForest::newSolutionFound, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); bestCost_ = opt_->infiniteCost(); // run each planner in its own thread, with the same ptc. for (std::size_t i = 0 ; i < threads.size() ; ++i) threads[i] = new std::thread(std::bind((solveFunctionType)&CForest::solve, this, planners_[i].get(), ptc)); for (auto & thread : threads) { thread->join(); delete thread; } // restore callback getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback); OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start)); return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution()); }
ompl::base::PlannerStatus ompl::tools::OptimizePlan::solve(double solveTime, unsigned int maxSol, unsigned int nthreads) { time::point end = time::now() + time::seconds(solveTime); unsigned int nt = std::min(nthreads, (unsigned int)planners_.size()); OMPL_DEBUG("Using %u threads", nt); base::PlannerStatus result; unsigned int np = 0; const base::ProblemDefinitionPtr &pdef = getProblemDefinition(); pp_.clearHybridizationPaths(); while (time::now() < end) { pp_.clearPlanners(); for (unsigned int i = 0; i < nt; ++i) { planners_[np]->clear(); pp_.addPlanner(planners_[np]); np = (np + 1) % planners_.size(); } base::PlannerStatus localResult = pp_.solve(std::max(time::seconds(end - time::now()), 0.0), true); if (localResult) { if (result != base::PlannerStatus::EXACT_SOLUTION) result = localResult; if (!pdef->hasOptimizationObjective()) { OMPL_DEBUG("Terminating early since there is no optimization objective specified"); break; } base::Cost obj_cost = pdef->getSolutionPath()->cost(pdef->getOptimizationObjective()); if (pdef->getOptimizationObjective()->isSatisfied(obj_cost)) { OMPL_DEBUG("Terminating early since solution path satisfies the optimization objective"); break; } if (pdef->getSolutionCount() >= maxSol) { OMPL_DEBUG("Terminating early since %u solutions were generated", maxSol); break; } } } // if we have more time, and we have a geometric path, we try to simplify it if (time::now() < end && result) { geometric::PathGeometric *p = dynamic_cast<geometric::PathGeometric *>(pdef->getSolutionPath().get()); if (p) { geometric::PathSimplifier ps(getProblemDefinition()->getSpaceInformation()); ps.simplify(*p, std::max(time::seconds(end - time::now()), 0.0)); } } return result; }
ompl::base::PlannerStatus ompl::geometric::CForest::solve(const base::PlannerTerminationCondition &ptc) { checkValidity(); time::point start = time::now(); std::vector<std::thread *> threads(planners_.size()); const base::ReportIntermediateSolutionFn prevSolutionCallback = getProblemDefinition()->getIntermediateSolutionCallback(); if (prevSolutionCallback) OMPL_WARN("Cannot use previously set intermediate solution callback with %s", getName().c_str()); pdef_->setIntermediateSolutionCallback( [this](const base::Planner *planner, const std::vector<const base::State *> &states, const base::Cost cost) { return newSolutionFound(planner, states, cost); }); bestCost_ = opt_->infiniteCost(); // run each planner in its own thread, with the same ptc. for (std::size_t i = 0; i < threads.size(); ++i) { base::Planner *planner = planners_[i].get(); threads[i] = new std::thread([this, planner, &ptc] { return solve(planner, ptc); }); } for (auto &thread : threads) { thread->join(); delete thread; } // restore callback getProblemDefinition()->setIntermediateSolutionCallback(prevSolutionCallback); OMPL_INFORM("Solution found in %f seconds", time::seconds(time::now() - start)); return base::PlannerStatus(pdef_->hasSolution(), pdef_->hasApproximateSolution()); }
KoulesSetup::KoulesSetup(unsigned int numKoules, const std::string& plannerName, double kouleVel) : ompl::control::SimpleSetup(ompl::control::ControlSpacePtr(new KoulesControlSpace(numKoules))) { initialize(numKoules, plannerName); double* state = getProblemDefinition()->getStartState(0)->as<KoulesStateSpace::StateType>()->values; double theta; ompl::RNG rng; for (unsigned int i = 0; i < numKoules; ++i) { theta = rng.uniformReal(0., 2. * boost::math::constants::pi<double>()); state[4 * i + 7] = kouleVel * cos(theta); state[4 * i + 8] = kouleVel * sin(theta); } }
void ompl::tools::OptimizePlan::addPlannerAllocator(const base::PlannerAllocator &pa) { planners_.push_back(pa(getProblemDefinition()->getSpaceInformation())); }
void ompl::tools::OptimizePlan::addPlanner(const base::PlannerPtr &planner) { if (planner && planner->getSpaceInformation().get() != getProblemDefinition()->getSpaceInformation().get()) throw Exception("Planner instance does not match space information"); planners_.push_back(planner); }