RoboRTS源码解读（二） roborts_costmap

[imuncle]

源码地址：https://github.com/RoboMaster/RoboRTS/tree/ros/roborts_costmap

---

1.文件结构

roborts_costmap
├── CMakeLists.txt
├── cmake_module
│   ├── FindEigen3.cmake
│   └── FindProtoBuf.cmake
├── config                                                          # 配置文件
│   ├── costmap_parameter_config_for_decision.prototxt
│   ├── costmap_parameter_config_for_global_plan.prototxt
│   ├── costmap_parameter_config_for_local_plan.prototxt
│   ├── inflation_layer_config_min.prototxt
│   ├── inflation_layer_config.prototxt
│   ├── obstacle_layer_config.prototxt
│   └── static_layer_config.prototxt
├── image                                                           # README中的图片
├── include
│   └── costmap
│       ├── costmap_2d.h                                            # Costmap2D类定义，代价地图父类
│       ├── costmap_interface.h                                     # 代价地图调用接口
│       ├── costmap_layer.h                                         # 代价地图模板
│       ├── costmap_math.h                                          # 简单的数学计算函数
│       ├── footprint.h                                             # 管理机器人足迹
│       ├── inflation_layer.h                                       # 膨胀地图层
│       ├── layer.h                                                 # 地图插件模板
│       ├── layerd_costmap.h                                        # 代价地图
│       ├── map_common.h                                            # 几种地图代价的定义
│       ├── observation_buffer.h                                    # 障碍物缓存容器
│       ├── observation.h                                           # 障碍物类型定义
│       ├── obstacle_layer.h                                        # 障碍物地图层
│       └── static_layer.h                                          # 静态地图层
├── proto
│   ├── costmap_parameter_setting.proto                             # 代价地图配置参数模板
│   ├── inflation_layer_setting.proto                               # 膨胀地图配置参数模板
│   ├── obstacle_layer_setting.proto                                # 障碍物地图配置参数模板
│   └── static_layer_setting.proto                                  # 静态地图配置参数模板
└── src                                                             # 功能描述同头文件
    ├── costmap_2d.cpp
    ├── costmap_interface.cpp
    ├── costmap_layer.cpp
    ├── costmap_math.cpp
    ├── footprint.cpp
    ├── inflation_layer.cpp
    ├── layer.cpp
    ├── layered_costmap.cpp
    ├── observation_buffer.cpp
    ├── obstacle_layer.cpp
    ├── static_layer.cpp
    └── test_costmap.cpp

2.类继承关系

3.代码结构

从上图可以看出，整个地图包主要由六大类，其中Costmap2D，Layer两类是基础类，CostmapLayer结合两者定义了地图层的基本模板，InflationLayer，ObstacleLayer，StaticLayer分别管理各自层的地图包，最后CostmapLayers（上图未画出）是总的地图（master），由其他三层代价地图合成，它们相互之间的关系如下图所示：

每种功能放置一层：静态地图是一层，障碍物是一层，膨胀层是一层。这三层组合成了master map（最终的costmap）。

每一层是以插件的形式实现的，实现插件的主体是CostmapLayers类，它有一个成员变量用于存放插件：

class CostmapLayers {
 public:
  // 省略...
 private:
  // 省略...
  std::vector<Layer*> plugins_;
};

除膨胀层外，其他所有地图都继承自CostmapLayer，而膨胀层直接继承自Layer，因为膨胀层并不需要自己维护一份地图。

总和所有地图并与其他节点交互的是costmap_interface.cpp，它建立了一个线程专门用于更新地图，每一次更新都会依次遍历所有地图层进行更新，该线程调用的是MapUpdateLoop函数，在该函数中调用UpdateMap函数，在这函数中最后调用了CostmapLayers的UpdateMap函数，该函数定义如下：

void CostmapLayers::UpdateMap(double robot_x, double robot_y, double robot_yaw) {
  static int count = 0;
  std::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.GetMutex()));
  if (is_rolling_window_) {
    // 如果是滚动视窗则机器人一直在地图正中央
    double new_origin_x = robot_x - costmap_.GetSizeXWorld() / 2;
    double new_origin_y = robot_y - costmap_.GetSizeYWorld() / 2;
    costmap_.UpdateOrigin(new_origin_x, new_origin_y);
  }
  if (plugins_.size() == 0) {
    ROS_WARN("No Layer");
    return;
  }

  minx_ = miny_ = 1e30;
  maxx_ = maxy_ = -1e30;
  for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) {
    // 都是世界坐标
    double prev_minx = minx_;
    double prev_miny = miny_;
    double prev_maxx = maxx_;
    double prev_maxy = maxy_;
    (*plugin)->UpdateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
    count++;
    if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy) {
      ROS_WARN("Illegal bounds change. The offending layer is %s", (*plugin)->GetName().c_str());
    }
  }
  int x0, xn, y0, yn;
  // 世界坐标转到地图坐标
  costmap_.World2MapWithBoundary(minx_, miny_, x0, y0);
  costmap_.World2MapWithBoundary(maxx_, maxy_, xn, yn);
  x0 = std::max(0, x0);
  xn = std::min(int(costmap_.GetSizeXCell()), xn + 1);
  y0 = std::max(0, y0);
  yn = std::min(int(costmap_.GetSizeYCell()), yn + 1);
  if (xn < x0 || yn < y0) {
    return;
  }
  costmap_.ResetPartMap(x0, y0, xn, yn);
  for (auto plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) {
    (*plugin)->UpdateCosts(costmap_, x0, y0, xn, yn);
  }

  bx0_ = x0;
  bxn_ = xn;
  by0_ = y0;
  byn_ = yn;
  is_initialized_ = true;
}

