0 引言

grid map学习笔记1已成功在Ubuntu18.04系统上源码编译grid map，并且用各个子工程的示例演示，本文主要目的是熟悉grid map的一些常规定义，比如grid map的多层定义，中心坐标系定义，栅格单元定义，迭代器的类型定义等，当然也有一些grid map封装好的packges，比如支持的可视化数据类型（点云，向量，占据栅格等），支持的各种栅格地图过滤器等。

👉 grid map github：https://github.com/ANYbotics/grid_map

本文系统环境：

• Ubuntu18.04
• ROS-melodic
• grid map

1 常规定义

1.1 单层grid map

单层grid map是指一个平面地图，在这个地图上，将整个空间分割成一个个大小相等的方格，每个方格称为一个网格（grid）。每个网格都可以用一个值来表示其在地图上的属性，例如障碍物、自由空间、未知区域等。这些属性值通常用颜色来表示，比如灰色代表障碍物，白色代表自由空间，黑色代表未知区域等。

单层grid map通常用于机器人地图构建和路径规划中，机器人通过传感器获取周围环境信息，然后根据这些信息更新地图上各个网格的属性值。机器人在规划路径时，可以使用单层grid map来表示环境，根据不同网格的属性值计算出可行的路径，从而避开障碍物并尽可能地避免未知区域。

单层grid map相对于其他类型的地图，具有简单、易于实现、易于存储和处理等优点。但它也存在一些限制，例如无法表示多层信息、无法表达细节等。

单层grid map的中心默认在栅格地图的中心，栅格地图的坐标系定义遵从右手法则，x朝前，y朝左，z朝上，每张grid map地图，从左上角开始编号，依次是(0,0)、(0,1)...(n,m)

一些常用的函数定义：

• getMapSize()：该函数用于获取地图的大小。输出为地图的宽度和高度，通常以网格数来表示。在机器人地图构建和路径规划中，通常需要获取地图的大小以便进行合理的路径规划和地图构建。
• getPosition(grid_x, grid_y)：该函数用于将网格坐标转化为世界坐标。输入参数为网格坐标(grid_x, grid_y)，输出为该网格的世界坐标。在机器人地图构建和路径规划中，通常需要将机器人在地图上的位置转化为世界坐标，以便更好地进行路径规划和控制。
• getGridIndex(pos_x, pos_y)：该函数用于将世界坐标转化为网格坐标。输入参数为世界坐标(pos_x, pos_y)，输出为该坐标所在网格的网格坐标。在机器人地图构建和路径规划中，通常需要将传感器获取到的障碍物位置转化为网格坐标，以便更新地图上各个网格的属性值。
• getResolution()：该函数用于获取网格地图的分辨率，即每个网格代表的实际距离。输出为一个实数值，单位为米/格。在机器人路径规划中，通常需要利用网格地图的分辨率计算机器人的运动距离和速度等参数。
• getSize()：该函数用于获取网格地图的大小，即网格地图的宽度和高度，以网格数为单位。输出为一个整数对，格式为(width, height)。在机器人路径规划和地图构建中，通常需要获取网格地图的大小以便进行合理的路径规划和地图构建。
• getLength()：该函数用于获取网格地图的尺寸，即网格地图的总网格数。输出为一个整数值，表示网格地图中网格的总数。在机器人路径规划和地图构建中，通常需要获取网格地图的尺寸以便进行合理的路径规划和地图构建。
• getIndex(x, y)：该函数用于将网格地图中的坐标转化为网格地图中的索引。输入参数为坐标(x, y)，输出为该坐标在网格地图中的索引。在机器人路径规划中，通常需要将世界坐标转化为网格地图中的索引以便进行路径规划。

1.2 多层grid map

多层grid map是指在单层grid map的基础上，增加了多个层次的信息表示。每个层次的信息表示可以包括不同的地图属性，例如高度、颜色、温度等。每个网格在多层grid map中可以包含多个属性值，每个属性值对应一个层次。

多层grid map常用于机器人导航和环境建模中，它可以同时表示不同层次的信息，例如机器人所处的高度、周围环境的颜色、温度和湿度等，从而更准确地表示环境信息，增强机器人的感知能力和路径规划能力。

