void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc,
SolutionInfo *sol)
{
base::Goal *goal = pdef_->getGoal().get();
auto *goal_s = dynamic_cast<base::GoalSampleableRegion *>(goal);
RNG rng;
auto *rmotion = new Motion(si_);
base::State *rstate = rmotion->state;
base::State *xstate = si_->allocState();
while (sol->solution == nullptr && ptc == false)
{
/* sample random state (with goal biasing) */
if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
goal_s->sampleGoal(rstate);
else
samplerArray_[tid]->sampleUniform(rstate);
/* find closest state in the tree */
nnLock_.lock();
Motion *nmotion = nn_->nearest(rmotion);
nnLock_.unlock();
base::State *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;
}
if (si_->checkMotion(nmotion->state, dstate))
{
/* create a motion */
auto *motion = new Motion(si_);
si_->copyState(motion->state, dstate);
motion->parent = nmotion;
nnLock_.lock();
nn_->add(motion);
nnLock_.unlock();
double dist = 0.0;
bool solved = goal->isSatisfied(motion->state, &dist);
if (solved)
{
sol->lock.lock();
sol->approxdif = dist;
sol->solution = motion;
sol->lock.unlock();
break;
}
if (dist < sol->approxdif)
{
sol->lock.lock();
if (dist < sol->approxdif)
{
sol->approxdif = dist;
sol->approxsol = motion;
}
sol->lock.unlock();
}
}
}
si_->freeState(xstate);
if (rmotion->state)
si_->freeState(rmotion->state);
delete rmotion;
}
