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