在多层grid map中，常用的函数包括：

• setGridValue(layer, grid_x, grid_y, value)：该函数用于设置指定层次和指定网格的属性值。输入参数为层次(layer)、网格坐标(grid_x, grid_y)和属性值(value)，用于更新地图上指定层次和指定网格的属性值。

• getGridValue(layer, grid_x, grid_y)：该函数用于获取指定层次和指定网格的属性值。输入参数为层次(layer)、网格坐标(grid_x, grid_y)，输出为该网格在指定层次的属性值。

• getMapSize()：该函数用于获取多层grid map的大小。输出为地图的宽度和高度，通常以网格数来表示。

• getNumLayers()：该函数用于获取多层grid map的层数。输出为整数值，表示多层grid map中包含的层数。

• getResolution()：该函数用于获取多层grid map的分辨率，即每个网格代表的实际距离。输出为一个实数值，单位为米/格。

多层grid map相对于单层grid map具有更丰富的信息表示能力，但也会带来更高的计算和存储成本。

1.3 迭代器类别

在grid map中，常用的迭代器包括：

• GridMapIterator：用于在grid map中迭代所有网格。该迭代器按行优先（row-major）的顺序遍历网格，即先遍历第一行的所有网格，然后是第二行的所有网格，以此类推。使用该迭代器可以遍历整个地图。
• GridMapIteratorRow：用于在grid map中迭代指定行的所有网格。该迭代器按列优先（column-major）的顺序遍历网格，即先遍历第一列的所有网格，然后是第二列的所有网格，以此类推。使用该迭代器可以遍历指定行的所有网格。
• GridMapIteratorRectangle：用于在grid map中迭代指定矩形区域内的所有网格。该迭代器按行优先的顺序遍历矩形区域内的网格。使用该迭代器可以遍历指定矩形区域内的所有网格。
• GridMapIteratorSubmap：用于在grid map中迭代指定子地图内的所有网格。该迭代器按行优先的顺序遍历子地图内的网格。使用该迭代器可以遍历指定子地图内的所有网格。

此外，还有一些特定形状的迭代器可以帮助更方便地访问grid map中的网格，例如：

• GridMapCircleIterator：用于在grid map中迭代指定圆形区域内的所有网格。使用该迭代器可以遍历指定圆形区域内的所有网格。
• GridMapLineIterator：用于在grid map中迭代指定线段路径上的所有网格。使用该迭代器可以遍历指定线段路径上的所有网格。
• GridMapPolygonIterator：用于在grid map中迭代指定多边形区域内的所有网格。使用该迭代器可以遍历指定多边形区域内的所有网格。
• GridMapEllipseIterator：用于在grid map中迭代指定椭圆形区域内的所有网格。使用该迭代器可以遍历指定椭圆形区域内的所有网格。
• GridMapSpiralIterator：用于在grid map中迭代指定螺旋线区域内的所有网格。使用该迭代器可以遍历指定螺旋线区域内的所有网格。

可以根据具体的需求选择不同的迭代器。在实际应用中，可以通过使用这些迭代器来实现各种操作，例如遍历网格、更新网格属性、计算路径等。

网格图	子图	圆形	线

多边形	椭圆形	螺旋	…
			…

对于每个迭代器类别，都有相应的函数来创建迭代器对象，例如：

• GridMapIterator: begin()和end()函数
• GridMapIteratorRow: beginRow()和endRow()函数
• GridMapIteratorRectangle: beginRectangle()和endRectangle()函数
• GridMapIteratorSubmap: beginSubmap()和endSubmap()函数
• GridMapCircleIterator: GridMapCircleIterator()构造函数
• GridMapLineIterator: GridMapLineIterator()构造函数
• GridMapPolygonIterator: GridMapPolygonIterator()构造函数
• GridMapEllipseIterator: GridMapEllipseIterator()构造函数
• GridMapSpiralIterator: GridMapSpiralIterator()构造函数

例如，以下代码演示了使用GridMapIterator遍历grid map中的所有网格：

// 创建GridMapIterator迭代器对象
GridMapIterator iter(grid_map);

