AMCL系列之map_range.c

map_range.c中函数map_calc_range用来估计无噪声的情况下传感器的测量值。使用的是Bresenham raytracing的算法。

ox、oy、oa分别指示了机器人的位置坐标和方向角，max_range则是激光传感器的量程。
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range) {

通过宏MAP_GXWX和MAP_GYWY获取扫描波束的起点和终点所对应的删格索引。

  int x0 = MAP_GXWX(map,ox);
  int y0 = MAP_GYWY(map,oy);
  int x1 = MAP_GXWX(map,ox + max_range * cos(oa));
  int y1 = MAP_GYWY(map,oy + max_range * sin(oa));

当扫描波束的y轴差异大于x轴差异时，将该方向标记为陡峭的(steep = 1)。

char steep;
if(abs(y1-y0) > abs(x1-x0))
    steep = 1;
else
    steep = 0;

如果是陡峭的，则交换x和y的坐标。

if (steep) {
    tmp = x0; x0 = y0; y0 = tmp;
    tmp = x1; x1 = y1; y1 = tmp;
}

下面分别计算x和y轴的索引扫描偏差量，并为error和deltaerr赋予初值。

int deltax = abs(x1-x0);
int deltay = abs(y1-y0);
int error = 0;
int deltaerr = deltay;

根据x和y轴的发展方向，设定搜索方向。

int xstep = (x0 < x1) ? 1 : -1;
int ystep = (y0 < y1) ? 1 : -1;

检查机器人的位置坐标，确保它在地图的上，并且没有处于未知区域或者占用删格中。

int x = x0;
int y = y0;
if(steep) {
    if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
        return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
} else {
    if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
        return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}

从机器人所在删格开始，沿着波束延伸方向依次检查所覆盖的删格是否空闲。若判定条件不满足，说明扫描波束要么触碰到了地图的边界，要么被未知区域或者占用删格阻挡了去路。 如果扫描到波束的终点都是空闲的，我们将返回传感器的最大量程。

    while(x != (x1 + xstep * 1)) {
        x += xstep;
        error += deltaerr;
        if(2*error >= deltax) {
            y += ystep;
            error -= deltax;
        }
        if(steep) {
            if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
                return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
        } else {
            if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
                return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
        }
    }
    return max_range;
}