0 引言

elevation mapping学习笔记1已经成功编译安装elevation mapping高程图工程，并运行示例turtlesim3_waffle_demo，在仿真环境下，控制带有传感器的机器人生成高程图，简单熟悉了高程图的实际应用。本文主要熟悉了该工程一些输入、输出、功能服务和参数配置的定义和说明。

👉 elevation mapping github：https://github.com/ANYbotics/elevation_mapping

本文系统环境：

Ubuntu18.04
ROS-melodic
elevation mapping

1 话题Topics

ros类工程肯定需要订阅话题和发布话题，elevation mapping高程图工程主要有三种需要订阅的话题类型，支持两种发布的话题。

1.1 订阅subscribe

elevation mapping高程图工程主要的输入数据有三部分，topic类型分别是：点云（相机，雷达等）、协方差位姿（机器人，里程计等）和tf关系，其中点云和tf关系是必需的，协方差位姿是非必需的。

/points( sensor_msgs/PointCloud2 )
/pose( geometry_msgs/PoseWithCovarianceStamped )
/tf（tf/tf消息）

比如运行TurtleBot3 Waffle示例，可分别echo这三种数据：

点云数据： rostopic echo /camera/depth/points

  frame_id: "camera_rgb_optical_frame"
height: 1080
width: 1920
fields: 
  - 
    name: "x"
    offset: 0
    datatype: 7
    count: 1
  - 
    name: "y"
    offset: 4
    datatype: 7
    count: 1
  - 
    name: "z"
    offset: 8
    datatype: 7
    count: 1
  - 
    name: "rgb"
    offset: 16
    datatype: 7
    count: 1
is_bigendian: False
point_step: 32
row_step: 61440

协方差位姿数据：rostopic echo /base_footprint_pose

  frame_id: "odom"
pose: 
  pose: 
    position: 
      x: -2.99972175706
      y: 1.00078679455
      z: -0.00100739962037
    orientation: 
      x: -5.58514904452e-06
      y: 0.00158964306941
      z: 0.00246946885279
      w: 0.999995687354
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

tf关系树：rostopic echo /tf

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1318
        nsecs: 164000000
      frame_id: "odom"
    child_frame_id: "base_footprint"
    transform: 
      translation: 
        x: -2.99971786734
        y: 1.00079767969
        z: -0.0010073996308
      rotation: 
        x: -5.63941461375e-06
        y: 0.00158964291266
        z: 0.00250363158562
        w: 0.999995602407

1.2 发布publish

elevation mapping高程图工程主要的输出数据有两部分：原始高程图elevation_map_raw和经过后处理的高程图elevation_map

elevation_map( grid_map_msgs/GridMap )
elevation_map_raw( grid_map_msgs/GridMap )

elevation_map_raw（原始高程图）：

elevation_map_raw是一种原始的高程图表示，通常以一个二维栅格（grid）的形式呈现。
它直接从传感器（如激光雷达或深度相机）获取原始测量数据，没有进行额外的滤波或处理。
每个栅格单元格（cell）存储了一个原始高度值，通常以米为单位。
elevation_map_raw提供了对地面高度的基本估计，但可能包含传感器噪音、遮挡物等不完整或不准确的数据。

elevation_map（修正高程图）：

elevation_map是在elevation_map_raw基础上进行处理和滤波后的高程图表示。
它通过使用滤波算法（如高斯滤波、中值滤波等）来平滑和修正原始数据，以减少噪音和不准确性。
elevation_map通常会对原始高度数据进行插值，使得在栅格之间的位置也具有高度估计值，从而提供更连续和均匀的地面高度表示。
由于经过处理和滤波，elevation_map通常比elevation_map_raw更适合用于高级算法和导航任务。

示例	elevation_map_raw	elevation_map
TurtleBot3 Waffle

2 服务Services

elevation mapping高程图工程还提供了一些服务，这些服务能方便查询高程图的信息，重载、保存高程图等功能。

trigger_fusion (std_srvs/Empty)

触发整个高程图的融合过程并发布:

rosservice call /elevation_mapping/trigger_fusion

get_submap (grid_map_msgs/GetGridMap)

获取所需位置和大小的融合高程子图。例如，您可以获取 odom 框架中描述的位置 (-0.5, 0.0) 和大小 (0.5, 1.2) 处的融合高程子图，并将其从控制台保存到文本文件中：

rosservice call -- /elevation_mapping/get_submap odom -0.5 0.0 0.5 1.2 []

get_raw_submap (grid_map_msgs/GetGridMap)

获取请求位置和大小的原始高程子图。例如，您可以获取 odom 框架中描述的位置 (-0.5, 0.0) 和大小 (0.5, 1.2) 处的原始高程子图，并将其从控制台保存到文本文件中：

rosservice call -- /elevation_mapping/get_raw_submap odom -0.5 0.0 0.5 1.2 []

clear_map (std_srvs/Empty)

出于重置目的开始清理整个地图。触发地图清理：

rosservice call /elevation_mapping/clear_map

masked_replace (grid_map_msgs/SetGridMap)

