ROS（12）ROSCostMap2D详解

12. Costmap2DROS分析

参考链接https://www.cnblogs.com/sakabatou/p/8297736.html

12.1. RVIZ中显示的地图

在之前的仿真中，能够在RVIZ中查看全局地图与局部地图，注意默认的视角下，xy坐标系很特殊：x轴向上，y轴向左。

12.2. 参数配置说明(turtlebot3仿真)

通过查看move_base.launch文件，我们重点看下面三个yaml配置：

costmap_common_params_$(arg model).yaml  
local_costmap_params.yaml  
global_costmap_params.yaml  

这里源码中有个文件example_params.yaml是全部的配置举例。
参数含义方面，在源码工程中Costmap2DConfig类或其他插件类的配置文件中有对参数的所有描述，也可以通过rosrun rqt_reconfigure rqt_reconfigure指令打开参数动态配置页面，将鼠标放到某个参数上即可显示参数的含义。注意有的参数是在程序中写死的，比如obstacle_range。

• costmap_common_params_burger.yaml

obstacle_range: 3.0
raytrace_range: 3.5 
footprint: [[-0.105, -0.105], [-0.105, 0.105], [0.041, 0.105], [0.041, -0.105]] #以小车中心为(0,0)，也就是robot_base_frame指定的坐标系，这个数组的点依次连线构成小车在地面的投影轮廓。
#robot_radius: 0.105 
inflation_radius: 1.0  #障碍上的膨胀半径，单位m
cost_scaling_factor: 3.0  #膨胀代价比例因子
map_type: costmap 
observation_sources: scan #obstacle_layer订阅的观察数据源主题
scan: {sensor_frame: base_scan, data_type: LaserScan, topic: scan, marking: true, clearing: true}

• global_costmap_params.yaml 全局代价地图配置

global_costmap:
  global_frame: map
  robot_base_frame: base_footprin

  update_frequency: 10.0 #Hz
  publish_frequency: 10.0
  transform_tolerance: 0.5 #单位s,在tf中可容忍的数据延迟
  static_map: true

• local_costmap_params.yaml 局部代价地图配置

  local_costmap:
    global_frame: odom
    robot_base_frame: base_footprint

    update_frequency: 10.0
    publish_frequency: 10.0
    transform_tolerance: 0.5

    static_map: false #非静态地图，默认是静态
    rolling_window: true #是否是滚动的窗
    width: 3 #地图宽，单位m，默认是10m
    height: 3
    resolution: 0.05 #地图精度，单位m，默认就是0.05

12.3. 源码解读

12.3.1. Costmap2DROS启动流程

Costmap2DROS的构造函数流程非常简单：

• 加载参数；
• 加载插件，有四种插件：inflation_layer obstacle_layer static_layer voxel_layer，会根据地当前的地图类型map_type是否是voxel来判断是否加载voxel_layer插件,同时还会根据static_map是否为true来判断是否需要加载static_layer插件；最终加载的插件情况：

  全局地图：static_layer obstacle_layer inflation_layer  
  局部地图：obstacle_layer inflation_layer  
  

• reconfigureCB调用，调用者未知，其中会重置地图大小、启动地图更新线程；
• 核心调用都在LayeredCostmap layered_costmap_对象中。

12.3.2. Costmap2DROS核心接口

• reconfigureCB() :

  重置配置，本函数会在初始化后调用一次，调用者未知。其中会调整地图大小信息(LayeredCostmap中的resizeMap，下面介绍)，并启动地图更新线程；

• mapUpdateLoop() :

  地图更新线程回调接口，会调用各个插件的更新接口，来更新边界Bound和代价Cost；

• start/stop/resetLayers() :

  最终会调用各个插件的active/deactivate/reset()接口；

• getRobotPose() :

  调用tf_.transform(robot_pose, global_pose, global_frame_)语句来获取机器人在全局坐标系下的坐标。这里robot_pose坐标为<0,0,0>，坐标空间为robot_base_frame,实际就是机器人的自身坐标系，global_pose为输出的坐标，global_frame_为全局坐标系名称，在当前仿真中全局地图是/map，局部地图是/odom，（这里在tf树中已经存在了/base_footprint到/odom再到/map的转换关系了），这里不太理解为何两个地图不都使用/map？这样在两个地图中获取的坐标会有一些偏差值。；