// 遍历grid map中的所有网格
for(auto index : iter) {
    // 对每个网格执行操作
    // ...
}

类似地，以下代码演示了使用GridMapCircleIterator遍历grid map中的一个圆形区域内的所有网格：

// 创建GridMapCircleIterator迭代器对象
GridMapCircleIterator iter(grid_map, center, radius);

// 遍历圆形区域内的所有网格
for (auto index : iter) {
    // 对每个网格执行操作
    // ...
}

需要注意的是，在使用迭代器访问网格时，可以使用索引（index）或坐标（position）方式来访问。例如，以下代码演示了使用索引方式来访问网格：

// 遍历grid map中的所有网格
for (auto index : iter) {
    // 获取网格索引对应的网格值
    double value = grid_map.at("layer_name", index);
    // 对网格执行操作
    // ...
}

类似地，以下代码演示了使用坐标方式来访问网格：

// 创建GridMapIteratorSubmap迭代器对象
GridMapIteratorSubmap iter(grid_map, position, length);

// 遍历子地图内的所有网格
for (auto position : iter) {
    // 获取坐标对应的网格值
    double value = grid_map.atPosition("layer_name", position);
    // 对网格执行操作
    // ...
}

1.4 移动grid map的位置

在grid map中，位置移动可以通过使用grid_map库提供的函数来实现。grid_map库提供了一系列函数，可以方便地对网格地图进行平移和旋转操作。

setPosition(…)	move(…)	…

比如，对于平移操作，可以使用grid_map::move函数。该函数接受一个Eigen::Vector2d类型的偏移量参数，用于指定网格地图在x和y方向上的平移量。例如，以下代码可以将网格地图平移一个单位：

grid_map::GridMap map;
// 假设已经加载了网格地图
Eigen::Vector2d offset(1.0, 1.0);
grid_map::move(map, offset);

对于旋转操作，可以使用grid_map::rotate函数。该函数接受一个double类型的旋转角度参数，以弧度为单位。例如，以下代码可以将网格地图旋转45度：

grid_map::GridMap map;
// 假设已经加载了网格地图
double angle = M_PI / 4.0; // 45度
grid_map::rotate(map, angle);

需要注意的是，调用这些函数后，网格地图的大小和分辨率不会发生变化。如果需要改变网格地图的大小和分辨率，可以使用grid_map::GridMap::add函数或grid_map::GridMap::setGeometry函数。

2 功能包

2.1 grid_map_rviz_plugin

grid_map_rviz_plugin是一个ROS中的插件，用于在ROS可视化工具RViz中显示二维栅格地图。它是grid_map库的一部分，该库是ROS中用于管理和处理2D网格地图的常用库。

使用grid_map_rviz_plugin，用户可以在RViz中可视化网格地图的不同层，例如高程、占用度或其他用户定义的层。此外，该插件还支持在RViz中可视化其他信息，例如机器人传感器读数、路径规划结果等。

使用grid_map_rviz_plugin，用户可以执行以下操作：

• 在RViz中添加grid_map_rviz_plugin插件。

• 将网格地图转换为ROS消息类型，并将其发布到ROS话题中。

• 在RViz中选择grid_map_rviz_plugin作为显示类型，并选择要显示的网格地图层。

• 根据需要调整RViz中的显示参数，例如颜色、透明度等。

使用grid_map_rviz_plugin，用户可以更轻松地理解和分析网格地图数据，从而更好地实现机器人导航、路径规划等应用。同时，由于RViz支持交互式操作，用户还可以通过RViz直接与网格地图进行交互，例如缩放、平移、旋转等操作。

2.2 grid_map_sdf

grid_map_sdf是一个ROS包中的功能包，用于构建二维栅格地图，并计算该地图中每个栅格的有向距离场（Signed Distance Field，SDF）。以下是对该包的详细解释。

SDF是一种表示几何对象的方法，可以用于计算两个对象之间的距离。对于一个给定的点，SDF可以告诉我们该点离最近的对象表面的距离，以及该点是否在对象内部或外部。这种表示方式被广泛用于机器人导航和避障等应用中。