允许通过服务调用设置高程图的各个图层。mask层只能用于设置某些单元格，而不是整个地图。mask中包含NAN 的单元格未设置，所有其他单元格均已设置。如果未提供mask层，则整个地图将设置在两个地图的交集中。所提供的地图的大小和位置可以与将要更改的地图不同。将高程图层中标有mask的一些单元格设置为 0.5 的服务调用示例如下：

  rosservice call /elevation_mapping/masked_replace "map:
    info:
      header:
        seq: 3
        stamp: {secs: 3, nsecs: 80000000}
        frame_id: 'odom'
      resolution: 0.1
      length_x: 0.3
      length_y: 0.3
      pose:
        position: {x: 5.0, y: 0.0, z: 0.0}
        orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}
    layers: [elevation,mask]
    basic_layers: [elevation]
    data:
    - layout:
        dim:
        - {label: 'column_index', size: 3, stride: 9}
        - {label: 'row_index', size: 3, stride: 3}
        data_offset: 0
      data: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
    - layout:
        dim:
        - {label: 'column_index', size: 3, stride: 9}
        - {label: 'row_index', size: 3, stride: 3}
        data_offset: 0
      data: [0, 0, 0, .NAN, .NAN, .NAN, 0, 0, 0]
    outer_start_index: 0
    inner_start_index: 0"

save_map (grid_map_msgs/ProcessFile)

将当前融合网格图和原始网格图保存到 rosbag 文件。字段topic_name必须是基本名称，即没有前导斜杠字符 (/)。如果字段topic_name为空，则elevation_map默认使用。使用默认主题名称的示例：

rosservice call /elevation_mapping/save_map "file_path: '/home/integration/elevation_map.bag' topic_name: ''"

load_map (grid_map_msgs/ProcessFile)

从 rosbag 文件加载融合网格图和原始网格图。字段topic_name必须是基本名称，即没有前导斜杠字符 (/)。如果字段topic_name为空，则elevation_map默认使用。使用默认主题名称的示例：

rosservice call /elevation_mapping/load_map "file_path: '/home/integration/elevation_map.bag' topic_name: ''"

reload_parameters (std_srvs/Trigger)

触发所有高程测绘参数的重新加载，可用于在线重新配置参数。用法示例：

rosservice call /elevation_mapping/reload_parameters {}

disable_updates (std_srvs/Empty)

停止使用传感器输入更新高程图。触发更新停止：

rosservice call /elevation_mapping/disable_updates {}

enable_updates (std_srvs/Empty)

开始使用传感器输入更新高程图。触发更新开始：

rosservice call /elevation_mapping/enable_updates {}

3 参数Parameters

