void ompl::geometric::BasicPRMmodif::growRoadmap(const std::vector<Milestone*> &start,
const std::vector<Milestone*> &goal,
const base::PlannerTerminationCondition &ptc,
base::State *workState)
{
while (ptc() == false)
{
// search for a valid state
bool found = false;
while (!found && ptc() == false)
{
unsigned int attempts = 0;
do
{
found = sampler_->sample(workState);
attempts++;
} while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
}
// add it as a milestone
if (found)
{
addMilestone(si_->cloneState(workState));
if (haveSolution(start, goal))
break;
}
}
}
void ompl::geometric::BasicPRMmodif::growRoadmap(double growTime)
{
time::point endTime = time::now() + time::seconds(growTime);
base::State *workState = si_->allocState();
while (time::now() < endTime)
{
// search for a valid state
bool found = false;
while (!found && time::now() < endTime)
{
unsigned int attempts = 0;
do
{
found = sampler_->sample(workState);
attempts++;
} while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
}
// add it as a milestone
if (found)
addMilestone(si_->cloneState(workState));
}
si_->freeState(workState);
}
ompl::base::PlannerStatus ompl::geometric::LazyPRM::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
// Add the valid start states as milestones
while (const base::State *st = pis_.nextStart())
startM_.push_back(addMilestone(si_->cloneState(st)));
if (startM_.size() == 0)
{
OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
return base::PlannerStatus::INVALID_START;
}
if (!goal->couldSample())
{
OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
// Ensure there is at least one valid goal state
if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
{
const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
goalM_.push_back(addMilestone(si_->cloneState(st)));
if (goalM_.empty())
{
OMPL_ERROR("%s: Unable to find any valid goal states", getName().c_str());
return base::PlannerStatus::INVALID_GOAL;
}
}
unsigned long int nrStartStates = boost::num_vertices(g_);
OMPL_INFORM("%s: Starting planning with %lu states already in datastructure", getName().c_str(), nrStartStates);
bestCost_ = opt_->infiniteCost();
base::State *workState = si_->allocState();
std::pair<std::size_t, std::size_t> startGoalPair;
base::PathPtr bestSolution;
bool fullyOptimized = false;
bool someSolutionFound = false;
unsigned int optimizingComponentSegments = 0;
// Grow roadmap in lazy fashion -- add vertices and edges without checking validity
while (ptc == false)
{
++iterations_;
sampler_->sampleUniform(workState);
Vertex addedVertex = addMilestone(si_->cloneState(workState));
const long int solComponent = solutionComponent(&startGoalPair);
// If the start & goal are connected and we either did not find any solution
// so far or the one we found still needs optimizing and we just added an edge
// to the connected component that is used for the solution, we attempt to
// construct a new solution.
if (solComponent != -1 && (!someSolutionFound || (long int)vertexComponentProperty_[addedVertex] == solComponent))
{
// If we already have a solution, we are optimizing. We check that we added at least
// a few segments to the connected component that includes the previously found
// solution before attempting to construct a new solution.
if (someSolutionFound)
{
if (++optimizingComponentSegments < magic::MIN_ADDED_SEGMENTS_FOR_LAZY_OPTIMIZATION)
continue;
optimizingComponentSegments = 0;
}
Vertex startV = startM_[startGoalPair.first];
Vertex goalV = goalM_[startGoalPair.second];
base::PathPtr solution;
do
{
solution = constructSolution(startV, goalV);
} while (!solution && vertexComponentProperty_[startV] == vertexComponentProperty_[goalV]);
if (solution)
{
someSolutionFound = true;
base::Cost c = solution->cost(opt_);
if (opt_->isSatisfied(c))
{
fullyOptimized = true;
bestSolution = solution;
bestCost_ = c;
break;
}
else
{
if (opt_->isCostBetterThan(c, bestCost_))
{
bestSolution = solution;
bestCost_ = c;
}
}
}
}
}
si_->freeState(workState);
if (bestSolution)
{
base::PlannerSolution psol(bestSolution);
psol.setPlannerName(getName());
// if the solution was optimized, we mark it as such
psol.setOptimized(opt_, bestCost_, fullyOptimized);
pdef_->addSolutionPath(psol);
}
OMPL_INFORM("%s: Created %u states", getName().c_str(), boost::num_vertices(g_) - nrStartStates);
return bestSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
}
bool ompl::geometric::BasicPRMmodif::solve(const base::PlannerTerminationCondition &ptc)
{
pis_.checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
if (!goal)
{
msg_.error("Goal undefined or unknown type of goal");
return false;
}
std::vector<Milestone*> startM;
std::vector<Milestone*> goalM;
// add the valid start states as milestones
while (const base::State *st = pis_.nextStart())
startM.push_back(addMilestone(si_->cloneState(st)));
if (startM.size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!goal->canSample())
{
msg_.error("Insufficient states in sampleable goal region");
return false;
}
if (!sampler_)
sampler_ = si_->allocValidStateSampler();
unsigned int nrStartStates = milestones_.size();
msg_.inform("Starting with %u states", nrStartStates);
base::State *xstate = si_->allocState();
std::pair<Milestone*, Milestone*> solEndpoints;
while (ptc() == false)
{
// find at least one valid goal state
if (goal->maxSampleCount() > goalM.size())
{
const base::State *st = goalM.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
if (st)
goalM.push_back(addMilestone(si_->cloneState(st)));
if (goalM.empty())
{
msg_.error("Unable to find any valid goal states");
break;
}
}
// if there already is a solution, construct it
// if (haveSolution(startM, goalM, &solEndpoints))
// {
// constructSolution(solEndpoints.first, solEndpoints.second);
// break;
// }
// othewise, spend some time building a roadmap
else
{
// if it is worth looking at other goal regions, plan for part of the time
if (goal->maxSampleCount() > goalM.size())
growRoadmap(startM, goalM, boost::bind(&growRoadmapTerminationCondition, ptc, time::now() + time::seconds(0.1)), xstate);
// otherwise, just go ahead and build the roadmap
else
growRoadmap(startM, goalM, ptc, xstate);
// if a solution has been found, construct it
// if (haveSolution(startM, goalM, &solEndpoints))
// {
// constructSolution(solEndpoints.first, solEndpoints.second);
// break;
// }
}
}
if (haveSolution(startM, goalM, &solEndpoints))
{
constructSolution(solEndpoints.first, solEndpoints.second);
}
si_->freeState(xstate);
msg_.inform("Created %u states", milestones_.size() - nrStartStates);
return goal->isAchieved();
}
