|
bool
NavFn::calcNavFnDijkstra(bool atStart)
{
//初始化数组准备下一轮传播
setupNavFn(true);
//第一个参数为最大传播次数限制，atStart决定是否在起点停止传播，因为算法是从终点Goal出发的。
// calculate the nav fn and path
propNavFnDijkstra(std::max(nx*ny/20,nx ny),atStart);
//计算路径，路径最大含有nx*ny/2个路径点
// path
int len = calcPath(nx*ny/2);
if (len > 0) // found plan
{
ROS_DEBUG("[NavFn] Path found, %d steps\n", len);
return true;
}
else
{
ROS_DEBUG("[NavFn] No path found\n");
return false;
}
}
//设置路径可能点数组，开始新一轮算法。
// Set up navigation potential arrays for new propagation
void
NavFn::setupNavFn(bool keepit)
{
// reset values in propagation arrays
for (int i=0; i<ns; i )
{
//设置所有点都是高cost的点，即未计算过的点，无限大的cost。
potarr[i] = POT_HIGH;
if (!keepit) costarr[i] = COST_NEUTRAL;
gradx[i] = grady[i] = 0.0;
}
//设置边界点都是障碍物类型的COST，即算法不能突破边界。
// outer bounds of cost array
COSTTYPE *pc;
pc = costarr;
for (int i=0; i<nx; i )
*pc = COST_OBS;
pc = costarr (ny-1)*nx;
for (int i=0; i<nx; i )
*pc = COST_OBS;
pc = costarr;
for (int i=0; i<ny; i , pc =nx)
*pc = COST_OBS;
pc = costarr nx - 1;
for (int i=0; i<ny; i , pc =nx)
*pc = COST_OBS;
//3种优先级的可能路径点缓冲变量，优先级 curP > nextP > overP 。
// priority buffers
curT = COST_OBS;
curP = pb1;
curPe = 0;
nextP = pb2;
nextPe = 0;
overP = pb3;
overPe = 0;
memset(pending, 0, ns*sizeof(bool));
//设置目标点的cost为0，传播从目标点开始，k是目标点在数组potarr里面的索引值。
//通过计算目标点的邻接点的cost持续向外传播。
// set goal
int k = goal[0] goal[1]*nx;
//初始化目标点k的cost为0，并把其邻接点（上下左右）放入当前缓冲器curP中，以便下一次传播。
initCost(k,0);
//计算障碍物点的数量。
// find # of obstacle cells
pc = costarr;
int ntot = 0;
for (int i=0; i<ns; i , pc )
{
if (*pc >= COST_OBS)
ntot ; // number of cells that are obstacles
}
nobs = ntot;
}
//初始化目标点k的cost为0，并把其邻接点（上下左右）放入当前缓冲器curP中，以便下一次传播。
// initialize a goal-type cost for starting propagation
void
NavFn::initCost(int k, float v)
{
potarr[k] = v;
push_cur(k 1);
push_cur(k-1);
push_cur(k-nx);
push_cur(k nx);
}
//点n必须不是障碍物才计算cost。
#define push_cur(n) { if (n>=0 && n<ns && !pending[n] && \
costarr[n]<COST_OBS && curPe<PRIORITYBUFSIZE) \
{ curP[curPe ]=n; pending[n]=true; }}
//
// main propagation function
// Dijkstra method, breadth-first
// runs for a specified number of cycles,
// or until it runs out of cells to update,
// or until the Start cell is found (atStart = true)
//
//根据Dijkstra算法开始传播，广度优先，跑cycles次算法直到所有格子都完成更新或到达了起始格子。
bool
NavFn::propNavFnDijkstra(int cycles, bool atStart)
{
int nwv = 0; // max priority block size
int nc = 0; // number of cells put into priority blocks
int cycle = 0; // which cycle we're on
//得到开始点的数组索引值
// set up start cell
int startCell = start[1]*nx start[0];
for (; cycle < cycles; cycle ) // go for this many cycles, unless interrupted
{
//当前缓冲器curP和下次缓冲器nextP都为空，表明所有点都计算过了，退出循环
if (curPe == 0 && nextPe == 0) // priority blocks empty
break;
// stats 统计计算过的点nc
nc = curPe;
if (curPe > nwv)
nwv = curPe;
//重置当前缓冲器中的点的待计算状态
// reset pending flags on current priority buffer
int *pb = curP;
int i = curPe;
while (i-- > 0)
pending[*(pb )] = false;
//调用updateCell计算当前缓冲器中的所有点
// process current priority buffer
pb = curP;
i = curPe;
while (i-- > 0)
updateCell(*pb );
if (displayInt > 0 && (cycle % displayInt) == 0)
displayFn(this);
//3个优先级，curP完了，接着nextP
// swap priority blocks curP <=> nextP
curPe = nextPe;
nextPe = 0;
pb = curP; // swap buffers
curP = nextP;
nextP = pb;
//nextP完了，接着overP
// see if we're done with this priority level
if (curPe == 0)
{
curT = priInc; // increment priority threshold
curPe = overPe; // set current to overflow block
overPe = 0;
pb = curP; // swap buffers
curP = overP;
overP = pb;
}
//如果覆盖率起点，结束此轮计算
// check if we've hit the Start cell
if (atStart)
if (potarr[startCell] < POT_HIGH)
break;
}
ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n",
cycle,nc,(int)((nc*100.0)/(ns-nobs)),nwv);
if (cycle < cycles) return true; // finished up here
else return false;
}
//计算每个点cost的函数
inline void
NavFn::updateCell(int n)
{
//得到左l 右r 上u 下d 4个邻居点的 cost（可能）值。
// get neighbors
float u,d,l,r;
l = potarr[n-1];
r = potarr[n 1];
u = potarr[n-nx];
d = potarr[n nx];
// ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n",
// potarr[n], l, r, u, d);
// ROS_INFO("[Update] cost: %d\n", costarr[n]);
//找到列（column）方向的低cost tc 和交叉（across）方向的低cost ta
// find lowest, and its lowest neighbor
float ta, tc;
if (l<r) tc=l; else tc=r;
if (u<d) ta=u; else ta=d;
//开始这次的更新，COST必须小于障碍物的COST。
// do planar wave update
if (costarr[n] < COST_OBS) // don't propagate into obstacles
{
float hf = (float)costarr[n]; // traversability factor
float dc = tc-ta; // relative cost between ta,tc
//dc取绝对值，ta保存为较小值
if (dc < 0) // ta is lowest
{
dc = -dc;
ta = tc;
}
// calculate new potential
//横差值或纵差值的差值大于当前点的cost，则直接累加横差值或纵差值的较小值得到pot，否则用插值公式计算。
float pot;
if (dc >= hf) // if too large, use ta-only update
pot = ta hf;
else // two-neighbor interpolation update
{
// use quadratic approximation
// might speed this up through table lookup, but still have to
// do the divide
float d = dc/hf;
float v = -0.2301*d*d 0.5307*d 0.7040;
pot = ta hf*v;
}
// ROS_INFO("[Update] new pot: %d\n", costarr[n]);
//如果计算出的pot值小于当期点的pot值，则表明当前传播路线更优，就更新邻接点。
// now add affected neighbors to priority blocks
if (pot < potarr[n])
{
float le = INVSQRT2*(float)costarr[n-1];
float re = INVSQRT2*(float)costarr[n 1];
float ue = INVSQRT2*(float)costarr[n-nx];
float de = INVSQRT2*(float)costarr[n nx];
potarr[n] = pot;
if (pot < curT) // low-cost buffer block 如果pot值小于当前阈值curT
{
//邻接点预期计算后的cost会小于当前的cost，则放入nextP进行下一次（curP计算完后）进行计算。
if (l > pot le) push_next(n-1);
if (r > pot re) push_next(n 1);
if (u > pot ue) push_next(n-nx);
if (d > pot de) push_next(n nx);
}
else // overflow block
{
if (l > pot le) push_over(n-1);
if (r > pot re) push_over(n 1);
if (u > pot ue) push_over(n-nx);
if (d > pot de) push_over(n nx);
}
}
}
}
//根据上面计算出的pot值，算出每个方向的梯度，求出路径
// Path construction
// Find gradient at array points, interpolate path
// Use step size of pathStep, usually 0.5 pixel
//
// Some sanity checks:
// 1. Stuck at same index position
// 2. Doesn't get near goal
// 3. Surrounded by high potentials
//
int
NavFn::calcPath(int n, int *st)
{
// test write
//savemap("test");
//path buf数组不够大时，重新初始化pathx, pathy
// check path arrays
if (npathbuf < n)
{
if (pathx) delete [] pathx;
if (pathy) delete [] pathy;
pathx = new float[n];
pathy = new float[n];
npathbuf = n;
}
//设置路径起始点，这儿为传入的起点start
// set up start position at cell
// st is always upper left corner for 4-point bilinear interpolation
if (st == NULL) st = start;
int stc = st[1]*nx st[0];
// set up offset
float dx=0;
float dy=0;
npath = 0;
// go for <n> cycles at most
for (int i=0; i<n; i )
{
// check if near goal
int nearest_point=std::max(0,std::min(nx*ny-1,stc (int)round(dx) (int)(nx*round(dy))));
if (potarr[nearest_point] < COST_NEUTRAL)//小于COST_NEUTRAL即表示到达目标点
{
pathx[npath] = (float)goal[0];
pathy[npath] = (float)goal[1];
return npath; // done!
}
if (stc < nx || stc > ns-nx) // would be out of bounds
{
ROS_DEBUG("[PathCalc] Out of bounds");
return 0;
}
// add to path加入当前点到路径
pathx[npath] = stc%nx dx;
pathy[npath] = stc/nx dy;
npath ;
//当前点的前后为同一个点，表示有冲突，无法继续向前
bool oscillation_detected = false;
if( npath > 2 &&
pathx[npath-1] == pathx[npath-3] &&
pathy[npath-1] == pathy[npath-3] )
{
ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
oscillation_detected = true;
}
int stcnx = stc nx;
int stcpx = stc-nx;
// check for potentials at eight positions near cell
if (potarr[stc] >= POT_HIGH ||
potarr[stc 1] >= POT_HIGH ||
potarr[stc-1] >= POT_HIGH ||
potarr[stcnx] >= POT_HIGH ||
potarr[stcnx 1] >= POT_HIGH ||
potarr[stcnx-1] >= POT_HIGH ||
potarr[stcpx] >= POT_HIGH ||
potarr[stcpx 1] >= POT_HIGH ||
potarr[stcpx-1] >= POT_HIGH ||
oscillation_detected)
{
ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath);
// check eight neighbors to find the lowest
int minc = stc;
int minp = potarr[stc];
int st = stcpx - 1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st ;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st ;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st = stc-1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st = stc 1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st = stcnx-1;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st ;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
st ;
if (potarr[st] < minp) {minp = potarr[st]; minc = st; }
stc = minc;
dx = 0;
dy = 0;
ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
potarr[stc], pathx[npath-1], pathy[npath-1]);
if (potarr[stc] >= POT_HIGH)
{
ROS_DEBUG("[PathCalc] No path found, high potential");
//savemap("navfn_highpot");
return 0;
}
}
// have a good gradient here
else
{
// get grad at four positions near cell计算梯度值
gradCell(stc);
gradCell(stc 1);
gradCell(stcnx);
gradCell(stcnx 1);
// get interpolated gradient
float x1 = (1.0-dx)*gradx[stc] dx*gradx[stc 1];//计算第一行x的梯度
float x2 = (1.0-dx)*gradx[stcnx] dx*gradx[stcnx 1];//计算第二行x的梯度
float x = (1.0-dy)*x1 dy*x2; // interpolated x//计算x的梯度
float y1 = (1.0-dx)*grady[stc] dx*grady[stc 1];//计算第一列y的梯度
float y2 = (1.0-dx)*grady[stcnx] dx*grady[stcnx 1];//计算第二列y的梯度
float y = (1.0-dy)*y1 dy*y2; // interpolated y//计算y的梯度
// show gradients
ROS_DEBUG("[Path] %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f\n",
gradx[stc], grady[stc], gradx[stc 1], grady[stc 1],
gradx[stcnx], grady[stcnx], gradx[stcnx 1], grady[stcnx 1],
x, y);
// check for zero gradient, failed 0梯度，无法继续，失败
if (x == 0.0 && y == 0.0)
{
ROS_DEBUG("[PathCalc] Zero gradient");
return 0;
}
// move in the right direction 增加对应方向的dx, dy
float ss = pathStep/hypot(x, y);
dx = x*ss;
dy = y*ss;
// check for overflow
if (dx > 1.0) { stc ; dx -= 1.0; }
if (dx < -1.0) { stc--; dx = 1.0; }
if (dy > 1.0) { stc =nx; dy -= 1.0; }
if (dy < -1.0) { stc-=nx; dy = 1.0; }
}
// ROS_INFO("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
// potarr[stc], x, y, pathx[npath-1], pathy[npath-1]);
}
// return npath; // out of cycles, return failure
ROS_DEBUG("[PathCalc] No path found, path too long");
//savemap("navfn_pathlong");
return 0; // out of cycles, return failure
} |
