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/prm/BasicPRM.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00040 #include <algorithm>
00041 #include <queue>
00042 #include <limits>
00043
00047 static const unsigned int FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK = 2;
00048
00049
00050 void ompl::geometric::BasicPRM::setup(void)
00051 {
00052 Planner::setup();
00053 if (!nn_)
00054 nn_.reset(new NearestNeighborsSqrtApprox<Milestone*>());
00055 nn_->setDistanceFunction(boost::bind(&BasicPRM::distanceFunction, this, _1, _2));
00056 }
00057
00058 void ompl::geometric::BasicPRM::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)
00059 {
00060 Planner::setProblemDefinition(pdef);
00061 startM_.clear();
00062 goalM_.clear();
00063 }
00064
00065 void ompl::geometric::BasicPRM::clear(void)
00066 {
00067 Planner::clear();
00068 sampler_.reset();
00069 freeMemory();
00070 if (nn_)
00071 nn_->clear();
00072 milestones_.clear();
00073 startM_.clear();
00074 goalM_.clear();
00075 componentCount_ = 0;
00076 componentSizes_.clear();
00077 lastStart_ = NULL;
00078 lastGoal_ = NULL;
00079 }
00080
00081 void ompl::geometric::BasicPRM::freeMemory(void)
00082 {
00083 for (unsigned int i = 0 ; i < milestones_.size() ; ++i)
00084 {
00085 if (milestones_[i]->state)
00086 si_->freeState(milestones_[i]->state);
00087 delete milestones_[i];
00088 }
00089 }
00090
00091 void ompl::geometric::BasicPRM::growRoadmap(double growTime)
00092 {
00093 time::point endTime = time::now() + time::seconds(growTime);
00094 base::State *workState = si_->allocState();
00095 while (time::now() < endTime)
00096 {
00097
00098 bool found = false;
00099 while (!found && time::now() < endTime)
00100 {
00101 unsigned int attempts = 0;
00102 do
00103 {
00104 found = sampler_->sample(workState);
00105 attempts++;
00106 } while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00107 }
00108
00109 if (found)
00110 addMilestone(si_->cloneState(workState));
00111 }
00112 si_->freeState(workState);
00113 }
00114
00115 void ompl::geometric::BasicPRM::growRoadmap(const std::vector<Milestone*> &start,
00116 const std::vector<Milestone*> &goal,
00117 const base::PlannerTerminationCondition &ptc,
00118 base::State *workState)
00119 {
00120 while (ptc() == false)
00121 {
00122
00123 bool found = false;
00124 while (!found && ptc() == false)
00125 {
00126 unsigned int attempts = 0;
00127 do
00128 {
00129 found = sampler_->sample(workState);
00130 attempts++;
00131 } while (attempts < FIND_VALID_STATE_ATTEMPTS_WITHOUT_TIME_CHECK && !found);
00132 }
00133
00134 if (found)
00135 {
00136 addMilestone(si_->cloneState(workState));
00137 if (haveSolution(start, goal))
00138 break;
00139 }
00140 }
00141 }
00142
00143 bool ompl::geometric::BasicPRM::haveSolution(const std::vector<Milestone*> &start, const std::vector<Milestone*> &goal,
00144 std::pair<Milestone*, Milestone*> *endpoints)
00145 {
00146 base::Goal *g = pdef_->getGoal().get();
00147 for (unsigned int i = 0 ; i < goal.size() ; ++i)
00148 for (unsigned int j = 0 ; j < start.size() ; ++j)
00149 if (goal[i]->component == start[j]->component && g->isStartGoalPairValid(goal[i]->state, start[j]->state))
00150 {
00151 if (endpoints)
00152 {
00153 endpoints->first = start[j];
00154 endpoints->second = goal[i];
00155 }
00156 return true;
00157 }
00158 return false;
00159 }
00160
00161 namespace ompl
00162 {
00163
00164 static bool growRoadmapTerminationCondition(const base::PlannerTerminationCondition &ptc, const time::point &endTime)
00165 {
00166 return ptc() || time::now() >= endTime;
00167 }
00168 }
00169
00170 bool ompl::geometric::BasicPRM::solve(const base::PlannerTerminationCondition &ptc)
00171 {
00172 checkValidity();
00173 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00174
00175 if (!goal)
00176 {
00177 msg_.error("Goal undefined or unknown type of goal");
00178 return false;
00179 }
00180
00181 unsigned int nrStartStates = milestones_.size();
00182
00183
00184 while (const base::State *st = pis_.nextStart())
00185 startM_.push_back(addMilestone(si_->cloneState(st)));
00186
00187 if (startM_.size() == 0)
00188 {
00189 msg_.error("There are no valid initial states!");
00190 return false;
00191 }
00192
00193 if (!goal->canSample())
00194 {
00195 msg_.error("Insufficient states in sampleable goal region");
00196 return false;
00197 }
00198
00199 if (!sampler_)
00200 sampler_ = si_->allocValidStateSampler();
00201
00202 msg_.inform("Starting with %u states", nrStartStates);
00203
00204 base::State *xstate = si_->allocState();
00205 std::pair<Milestone*, Milestone*> solEndpoints;
00206
00207 while (ptc() == false)
00208 {
00209
00210 if (goal->maxSampleCount() > goalM_.size() || goalM_.empty())
00211 {
00212 const base::State *st = goalM_.empty() ? pis_.nextGoal(ptc) : pis_.nextGoal();
00213 if (st)
00214 goalM_.push_back(addMilestone(si_->cloneState(st)));
00215 if (goalM_.empty())
00216 {
00217 msg_.error("Unable to find any valid goal states");
00218 break;
00219 }
00220 }
00221
00222
00223 if (haveSolution(startM_, goalM_, &solEndpoints))
00224 {
00225 constructSolution(solEndpoints.first, solEndpoints.second);
00226 break;
00227 }
00228
00229 else
00230 {
00231
00232 if (goal->maxSampleCount() > goalM_.size())
00233 growRoadmap(startM_, goalM_, boost::bind(&growRoadmapTerminationCondition, ptc, time::now() + time::seconds(0.1)), xstate);
00234
00235 else
00236 growRoadmap(startM_, goalM_, ptc, xstate);
00237
00238 if (haveSolution(startM_, goalM_, &solEndpoints))
00239 {
00240 constructSolution(solEndpoints.first, solEndpoints.second);
00241 break;
00242 }
00243 }
00244 }
00245 si_->freeState(xstate);
00246
00247 msg_.inform("Created %u states", milestones_.size() - nrStartStates);
00248
00249 return goal->isAchieved();
00250 }
00251
00252 void ompl::geometric::BasicPRM::nearestNeighbors(Milestone *milestone, std::vector<Milestone*> &nbh)
00253 {
00254 nn_->nearestK(milestone, maxNearestNeighbors_, nbh);
00255 }
00256
00257 ompl::geometric::BasicPRM::Milestone* ompl::geometric::BasicPRM::addMilestone(base::State *state)
00258 {
00259 Milestone *m = new Milestone();
00260 m->state = state;
00261 m->component = componentCount_;
00262 componentSizes_[m->component] = 1;
00263
00264
00265 std::vector<Milestone*> nbh;
00266 nearestNeighbors(m, nbh);
00267
00268 for (unsigned int i = 0 ; i < nbh.size() ; ++i)
00269 if (si_->checkMotion(m->state, nbh[i]->state))
00270 {
00271 m->adjacent.push_back(nbh[i]);
00272 nbh[i]->adjacent.push_back(m);
00273 m->costs.push_back(si_->distance(m->state, nbh[i]->state));
00274 nbh[i]->costs.push_back(m->costs.back());
00275 uniteComponents(m, nbh[i]);
00276 }
00277
00278
00279
00280 if (m->component == componentCount_)
00281 componentCount_++;
00282 m->index = milestones_.size();
00283 milestones_.push_back(m);
00284 nn_->add(m);
00285 return m;
00286 }
00287
00288 void ompl::geometric::BasicPRM::uniteComponents(Milestone *m1, Milestone *m2)
00289 {
00290 if (m1->component == m2->component)
00291 return;
00292
00293 if (componentSizes_[m1->component] > componentSizes_[m2->component])
00294 std::swap(m1, m2);
00295
00296 const unsigned long c = m2->component;
00297 componentSizes_[c] += componentSizes_[m1->component];
00298 componentSizes_.erase(m1->component);
00299
00300 std::queue<Milestone*> q;
00301 q.push(m1);
00302
00303 while (!q.empty())
00304 {
00305 Milestone *m = q.front();
00306 q.pop();
00307 if (m->component != c)
00308 {
00309 m->component = c;
00310 for (unsigned int i = 0 ; i < m->adjacent.size() ; ++i)
00311 if (m->adjacent[i]->component != c)
00312 q.push(m->adjacent[i]);
00313 }
00314 }
00315 }
00316
00317 void ompl::geometric::BasicPRM::reconstructLastSolution(void)
00318 {
00319 if (lastStart_ && lastGoal_)
00320 constructSolution(lastStart_, lastGoal_);
00321 else
00322 msg_.warn("There is no solution to reconstruct");
00323 }
00324
00325 void ompl::geometric::BasicPRM::constructSolution(const Milestone* start, const Milestone* goal)
00326 {
00327 const unsigned int N = milestones_.size();
00328 std::vector<double> dist(N, std::numeric_limits<double>::infinity());
00329 std::vector<int> prev(N, -1);
00330 std::vector<int> seen(N, 0);
00331
00332 dist[goal->index] = 0.0;
00333 for (unsigned int i = 0 ; i < N ; ++i)
00334 {
00335 double minDist = std::numeric_limits<double>::infinity();
00336 int index = -1;
00337 for (unsigned int j = 0 ; j < N ; ++j)
00338 if (seen[j] == 0 && dist[j] < minDist)
00339 {
00340 minDist = dist[j];
00341 index = j;
00342 }
00343 if (index < 0)
00344 break;
00345 seen[index] = 1;
00346
00347 for (unsigned int j = 0 ; j < milestones_[index]->adjacent.size() ; ++j)
00348 {
00349 const unsigned int idx = milestones_[index]->adjacent[j]->index;
00350 double altDist = dist[index] + milestones_[index]->costs[j];
00351 if (altDist < dist[idx])
00352 {
00353 dist[idx] = altDist;
00354 prev[idx] = index;
00355 }
00356 }
00357 }
00358 if (prev[start->index] >= 0)
00359 {
00360 PathGeometric *p = new PathGeometric(si_);
00361 int pos = start->index;
00362 do
00363 {
00364 p->states.push_back(si_->cloneState(milestones_[pos]->state));
00365 pos = prev[pos];
00366 } while (pos >= 0);
00367 pdef_->getGoal()->setSolutionPath(base::PathPtr(p));
00368 lastStart_ = start;
00369 lastGoal_ = goal;
00370 }
00371 else
00372 throw Exception(name_, "Internal error in computing shortest path");
00373 }
00374
00375 void ompl::geometric::BasicPRM::getPlannerData(base::PlannerData &data) const
00376 {
00377 Planner::getPlannerData(data);
00378
00379 for (unsigned int i = 0 ; i < milestones_.size() ; ++i)
00380 for (unsigned int j = 0 ; j < milestones_[i]->adjacent.size() ; ++j)
00381 {
00382 data.recordEdge(milestones_[i]->state, milestones_[i]->adjacent[j]->state);
00383 data.tagState(milestones_[i]->state, milestones_[i]->component);
00384 }
00385 }