void ompl::geometric::pSBL::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol) { RNG rng; std::vector<Motion*> solution; base::State *xstate = si_->allocState(); bool startTree = rng.uniformBool(); while (!sol->found && ptc == false) { bool retry = true; while (retry && !sol->found && ptc == false) { removeList_.lock.lock(); if (!removeList_.motions.empty()) { if (loopLock_.try_lock()) { retry = false; std::map<Motion*, bool> seen; for (unsigned int i = 0 ; i < removeList_.motions.size() ; ++i) if (seen.find(removeList_.motions[i].motion) == seen.end()) removeMotion(*removeList_.motions[i].tree, removeList_.motions[i].motion, seen); removeList_.motions.clear(); loopLock_.unlock(); } } else retry = false; removeList_.lock.unlock(); } if (sol->found || ptc) break; loopLockCounter_.lock(); if (loopCounter_ == 0) loopLock_.lock(); loopCounter_++; loopLockCounter_.unlock(); TreeData &tree = startTree ? tStart_ : tGoal_; startTree = !startTree; TreeData &otherTree = startTree ? tStart_ : tGoal_; Motion *existing = selectMotion(rng, tree); if (!samplerArray_[tid]->sampleNear(xstate, existing->state, maxDistance_)) continue; /* create a motion */ Motion *motion = new Motion(si_); si_->copyState(motion->state, xstate); motion->parent = existing; motion->root = existing->root; existing->lock.lock(); existing->children.push_back(motion); existing->lock.unlock(); addMotion(tree, motion); if (checkSolution(rng, !startTree, tree, otherTree, motion, solution)) { sol->lock.lock(); if (!sol->found) { sol->found = true; PathGeometric *path = new PathGeometric(si_); for (unsigned int i = 0 ; i < solution.size() ; ++i) path->append(solution[i]->state); pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName()); } sol->lock.unlock(); } loopLockCounter_.lock(); loopCounter_--; if (loopCounter_ == 0) loopLock_.unlock(); loopLockCounter_.unlock(); } si_->freeState(xstate); }