elevation mapping高程图工程的配置文件主要在elevation_mapping/config/sensor_processors/**.yaml、elevation_mapping/elevation_mapping_demos/config/elevation_maps/**.yaml、elevation_mapping/elevation_mapping_demos/config/postprocessing/**.yaml和、elevation_mapping/elevation_mapping_demos/config/robots/**.yaml，分别对应传感器的参数设置、高程图的参数设置、后处理的参数设置和机器人的参数设置。

input_sources(输入点云参数)
首先是机器人中的参数配置，目前已不建议使用point_cloud_topic字段，建议改用input_sources，详细可参考如下对输入点云的参数设置：

input_sources:
    front: # A name to identify the input source
      type: pointcloud # Supported types: pointcloud
      topic: /lidar_front/depth/points
      queue_size: 1
      publish_on_update: true # Wheter to publish the elevation map after a callback from this source. 
    rear:
      type: pointcloud
      topic: /lidar_rear/depth/points
      queue_size: 5
      publish_on_update: false

robot_pose_topic（字符串，默认：/robot_state/pose）

机器人位姿和协方差主题的名称。
base_frame_id（字符串，默认：/robot）

机器人基础 tf 的 id。
map_frame_id（字符串，默认值：/map）

高程图tf的id。
track_point_frame_id（字符串，默认：/robot）

高程图随着机器人沿着_轨迹点_移动。这是定义跟踪点的 tf 帧的 id。
track_point_x, track_point_y, track_point_z（双精度，默认值：0.0, 0.0, 0.0）

高程图随着机器人沿着_轨迹点_移动。这是轨迹点在中的位置track_point_frame_id。
robot_pose_cache_size（整数，默认值：200，最小值：0）

机器人姿态缓存的大小。
min_update_rate（双精度，默认值：2.0）

根据新测量值或机器人姿态估计更新高程图的最小更新率（以 Hz 为单位）。
fused_map_publishing_rate（双精度，默认值：1.0）

发布整个（融合）高程地图的帧率。
relocate_rate（双精度，默认值：3.0）

检查高程图是否遵循跟踪点进行重定位的速率（以 Hz 为单位）。
length_in_x, length_in_y（双精度，默认值：1.5，最小值：0.0）

高程图的大小（以米为单位）。
position_x, position_y（双精度，默认值：0.0）

高程图框架中高程图中心的位置。此参数设置生成的高程图与发布它的框架 ( map_frame_id) 之间的平面位置偏移。track_point_frame_id仅当不使用参数时才有用。
resolution（双精度，默认值：0.01，最小值：0.0）

高程图的分辨率（像元大小，以米/像元为单位）。
min_variance, max_variance（双精度，默认值：9.0e-6, 0.01）

高程地图方差数据的最小值和最大值。
mahalanobis_distance_threshold（双精度，默认值：2.5）

高程图中的每个像元的高度值都具有不确定性。根据现有高度分布的马氏距离和新测量值，传入数据将与现有估计融合、覆盖或忽略。该参数确定马哈拉诺比斯距离的阈值，该阈值决定如何处理传入的测量值。
sensor_processor/ignore_points_above（双精度，默认值：inf）

深度传感器引入的点高度的硬阈值。在数据收集步骤中，高度超过此阈值的点将被视为无效。
sensor_processor/ignore_points_below（双精度，默认值：-inf）

深度传感器引入的点高度的硬阈值。在数据收集步骤中，高度低于此阈值的点将不会被视为有效。
multi_height_noise（双精度，默认值：9.0e-7）

添加到高于该特定位置的当前高程图的测量值的噪声。仅当点超过马哈拉诺比斯距离阈值时，才会执行此噪声添加过程。较高的值有助于更快地适应动态环境（例如，移动的物体），但可能会导致高度估计中产生更多噪声。
min_horizontal_variance, max_horizontal_variance（双精度，默认：pow(分辨率 / 2.0, 2), 0.5)

高程地图水平方差数据的最小值和最大值。
enable_visibility_cleanup（布尔值，默认值：true）

启用/禁用一个单独的线程，该线程通过源自传感器框架的光线追踪从地图中删除不再可见的元素。
visibility_cleanup_rate（双精度，默认值：1.0）

执行可见性清理的速率（以 Hz 为单位）。
enable_continuous_cleanup（布尔值，默认值： false）

启用/禁用高程图的连续清理。如果启用，在每个新传感器数据到达时，高程图将被清除并仅用来自传感器的最新数据填充。启用连续清理后，可见性清理将自动禁用，因为在这种情况下不需要。
num_callback_threads（整数，默认值：1，分钟：1）

用于处理回调的线程数。更多线程会导致更高的吞吐量，但代价是更多的资源使用。
postprocessor_pipeline_name（字符串，默认值：postprocessor_pipeline）

要执行后处理的管道的名称。它期望将管道配置加载到该名称下的节点的私有命名空间中。例如：

  <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
      ...
      <rosparam command="load" file="$(find elevation_mapping_demos)/config/postprocessor_pipeline.yaml" />
  </node>

postprocessor_num_threads（整数，默认值：1，最小值：1）

用于异步后处理的线程数。更多线程会导致更高的吞吐量，但代价是更多的资源使用。
scanning_duration（双精度，默认值：1.0）

用于可见性清理的传感器扫描持续时间（以秒为单位）。将此粗略设置为两次连续完整扫描之间所需的持续时间（例如，对于 30 Hz 的 ToF 相机为 0.033，对于旋转激光扫描仪为 3 s）。根据扫描的密集程度或稀疏程度，增加或减少扫描持续时间。较小的值会导致更快的动态对象删除，较大的值有助于减少错误的地图清理。
sensor_cutoff_min_depth, sensor_cutoff_max_depth（双精度，默认值：0.2、2.0）

距离传感器测量长度的最小值和最大值。忽略此间隔之外的测量值。
sensor_model_normal_factor_a, sensor_model_normal_factor_b, sensor_model_normal_factor_c, sensor_model_lateral_factor（双精度）

传感器噪声模型的数据。
initialize_elevation_map（布尔）， initialization_method（整数）， length_in_x_init_submap（双精度，m）， length_in_y_init_submap（双精度，m）， init_submap_height_offset（双精度，m）， init_submap_variance（双精度）， target_frame_init_submap（字符串）

如果启用（prinistize_elevation_map：true），请在init_submap_height_offset的高度偏移围绕target_frame_init_submap的orign_submap_height_offset的高度偏移量初始化size（umpap_length_x，submap_length_x，submap_length_y）的平面（onimplization_method：0）。该方差设置为init_submap_variance。
increase_height_alpha（双精度，默认：0.0，最小值：0.0，最大值：0.99）

高程 = $increase_height_alpha * previous_z + (1.0 -increase_height_alpha) * new_measured_z$ 凸组合参数，用于针对分布点形成新的融合高度观测。对于尚未观察到的cell，高度高于马哈拉诺比斯上限的观察会scanning_duration 触发高度估计的重新初始化。重新初始化被参数化为先前高度估计和观测的凸组合：
- 0.0：新的观察作为初始化新模式，先前的数据被丢弃。
- 1.0：不考虑当前扫描中更高的、不符合分布的新观察点。先验保留为模式。
- 介于两者之间：值越高，对现有的先前估计的偏差就越大。将形成估计和测量之间的高度和方差的凸组合以初始化新的高斯高度分布。