在ROS中，grid_map_sdf包提供了一种使用栅格地图计算SDF的方法。它使用Fast Marching算法来计算每个栅格到最近障碍物的距离，可以有效地处理复杂的环境和障碍物形状。

使用grid_map_sdf包，可以将传感器数据转换为栅格地图，并计算出每个栅格的SDF值。这些值可以用于机器人的导航和避障，以及其他需要几何信息的任务。

2.3 grid_map_visualization

grid_map_visualization 是一种将网格地图可视化的方法或工具。网格地图是一种将环境或区域划分为规则网格单元的表示方式，通常用于机器人导航、路径规划、环境建模等应用。

grid_map_visualization节点订阅grid_map_msgs/GridMap类型的主题并发布可在RViz中可视化的消息。可视化工具发布的主题可以使用 YAML 参数文件进行完全配置。可以添加任意数量的具有不同参数的可视化。

2.3.1 订阅的主题

订阅的主题比较明确，就是grid_map_msgs/GridMap类型的栅格图主题，默认为/grid_map

2.3.2 发布的主题

发布的主题通过YAML参数文件进行配置。可能的主题有点云(Point cloud)，向量(Vectors)，占用网格(Occupancy grid)，网格单元(Grid cells)等，这里以grid_map/grid_map_demos/config/tutorial_demo.yaml配置文件为例：

点云	向量	占用栅格	网格单元

• point_cloud( sensor_msgs/PointCloud2 )

将网格图显示为点云。使用参数选择要转换为点的图层layer；

  name: elevation
  type: point_cloud
  params:
   layer: elevation
   flat: false # optional

• flat_point_cloud( sensor_msgs/PointCloud2 )

将网格图显示为“平面”点云，即所有点都具有相同高度。在RViz的帮助下，可以方便地可视化RViz中的2D地图或图像（甚至视频流）Color Transformer。该参数height确定平面点云需要的z位置；

  name: flat_grid
  type: flat_point_cloud
  params:
   height: 0.0

注意：为了从空/无效单元格中省略平面点云中的点，请指定使用来检查效果的图层setBasicLayers(...)；

• vectors（可视化消息/标记）

将网格地图的矢量数据可视化为视觉标记。使用参数指定保存保护的x、y和z数量的层layer_prefix，该参数position_layer定义使用矢量起点的图层；

  name: surface_normals
  type: vectors
  params:
   layer_prefix: normal_
   position_layer: elevation
   scale: 0.06
   line_width: 0.005
   color: 15600153 # red

• occupancy_grid( nav_msgs/OccupancyGrid)

将网格地图的图层可视化为占用网格。使用参数指定要可视化的图层layer，并使用和指定上限和data_min下限data_max；

  name: traversability_grid
  type: occupancy_grid
  params:
   layer: traversability
   data_min: -0.15
   data_max: 0.15

• grid_cells( nav_msgs/GridCells)

将网格地图的图层可视化为网格单元。使用参数指定要可视化的图层layer，并使用和指定上限和lower_threshold下限upper_threshold；

  name: elevation_cells
  type: grid_cells
  params:
   layer: elevation
   lower_threshold: -0.08 # optional, default: -inf
   upper_threshold: 0.08 # optional, default: inf

• region（可视化消息/标记）

显示网格地图的边界；

  name: map_region
  type: map_region
  params:
   color: 3289650
   line_width: 0.003

注意：颜色值采用 RGB 形式作为串联（每个通道值 0-255）。以绿色为例（红色：0，绿色：255，蓝色：0

2.4 grid_map_filters

grid_map_filters包包含过滤器，可以将它们评估多个网格地图以对图层中的数据执行计算。网格地图过滤器基于ROS Filters，这意味着过滤器链可以配置为YAML文件。另外，可以通过 ROS 插件机制编写附加过滤器并设置可用，例如来自包的过滤器。InpaintFiltergrid_map_cv

grid_map_filters包中提供了几个基本的过滤器：

• gridMapFilters/ThresholdFilter

如果condition_layer超过上限或下限阈值（一次仅一个阈值），则将输出层中的值设置指定值；

  name: lower_threshold
  type: gridMapFilters/ThresholdFilter
  params:
    condition_layer: layer_name
    output_layer: layer_name
    lower_threshold: 0.0 # alternative: upper_threshold
    set_to: 0.0 # # Other uses: .nan, .inf

