移动机器人D*Lite路径规划算法源码

/* Dstar.cpp * James Neufeld (neufeld@cs.ualberta.ca) */ #include "Dstar.h" #include <stdio.h> #ifdef USE_OPEN_GL #ifdef MACOS #include <OpenGL/gl.h> #else #include <GL/gl.h> #endif #endif /* void Dstar::Dstar() * -------------------------- * Constructor sets constants. */ Dstar::Dstar() { maxSteps = 80000; // node expansions before we give up C1 = 1; // cost of an unseen cell } /* float Dstar::keyHashCode(state u) * -------------------------- * Returns the key hash code for the state u, this is used to compare * a state that have been updated */ float Dstar::keyHashCode(state u) { return (float)(u.k.first + 1193*u.k.second); } /* bool Dstar::isValid(state u) * -------------------------- * Returns true if state u is on the open list or not by checking if * it is in the hash table. */ bool Dstar::isValid(state u) { ds_oh::iterator cur = openHash.find(u); if (cur == openHash.end()) return false; if (!close(keyHashCode(u), cur->second)) return false; return true; } /* void Dstar::getPath() * -------------------------- * Returns the path created by replan() */ list<state> Dstar::getPath() { return path; } /* bool Dstar::occupied(state u) * -------------------------- * returns true if the cell is occupied (non-traversable), false * otherwise. non-traversable are marked with a cost < 0. */ bool Dstar::occupied(state u) { ds_ch::iterator cur = cellHash.find(u); if (cur == cellHash.end()) return false; return (cur->second.cost < 0); } /* void Dstar::init(int sX, int sY, int gX, int gY) * -------------------------- * Init dstar with start and goal coordinates, rest is as per * [S. Koenig, 2002] */ void Dstar::init(int sX, int sY, int gX, int gY) { cellHash.clear(); path.clear(); openHash.clear(); while(!openList.empty()) openList.pop(); k_m = 0; s_start.x = sX; s_start.y = sY; s_goal.x = gX; s_goal.y = gY; cellInfo tmp; tmp.g = tmp.rhs = 0; tmp.cost = C1; cellHash[s_goal] = tmp; tmp.g = tmp.rhs = heuristic(s_start,s_goal); tmp.cost = C1; cellHash[s_start] = tmp; s_start = calculateKey(s_start); s_last = s_start; } /* void Dstar::makeNewCell(state u) * -------------------------- * Checks if a cell is in the hash table, if not it adds it in. */ void Dstar::makeNewCell(state u) { if (cellHash.find(u) != cellHash.end()) return; cellInfo tmp; tmp.g = tmp.rhs = heuristic(u,s_goal); tmp.cost = C1; cellHash[u] = tmp; } /* double Dstar::getG(state u) * -------------------------- * Returns the G value for state u. */ double Dstar::getG(state u) { if (cellHash.find(u) == cellHash.end()) return heuristic(u,s_goal); return cellHash[u].g; } /* double Dstar::getRHS(state u) * -------------------------- * Returns the rhs value for state u. */ double Dstar::getRHS(state u) { if (u == s_goal) return 0; if (cellHash.find(u) == cellHash.end()) return heuristic(u,s_goal); return cellHash[u].rhs; } /* void Dstar::setG(state u, double g) * -------------------------- * Sets the G value for state u */ void Dstar::setG(state u, double g) { makeNewCell(u); cellHash[u].g = g; } /* void Dstar::setRHS(state u, double rhs) * -------------------------- * Sets the rhs value for state u */ double Dstar::setRHS(state u, double rhs) { makeNewCell(u); cellHash[u].rhs = rhs; } /* double Dstar::eightCondist(state a, state b) * -------------------------- * Returns the 8-way distance between state a and state b. */ double Dstar::eightCondist(state a, state b) { double temp; double min = abs(a.x - b.x); double max = abs(a.y - b.y); if (min > max) { double temp = min; min = max; max = temp; } return ((M_SQRT2-1.0)*min + max); } /* int Dstar::computeShortestPath() * -------------------------- * As per [S. Koenig, 2002] except for 2 main modifications: * 1. We stop planning after a number of steps, 'maxsteps' we do this * because this algorithm can plan forever if the start is * surrounded by obstacles. * 2. We lazily remove states from the open list so we never have to * iterate through it. */ int Dstar::computeShortestPath() { list<state> s; list<state>::iterator i; if (openList.empty()) return 1; int k=0; while ((!openList.empty()) && (openList.top() < (s_start = calculateKey(s_start))) || (getRHS(s_start) != getG(s_start))) { if (k++ > maxSteps) { fprintf(stderr, "At maxsteps\n"); return -1; } state u; bool test = (getRHS(s_start) != getG(s_start)); // lazy remove while(1) { if (openList.empty()) return 1; u = openList.top(); openList.pop(); if (!isValid(u)) continue; if (!(u < s_start) && (!test)) return 2; break; } ds_oh::iterator cur = openHash.find(u); openHash.erase(cur); state k_old = u; if (k_old < calculateKey(u)) { // u is out of date insert(u); } else if (getG(u) > getRHS(u)) { // needs update (got better) setG(u,getRHS(u)); getPred(u,s); for (i=s.begin();i != s.end(); i++) { updateVertex(*i); } } else { // g <= rhs, state has got worse setG(u,INFINITY); getPred(u,s); for (i=s.begin();i != s.end(); i++) { updateVertex(*i); } updateVertex(u); } } return 0; } /* bool Dstar::close(double x, double y) * -------------------------- * Returns true if x and y are within 10E-5, false otherwise */ bool Dstar::close(double x, double y) { if (isinf(x) && isinf(y)) return true; return (fabs(x-y) < 0.00001); } /* void Dstar::updateVertex(state u) * -------------------------- * As per [S. Koenig, 2002] */ void Dstar::updateVertex(state u) { list<state> s; list<state>::iterator i; if (u != s_goal) { getSucc(u,s); double tmp = INFINITY; double tmp2; for (i=s.begin();i != s.end(); i++) { tmp2 = getG(*i) + cost(u,*i); if (tmp2 < tmp) tmp = tmp2; } if (!close(getRHS(u),tmp)) setRHS(u,tmp); } if (!close(getG(u),getRHS(u))) insert(u); } /* void Dstar::insert(state u) * -------------------------- * Inserts state u into openList and openHash. */ void Dstar::insert(state u) { ds_oh::iterator cur; float csum; u = calculateKey(u); cur = openHash.find(u); csum = keyHashCode(u); // return if cell is already in list. TODO: this should be // uncommented except it introduces a bug, I suspect that there is a // bug somewhere else and having duplicates in the openList queue // hides the problem... //if ((cur != openHash.end()) && (close(csum,cur->second))) return; openHash[u] = csum; openList.push(u); } /* void Dstar::remove(state u) * -------------------------- * Removes state u from openHash. The state is removed from the * openList lazilily (in replan) to save computation. */ void Dstar::remove(state u) { ds_oh::iterator cur = openHash.find(u); if (cur == openHash.end()) return; openHash.erase(cur); } /* double Dstar::trueDist(state a, state b) * -------------------------- * Euclidean cost between state a and state b. */ double Dstar::trueDist(state a, state b) { float x = a.x-b.x; float y = a.y-b.y; return sqrt(x*x + y*y); } /* double Dstar::heuristic(state a, state b) * -------------------------- * Pretty self explanitory, the heristic we use is the 8-way distance * scaled by a constant C1 (should be set to <= min cost). */ double Dstar::heuristic(state a, state b) { return eightCondist(a,b)*C1; } /* state Dstar::calculateKey(state u) * -------------------------- * As per [S. Koenig, 2002] */ state Dstar::calculateKey(state u) { double g=getG(u); double rhs=getRHS(u); //double val = fmin(g,rhs); if (g > rhs) { u.k.first = rhs + 4*heuristic(u,s_start) + k_m; u.k.second = rhs;} else { u.k.first = g + heuristic(u,s_start)