00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/planners/rrt/pRRT.h"
00038 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include "ompl/tools/config/SelfConfig.h"
00041 #include <boost/thread/thread.hpp>
00042 #include <limits>
00043
00044 void ompl::geometric::pRRT::setup(void)
00045 {
00046 Planner::setup();
00047 SelfConfig sc(si_, getName());
00048 sc.configurePlannerRange(maxDistance_);
00049
00050 if (!nn_)
00051 nn_.reset(new NearestNeighborsGNAT<Motion*>());
00052 nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
00053 }
00054
00055 void ompl::geometric::pRRT::clear(void)
00056 {
00057 Planner::clear();
00058 samplerArray_.clear();
00059 freeMemory();
00060 if (nn_)
00061 nn_->clear();
00062 }
00063
00064 void ompl::geometric::pRRT::freeMemory(void)
00065 {
00066 if (nn_)
00067 {
00068 std::vector<Motion*> motions;
00069 nn_->list(motions);
00070 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00071 {
00072 if (motions[i]->state)
00073 si_->freeState(motions[i]->state);
00074 delete motions[i];
00075 }
00076 }
00077 }
00078
00079 void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
00080 {
00081 checkValidity();
00082 base::Goal *goal = pdef_->getGoal().get();
00083 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00084 RNG rng;
00085
00086 Motion *rmotion = new Motion(si_);
00087 base::State *rstate = rmotion->state;
00088 base::State *xstate = si_->allocState();
00089
00090 while (sol->solution == NULL && ptc() == false)
00091 {
00092
00093 if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
00094 goal_s->sampleGoal(rstate);
00095 else
00096 samplerArray_[tid]->sampleUniform(rstate);
00097
00098
00099 nnLock_.lock();
00100 Motion *nmotion = nn_->nearest(rmotion);
00101 nnLock_.unlock();
00102 base::State *dstate = rstate;
00103
00104
00105 double d = si_->distance(nmotion->state, rstate);
00106 if (d > maxDistance_)
00107 {
00108 si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00109 dstate = xstate;
00110 }
00111
00112 if (si_->checkMotion(nmotion->state, dstate))
00113 {
00114
00115 Motion *motion = new Motion(si_);
00116 si_->copyState(motion->state, dstate);
00117 motion->parent = nmotion;
00118
00119 nnLock_.lock();
00120 nn_->add(motion);
00121 nnLock_.unlock();
00122
00123 double dist = 0.0;
00124 bool solved = goal->isSatisfied(motion->state, &dist);
00125 if (solved)
00126 {
00127 sol->lock.lock();
00128 sol->approxdif = dist;
00129 sol->solution = motion;
00130 sol->lock.unlock();
00131 break;
00132 }
00133 if (dist < sol->approxdif)
00134 {
00135 sol->lock.lock();
00136 if (dist < sol->approxdif)
00137 {
00138 sol->approxdif = dist;
00139 sol->approxsol = motion;
00140 }
00141 sol->lock.unlock();
00142 }
00143 }
00144 }
00145
00146 si_->freeState(xstate);
00147 if (rmotion->state)
00148 si_->freeState(rmotion->state);
00149 delete rmotion;
00150 }
00151
00152 bool ompl::geometric::pRRT::solve(const base::PlannerTerminationCondition &ptc)
00153 {
00154 base::GoalRegion *goal = dynamic_cast<base::GoalRegion*>(pdef_->getGoal().get());
00155
00156 if (!goal)
00157 {
00158 msg_.error("Goal undefined");
00159 return false;
00160 }
00161
00162 samplerArray_.resize(threadCount_);
00163
00164 while (const base::State *st = pis_.nextStart())
00165 {
00166 Motion *motion = new Motion(si_);
00167 si_->copyState(motion->state, st);
00168 nn_->add(motion);
00169 }
00170
00171 if (nn_->size() == 0)
00172 {
00173 msg_.error("There are no valid initial states!");
00174 return false;
00175 }
00176
00177 msg_.inform("Starting with %u states", nn_->size());
00178
00179 SolutionInfo sol;
00180 sol.solution = NULL;
00181 sol.approxsol = NULL;
00182 sol.approxdif = std::numeric_limits<double>::infinity();
00183
00184 std::vector<boost::thread*> th(threadCount_);
00185 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00186 th[i] = new boost::thread(boost::bind(&pRRT::threadSolve, this, i, ptc, &sol));
00187 for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00188 {
00189 th[i]->join();
00190 delete th[i];
00191 }
00192
00193 bool approximate = false;
00194 if (sol.solution == NULL)
00195 {
00196 sol.solution = sol.approxsol;
00197 approximate = true;
00198 }
00199
00200 if (sol.solution != NULL)
00201 {
00202
00203 std::vector<Motion*> mpath;
00204 while (sol.solution != NULL)
00205 {
00206 mpath.push_back(sol.solution);
00207 sol.solution = sol.solution->parent;
00208 }
00209
00210
00211 PathGeometric *path = new PathGeometric(si_);
00212 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00213 path->states.push_back(si_->cloneState(mpath[i]->state));
00214
00215 goal->setDifference(sol.approxdif);
00216 goal->setSolutionPath(base::PathPtr(path), approximate);
00217
00218 if (approximate)
00219 msg_.warn("Found approximate solution");
00220 }
00221
00222 msg_.inform("Created %u states", nn_->size());
00223
00224 return goal->isAchieved();
00225 }
00226
00227 void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
00228 {
00229 Planner::getPlannerData(data);
00230
00231 std::vector<Motion*> motions;
00232 if (nn_)
00233 nn_->list(motions);
00234
00235 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00236 data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00237 }
00238
00239 void ompl::geometric::pRRT::setThreadCount(unsigned int nthreads)
00240 {
00241 assert(nthreads > 0);
00242 threadCount_ = nthreads;
00243 }