• gridMapFilters/MeanInRadiusFilter

计算一个层的每个单元格中半径内的平均值；

  name: mean_in_radius
  type: gridMapFilters/MeanInRadiusFilter
  params:
    input_layer: input
    output_layer: output
    radius: 0.06 # in m.

• gridMapFilters/MedianFillFilter

计算一个层的每个NAN单元的中位数（有限的）贴片内（有限）的中位数。可选地，将中位数计算用于已经有限的值，这些点的补丁半径由现有value_radius给出。请注意，仅当Fill_mask对此有效时才执行填充计算。

  name: median
  type: gridMapFilters/MedianFillFilter
  params:
    input_layer: input
    output_layer: output
    fill_hole_radius: 0.11 # in m. 
    filter_existing_values: false # Default is false. If enabled it also does a median computation for existing values. 
    existing_value_radius: 0.2 # in m. Note that this option only has an effect if filter_existing_values is set true. 
    fill_mask_layer: fill_mask # A layer that is used to compute which areas to fill. If not present in the input it is automatically computed. 
    debug: false # If enabled, the additional debug_infill_mask_layer is published. 
    debug_infill_mask_layer: infill_mask # Layer used to visualize the intermediate, sparse-outlier removed fill mask. Only published if debug is enabled.

• gridMapFilters/NormalVectorsFilter

计算地图中图层的正常向量；

  name: surface_normals
  type: gridMapFilters/NormalVectorsFilter
  params:
    input_layer: input
    output_layers_prefix: normal_vectors_
    radius: 0.05
    normal_vector_positive_axis: z

• gridMapFilters/NormalColorMapFilter

根据普通向量层计算新的颜色层；

  name: surface_normals
  type: gridMapFilters/NormalColorMapFilter
  params:
    input_layers_prefix: normal_vectors_
    output_layer: normal_color

• gridMapFilters/MathExpressionFilter

用网格图的层解析并评估数学基质表达式。有关表达式的文档，请参见 EigenLab

  name: math_expression
  type: gridMapFilters/MathExpressionFilter
  params:
    output_layer: output
    expression: acos(normal_vectors_z) # Slope.
    # expression: abs(elevation - elevation_smooth) # Surface roughness.
    # expression: 0.5 * (1.0 - (slope / 0.6)) + 0.5 * (1.0 - (roughness / 0.1)) # Weighted and normalized sum.

• gridMapFilters/SlidingWindowMathExpressionFilter

在网格图层上的滑动窗口中解析并评估数学矩阵表达式。有关表达式的文档，请参见EigenLab

  name: math_expression
  type: gridMapFilters/SlidingWindowMathExpressionFilter
  params:
    input_layer: input
    output_layer: output
    expression: meanOfFinites(input) # Box blur
    # expression: sqrt(sumOfFinites(square(input - meanOfFinites(input))) ./ numberOfFinites(input)) # Standard deviation
    # expression: 'sumOfFinites([0,-1,0;-1,5,-1;0,-1,0].*elevation_inpainted)' # Sharpen with kernel matrix
    compute_empty_cells: true
    edge_handling: crop # options: inside, crop, empty, mean
    window_size: 5 # in number of cells (optional, default: 3), make sure to make this compatible with the kernel matrix
    # window_length: 0.05 # instead of window_size, in m

• gridMapFilters/DuplicationFilter

复制网格图的一层；

  name: duplicate
  type: gridMapFilters/DuplicationFilter
  params:
    input_layer: input
    output_layer: output

• gridMapFilters/DeletionFilter

从网格图中删除图层；

  name: delete
  type: gridMapFilters/DeletionFilter
  params:
    layers: [color, score] # List of layers.

此外，grid_map_cv包还提供以下过滤器：

• gridMapCv/InpaintFilter

使用 OpenCV 修复/填充图层中的孔。

  name: inpaint
  type: gridMapCv/InpaintFilter
  params:
    input_layer: input
    output_layer: output
    radius: 0.05 # in m

Reference:

• https://github.com/anybotics/grid_map

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