可以看到其中有一个for循环遍历了所有plugin（插件，即不同地图层），里面调用了地图的UpdateBounds和UpdateCosts两个函数，很明显这就是插件地图中需要实现的两个方法，事实上在Layer类中这两个方法确实被声明为虚函数，需要子类重载实现。另外如果有必要的话还可以通过重载OnInitialize函数来实现自己的初始化函数，该函数在Layer类中调用如下：

void Layer::Initialize(CostmapLayers *parent, std::string name, tf::TransformListener *tf) {
  layered_costmap_ = parent;
  name_ = name;
  tf_ = tf;
  OnInitialize();
}

下面分别来看三个地图层的工作方式。

1.静态地图层

顾名思义，静态地图是静态的，只会在刚开始的时候更新一下。

静态地图订阅了"map"话题，该话题由ROS的map_server包发布，这个包通过读取一张图片（灰度图），按照图片灰度的大小生成地图对应的代价栅格地图，如果first_map_only_为true，则在收到第一条地图消息后就关闭订阅。

在UpdateBounds阶段将更新整张地图，UpdateCosts阶段判断是否采用滚动窗口，如果是，则地图随着机器人移动而移动，需要获取静态地图坐标系到全局坐标系的转换。

2.障碍物地图层

因为要更新障碍物，所以该层订阅了激光雷达的数据，并将数据转换为点云的形式存储下来，点云的处理在observation_buffer.cpp里面，存储类型定义在observation.h中。在接收到雷达数据后调用BufferCloud函数将点云存进buffer里面。

添加点云时还会删除过时的点云，因为机器人是无法分辨哪些障碍物是场地静止元素，哪些是移动的机器人或其他东西，所以需要删除过时的点云，可以设置点云有效时间，如果时间为0,则只保留最近的一次点云。另外需要注意的是，并不是一个点云对应一个障碍物，一个点云就是一次雷达传回来的数据总和，它包含的是机器人当前四周的所有情况，所有障碍物都被包含在里面。

在UpdateBounds中设置了如果不窗口滚动跟随，就每2秒更新一次地图。然后该函数调用了RaytraceFreespace函数，将点云中的点位置与激光雷达位置连线上的栅格设置为FREE，即没有障碍，这里面用到了Bresenham算法，可以百度简单了解。

另外在UpdataBounds中还执行类clear和mark操作，它们操作的是观察缓冲区。在ObstacleLayer中存在三个缓冲数组，用于存放三类缓冲区：观察缓冲区、mark缓冲区、clear缓冲区。

std::vector<std::shared_ptr<ObservationBuffer> > observation_buffers_;
std::vector<std::shared_ptr<ObservationBuffer> > marking_buffers_;
std::vector<std::shared_ptr<ObservationBuffer> > clearing_buffers_;

一个缓冲区（ObservationBuffer）对应一个观察源，用于存放观察源的数据。

clear操作

clear操作时在costmap global坐标系下的二维平面内，根据clear缓冲区中的各个Observation（点云元素），使用RaytraceFreespace设置地图中的FREE_SPACE。

mark操作

mark操作也比较简单，一次获取mark缓冲区的各个Observation，计算与激光雷达原点的距离，如果距离不超过obstacle_range，则将点云对应坐标的地图信息设置为LETHAL_OBSTACLE。

由于mark和clear使用的是同一块Observation数据，因此有时出现边界点标记但是清除不掉的现象，这估计就是边界通过画线的方式进行clear操作时会偶尔覆盖不到一些像素。解决这个问题的思路是将clear的扇形范围取的比mark的范围大一些（针对LaserScan这种扇形数据而言），从而保证边界的清除效果。由于mark和clear使用同一块数据，因此比较难以单独改变范围。可以采用的一个思路是修改clear部分的代码，在利用点云画直线清除时，加粗直线。另一种是，自己创建节点订阅激光数据，并发布一个角度范围小的LaserScan数据，同时在ObstacleLayer中增加一个源，然原始激光对应的源只clear，变换后的激光对应的源只mark。

最后根据mark和clear操作的范围更新出入的地图边界信息。

在UpdateCosts中，将前面计算出来的代价更新到master map中。

3.膨胀层

膨胀层没有继承CostmapLayer，而是直接继承Layer，所以它并没有维护自己的地图层，而是直接操作master map。

膨胀的时候使用到了机器人足迹Footprint信息，这里简单介绍一下：

机器人在Costmap2D中的模型其实是两个同心圆，一个机器人的轮廓外切圆，一个机器人轮廓内切圆。然后根据机器人中心至边界或者障碍物的距离和两个同心圆半径比较来判断是否碰撞，下图是某时刻机器人周围栅格的代价计算示意图。

膨胀层的UpdateBounds并没有做什么事情，因为它本就没有自己的地图。另外膨胀层在OnInitialize中计算了一个如上图所示的距离-代价对照表（数组）cached_costs_，因为该表与机器人的足迹有关，所以当机器人足迹改变的时候会调用OnFootprintChanged重新计算该表。

在UpdateCosts中，先从障碍物自身出发，往外膨胀，遍历障碍物的栅格点，将每个栅格点的上下左右四个栅格进行处理，计算它们与障碍物边缘的距离，然后根据距离-代价对照表更新代价，这样遍历完一遍之后就完成了膨胀。