bool ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
checkValidity();
base::Goal *goal = pdef_->getGoal().get();
base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
parent_ = NULL;
if (!goal)
{
msg_.error("Goal undefined");
return false;
}
while (const base::State *st = pis_.nextStart())
{
Motion *motion = new Motion(si_);
si_->copyState(motion->state, st);
// TODO set this based on initial bearing
motion->set_bearing(initialBearing_);
nn_->add(motion);
}
if (nn_->size() == 0)
{
msg_.error("There are no valid initial states!");
return false;
}
if (!sampler_)
sampler_ = si_->allocStateSampler();
msg_.inform("Starting with %u states", nn_->size());
Motion *solution = NULL;
Motion *approximation = NULL;
double approximatedist = std::numeric_limits<double>::infinity();
bool sufficientlyShort = false;
Motion *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
std::vector<Motion*> solCheck;
std::vector<Motion*> nbh;
std::vector<double> dists;
std::vector<int> valid;
unsigned int rewireTest = 0;
while (ptc() == false)
{
bool validneighbor = false;
Motion *nmotion;
base::State *dstate;
//ADDED
parent_ = NULL;
//while (!validneighbor)
//{
if (ptc())
{
msg_.error("Loop failed to find proper states!");
return false;
}
// sample random state (with goal biasing)
if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
sampler_->sampleUniform(rstate);
// find closest state in the tree
nmotion = nn_->nearest(rmotion);
dstate = rstate;
// find state to add
double d = si_->distance(nmotion->state, rstate);
if (d > maxDistance_)
{
si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
dstate = xstate;
}
validneighbor = check_bearing(nmotion, dstate);
//}
parent_ = nmotion;
if (si_->checkMotion(nmotion->state, dstate))
{
// create a motion
//ADDED
double distN = si_->distance(dstate, nmotion->state);
Motion *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
motion->set_bearing();
motion->cost = nmotion->cost + distN;
// find nearby neighbors
double r = std::min(ballRadiusConst_ * (sqrt(log((double)(1 + nn_->size())) / ((double)(nn_->size())))),
ballRadiusMax_);
nn_->nearestR(motion, r, nbh);
rewireTest += nbh.size();
// cache for distance computations
dists.resize(nbh.size());
// cache for motion validity
valid.resize(nbh.size());
std::fill(valid.begin(), valid.end(), 0);
if(delayCC_)
{
// calculate all costs and distances
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
nbh[i]->cost += si_->distance(nbh[i]->state, dstate);
// sort the nodes
std::sort(nbh.begin(), nbh.end(), compareMotion);
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
nbh[i]->cost -= dists[i];
}
// collision check until a valid motion is found
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
if (nbh[i] != nmotion)
{
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
//ADDED
parent_ = nbh[i];
if (si_->checkMotion(nbh[i]->state, dstate) /*&& check_bearing(nbh[i], motion->state)*/)
{
motion->cost = c;
motion->parent = nbh[i];
motion->set_bearing();
valid[i] = 1;
break;
}
else
valid[i] = -1;
}
}
else
{
valid[i] = 1;
dists[i] = distN;
break;
}
}
}
else
{
// find which one we connect the new state to
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
{
if (nbh[i] != nmotion)
{
dists[i] = si_->distance(nbh[i]->state, dstate);
double c = nbh[i]->cost + dists[i];
if (c < motion->cost)
{
// ADDED
parent_ = nbh[i];
if (si_->checkMotion(nbh[i]->state, dstate) /*&& check_bearing(nbh[i], dstate)*/)
{
motion->cost = c;
motion->parent = nbh[i];
motion->set_bearing();
valid[i] = 1;
}
else
valid[i] = -1;
}
}
else
{
valid[i] = 1;
dists[i] = distN;
}
}
}
// add motion to the tree
nn_->add(motion);
motion->parent->children.push_back(motion);
solCheck.resize(1);
solCheck[0] = motion;
// rewire tree if needed
for (unsigned int i = 0 ; i < nbh.size() ; ++i)
if (nbh[i] != motion->parent)
{
double c = motion->cost + dists[i];
if ((c < nbh[i]->cost))
{
parent_ = motion;
bool v = valid[i] == 0 ? si_->checkMotion(nbh[i]->state, dstate) : valid[i] == 1;
if (v /*&& check_bearing(motion, nbh[i]->state)*/)
{
// Remove this node from its parent list
removeFromParent (nbh[i]);
double delta = c - nbh[i]->cost;
// Add this node to the new parent
nbh[i]->parent = motion;
nbh[i]->cost = c;
nbh[i]->set_bearing();
nbh[i]->parent->children.push_back(nbh[i]);
solCheck.push_back(nbh[i]);
// Update the costs of the node's children
updateChildCosts(nbh[i], delta);
}
}
}
// check if we found a solution
for (unsigned int i = 0 ; i < solCheck.size() ; ++i)
{
double dist = 0.0;
bool solved = goal->isSatisfied(solCheck[i]->state, &dist);
sufficientlyShort = solved ? goal->isPathLengthSatisfied(solCheck[i]->cost) : false;
if (solved)
{
if (sufficientlyShort)
{
solution = solCheck[i];
break;
}
else if (!solution || (solCheck[i]->cost < solution->cost))
{
solution = solCheck[i];
}
}
else if (!solution && dist < approximatedist)
{
approximation = solCheck[i];
approximatedist = dist;
}
}
}
// terminate if a sufficient solution is found
if (solution && sufficientlyShort)
break;
}
double solutionCost;
bool approximate = (solution == NULL);
bool addedSolution = false;
if (approximate)
{
solution = approximation;
solutionCost = approximatedist;
}
else
solutionCost = solution->cost;
if (solution != NULL)
{
// construct the solution path
std::vector<Motion*> mpath;
while (solution != NULL)
{
mpath.push_back(solution);
solution = solution->parent;
}
// set the solution path
PathGeometric *path = new PathGeometric(si_);
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
path->append(mpath[i]->state);
goal->addSolutionPath(base::PathPtr(path), approximate, solutionCost);
addedSolution = true;
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
msg_.inform("Created %u states. Checked %lu rewire options.", nn_->size(), rewireTest);
return addedSolution;
}