• tf树关系：
  

12.3.3. LayeredCostmap

本类主要维护了Costmap2D costmap_，管理了各个layer插件。
本类主要操作接口：

• LayeredCostmap::resizeMap() :

  【参数】：
  size_x : 计算公式为map_width_meters / resolution，比如10m宽，精度0.05，计算后是200，表示有200列单元格。这里局部地图和全局地图计算后的宽是不一样的。
  size_y : 同上;
  resolution : 精度；
  origin_x/origin_y : 默认数值是0，全局地图是静态图的左下角坐标，也即地图文件yaml中存放的origin参数；局部地图是以AGV为中心点，局部地图长宽限定下的地图左下角在global_frame坐标系中的坐标。

  【执行流程】：
  1.调用Costmap2D::resizeMap()接口，内部根据传入的数据，重新初始化地图；
  2.调用各个插件的matchSize()接口：
  StaticLayer::matchSize() : 在非滚动窗口的情况下才会调整地图大小；

• LayeredCostmap::updateMap() :

  【参数】：机器人在global_frame中的坐标（global_frame全局地图是/map,局部地图是/odom）
  【执行流程】：
  1.如果地图是rolling地图(局部地图)，那么重新调整origin_x/origin_y的位置：

  if (rolling_window_)
  {
   double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
   double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
   costmap_.updateOrigin(new_origin_x, new_origin_y);
  }
  

  此过程首先计算new_origin_x和new_origin_y，它们是以当前AGV坐标为中心点，套上局部地图大小后计算出来的局部地图左下角坐标。然后调用了costmap_.updateOrigin()接口，该接口参考后面的说明。
  2.进行边界更新，调用插件的updateBounds()接口，这里static_layer中直接返回了；
  3.进行代价更新，调用插件的updateCosts()接口，这里static_layer中只是进行了拷贝。

12.3.4. Costmap2D

Costmap2D是最终的地图数据，各个layer插件的数据最终体现到该数据中。它内部维护了一个一维数组，类型为unsigned char，是一个类似于图像的数据，维护了该图像的长、宽及原始<x,y>坐标。
关键接口说明：

• Costmap2D::getSizeInCellsX()/getSizeInCellsY():

返回地图用网格衡量的的长、宽，即长宽上的网格个数。

• Costmap2D::getSizeInMetersX()/getSizeInMetersY():

返回地图用m衡量的长和宽，计算公式是(size_x_ - 1 + 0.5) * resolution_，这个公式没看懂，为啥要减去0.5？

• Costmap2D::updateOrigin():

此函数主要作用是AGV移动一个距离后，当前的局部地图和之前的局部地图重合的部分数据直接进行拷贝之前的，不重合的部分设置为默认数值，重新根据局部地图的插件进行计算。如图：

• Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy):

将地图网格坐标转变为世界坐标，源码如下：

wx = origin_x_ + (mx + 0.5) * resolution_; //是基于origin_x_进行的偏移
wy = origin_y_ + (my + 0.5) * resolution_;

12.3.5. StaticLayer

• StaticLayer::onInitialize()：

1.初始化参数；
2.从/map队列中请求静态地图数据，并进行初始化，接口为incomingMap；

• StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map):

参数为nav_msgs::OccupancyGridConstPtr，是网格地图数据；主要是通过interpretValue接口初始化地图中每个单元格的数据。每个单元格一共有下面几种数据类型：

static const unsigned char NO_INFORMATION = 255; //原始地图中是-1（该值可以配置），表示激光未扫描到的部分；
static const unsigned char LETHAL_OBSTACLE = 254; //原始地图中默认大于100的（该值可以配置），被设置为致命障碍物
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; //内切 膨胀 障碍，这个在inflation插件中使用；
static const unsigned char FREE_SPACE = 0; //空闲区域，其他部分

12.3.6. 其他插件由于时间和项目问题暂未详细查看