这周开始又要开始阅读navigation stack的代码了,最近一段时间本人尝试将navigation stack中的代价地图和路径规划模块提取出来,丢弃掉ROS的wrapper,以便在不同平台中使用。期间撸了一下costmap的源码,关于代码地图的加载、生成、更新有了新的认识,故整理一下。
Costmap_2D类图
在Ubuntu中找了几个从源码生成uml的工具,要么收费(scitool的Understand),要么不好用(不点名),没办法,只能用宇宙IDE,一键生成类图,完美!这里只显示了几个关键的包,其余的组件包没有显示,可以单独阅读。
如果一开始没有这个类图时,容易看得云里雾里,有了这个图,整个包的结构就非常清晰了。用过的都知道,这里的代码地图是分层的,有静态地图层(StaticLayer),也有用于表示传感器信息的(ObstacleLayer)等。但现实世界中,机器人是有尺寸的,所以还需要将静态地图层和障碍物层进行膨胀,从而在规划路径的时候能够考虑机器人的尺寸,而膨胀就由InflationLayer完成。
静态地图层(StaticLayer)
静态地图层就比较简单了,map_server加载地图后,将地图发布出来,costmap_2d_ros接受到地图数据后,便加载各个层,静态地图层只是简单地将灰度图中的像素值换算成ros代价地图中的代价值,然后便进入下一层的更新了。这里只是将转换的代码贴出了说一说:
void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
// 前面就是一些检查和初始化,忽略之...
// 这里就是将图片的像素值转换成代价值了,具体转换可以看interpretValue函数
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map->data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
}
// 转换函数
unsigned char StaticLayer::interpretValue(unsigned char value)
{
// 实际上就是高于一个阈值lethal_threshold那么就认为是障碍物
// 没有信息的就认为是未探测的区域
// 否则为自由空间
if (track_unknown_space_ && value == unknown_cost_value_)
return NO_INFORMATION;
else if (!track_unknown_space_ && value == unknown_cost_value_)
return FREE_SPACE;
else if (value >= lethal_threshold_)
return LETHAL_OBSTACLE;
else if (trinary_costmap_)
return FREE_SPACE;
// 对于其他的数值则进行插值运算
double scale = (double) value / lethal_threshold_;
return scale * LETHAL_OBSTACLE;
}
障碍物层(ObstacleLayer)
障碍物层是将传感器检测到的点云投影到地图中,不同的传感器来源会分别存到Observation类中。只要遍历存储Obsrvation的容器,将检测到的点云的每个点分别投影到地图中,将对应的网格的代价值设为LETHAL_OBSTACLE,其实现函数在void ObstacleLayer::updateBounds(...)
中:
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
{
...
// 先打扫一下旧垃圾,清除一下障碍物
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
}
// 获取每个观察层的点云数据
for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
{
const Observation& obs = *it;
const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.cloud_);
double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;
//将每个点云添加进障碍物层中
for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
double px = cloud.points[i].x, py = cloud.points[i].y, pz = cloud.points[i].z;
// 障碍物高度约束
if (pz > max_obstacle_height_)
{
ROS_DEBUG("The point is too high");
continue;
}
// 距离约束
double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z);
if (sq_dist >= sq_obstacle_range)
{
ROS_DEBUG("The point is too far away");
continue;
}
// 检查一下这个点在不在地图中,不在地图中还放啥放,下一位!
unsigned int mx, my;
if (!worldToMap(px, py, mx, my))
{
ROS_DEBUG("Computing map coords failed");
continue;
}
unsigned int index = getIndex(mx, my);
// 查到的点设为障碍物点,投影完毕!
costmap_[index] = LETHAL_OBSTACLE;
// 扩张一下领地,让[x,y]囊括进边界内
touch(px, py, min_x, min_y, max_x, max_y);
}
}
// 将机器人囊括进边界内
updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
膨胀层(InflationLayer)
膨胀层的工作原理是根据机器人的大小,生成一个膨胀代价矩阵(我自己这么叫的),然后,去遍历地图中所有障碍物的网格点,根据膨胀代价矩阵,修改障碍物网格点周围的网格点的代价值,最终实现膨胀的效果,请发挥你们的想象力。。。
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
...
// 将是障碍物的网格记录到obs_bin中
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
unsigned int index = cell.index_;
// 如果访问过的网格就不在访问了
if (seen_[index])
{
continue;
}
seen_[index] = true;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
/*
inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
// cached_costs_就是我说的膨胀代价矩阵,根据距离可以得到[mx,my]所在的网格的膨胀代价值
return cached_costs_[dx][dy];
}
*/
unsigned char old_cost = master_array[index];
// 虽然这里写的复杂,但其实就是想表示用高的代价值去替换到旧的低的代价值
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);
// 下面这几句就是将[mx,my]四周的网格点也进行膨胀
if (mx > 0)
enqueue(index - 1, mx - 1, my, sx, sy);
if (my > 0)
enqueue(index - size_x, mx, my - 1, sx, sy);
if (mx < size_x - 1)
enqueue(index + 1, mx + 1, my, sx, sy);
if (my < size_y - 1)
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
// 大功告成,收拾下垃圾...
inflation_cells_.clear();
}
清除
清除代价地图的方式也很好理解,以单线激光雷达的激光线为例子,当激光线发射出去之后,在某处检测到一个障碍物,那说明:从发射的地方至某处之间是free的,那么这之间的旧的障碍物应当被删除,这之间网格的代价值应当被修改为free(在膨胀前)。
void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y)
{
double ox = clearing_observation.origin_.x;
double oy = clearing_observation.origin_.y;
pcl::PointCloud < pcl::PointXYZ > cloud = *(clearing_observation.cloud_);
// 获取传感器的原点
unsigned int x0, y0;
if (!worldToMap(ox, oy, x0, y0))
{
ROS_WARN_THROTTLE(
1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
ox, oy);
return;
}
...
// 清除传感器原点到检测到的点之间的障碍物
for (unsigned int i = 0; i < cloud.points.size(); ++i)
{
double wx = cloud.points[i].x;
double wy = cloud.points[i].y;
// 但是...要考虑传感器数据如果在边界外,那么要做一些特殊处理
// 其实就是求出传感器数据点和传感器原点连成的线段和边界的交点
// 然后清除传感器原点到交点之间的障碍物就可以了
double a = wx - ox;
double b = wy - oy;
// the minimum value to raytrace from is the origin
if (wx < origin_x)
{
double t = (origin_x - ox) / a;
wx = origin_x;
wy = oy + b * t;
}
if (wy < origin_y)
{
double t = (origin_y - oy) / b;
wx = ox + a * t;
wy = origin_y;
}
// the maximum value to raytrace to is the end of the map
if (wx > map_end_x)
{
double t = (map_end_x - ox) / a;
wx = map_end_x - .001;
wy = oy + b * t;
}
if (wy > map_end_y)
{
double t = (map_end_y - oy) / b;
wx = ox + a * t;
wy = map_end_y - .001;
}
// now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
unsigned int x1, y1;
// check for legality just in case
if (!worldToMap(wx, wy, x1, y1))
continue;
unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
MarkCell marker(costmap_, FREE_SPACE);
// 终于...进行清除
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
}
template<class ActionType>
inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length = UINT_MAX)
{
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = sign(dx); // 看x是前进一格还是后退一个格子
int offset_dy = sign(dy) * size_x_; // 与上同理
unsigned int offset = y0 * size_x_ + x0;
// we need to chose how much to scale our dominant dimension, based on the maximum length of the line
double dist = hypot(dx, dy);
double scale = (dist == 0.0) ? 1.0 : std::min(1.0, max_length / dist);
// if x is dominant
if (abs_dx >= abs_dy)
{
int error_y = abs_dx / 2;
bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
return;
}
// otherwise y is dominant
int error_x = abs_dy / 2;
bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
}
template<class ActionType>
inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a,
int offset_b, unsigned int offset, unsigned int max_length)
{
unsigned int end = std::min(max_length, abs_da);
for (unsigned int i = 0; i < end; ++i)
{
at(offset);
offset += offset_a;
/*
* 下面这个情况,error_b + abs_db就会超过abs_da,所以每次x+1后,y也会+1
* _
* _|
* _|
* _|
* _|
*
* 下面这个情况,error_b + n* abs_db才会超过abs_da,所以每次x+n后,y才会+1
* ___|
* ___|
* ___|
* ___|
*/
error_b += abs_db; // 为了控制前进上升的斜率
if ((unsigned int)error_b >= abs_da)
{
offset += offset_b;
error_b -= abs_da;
}
}
at(offset);
}
每个层通过updateCost或者updateBounds更新自身所维护的costmap_(每一层相互独立),并且值更新到layer_costmap中的costmap_(其是最终合成所有层得到的costmap地图)。