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