elevation_mapping使用笔记

节点相关介绍: elevation_mapping

这是该工程最主要的节点，该节点主要使用距离传感器，机器人姿态、方差信息，建立一个具有方差的高程图

订阅话题：

• /points (sensor_msgs/PointCloud2)

  距离传感器所测量的数据

• /pose (geometry_msgs/PoseWithCovarianceStamped)

  机器人姿态和姿态协方差

• /tf (tf/tfMessage)

  The transformation tree.

发布话题：

• elevation_map (grid_map_msg/GridMap)

  整个（融合）高程图。 它会定期发布（请参阅fused_map_publishing_rate参数）或者在调用trigger_fusion服务之后发布。

• elevation_map_raw (grid_map_msg/GridMap)

  融合之前的原始地图

Services

• trigger_fusion (std_srvs/Empty)

  触发整个高程图的融合过程并将其发布。 例如，您可以从控制台触发地图融合步骤

    rosservice call /elevation_mapping/trigger_fusion
  

• get_submap (grid_map_msg/GetGridMap)

  获取所请求位置和大小的融合高程子图。 例如，您可以在位置（-0.5,0.0）和大小（0.5,1.2）处获取融合高程子图

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

• clear_map (std_srvs/Empty)

  启动清除整个地图以进行重置

    rosservice call /elevation_mapping/clear_map
  

• 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: ''"
  

参数：

• point_cloud_topic (string, default: "/points")

  距离测量话题的名称

• robot_pose_topic (string, default: "/robot_state/pose")

  机器人姿势和协方差话题的名称

• base_frame_id (string, default: "/robot")

  The id of the robot base tf frame.

• map_frame_id (string, default: "/map")

  The id of the tf frame of the elevation map.

• track_point_frame_id (string, default: "/robot")

  The elevation map is moved along with the robot following a track point. This is the id of the tf frame in which the track point is defined.

• track_point_x, track_point_y, track_point_z (double, default: 0.0, 0.0, 0.0)

  The elevation map is moved along with the robot following a track point. This is the position of the track point in the track_point_frame_id.

• robot_pose_cache_size (int, default: 200, min: 0)

  机器人位姿缓存的大小

• min_update_rate (double, default: 2.0)

  从机器人姿态估计的新测量值更新高程图的最小更新速率（以Hz为单位）

• fused_map_publishing_rate (double, default: 1.0)

  The rate for publishing the entire (fused) elevation map.

• relocate_rate (double, default: 3.0)

  The rate (in Hz) at which the elevation map is checked for relocation following the tracking point.

• length_in_x, length_in_y (double, default: 1.5, min: 0.0)

  The size (in m) of the elevation map.

• position_x, position_y (double, default: 0.0)

  高程图中心的位置，在高程图框架中。 此参数设置生成的高程图与其发布的帧之间的平面位置偏移（map_frame_id）。 仅在未使用track_point_frame_id参数时才有用。

• resolution (double, default: 0.01, min: 0.0)

  The resolution (cell size in m/cell) of the elevation map.

• min_variance, max_variance (double, default: 9.0e-6, 0.01)

  高程图方差数据的最小值和最大值。

• mahalanobis_distance_threshold (double, default: 2.5)

  高程图中的每个单元格的高度值都存在不确定性。 根据现有高度分布的Mahalonobis距离和新测量值，输入数据与现有估计值融合，覆盖或忽略。 此参数确定马哈拉诺比斯距离的阈值，该阈值确定如何处理传入的测量。

• sensor_processor/ignore_points_above (double, default: 0.4)    深度传感器引入的点高度的硬阈值。 在数据收集步骤中，高度超过此阈值的点将被视为无效。

• multi_height_noise (double, default: 9.0e-7)

  噪声添加到高于该特定位置的当前高程图的测量值。 仅当点落在马哈拉诺比斯距离阈值上时才执行此噪声添加过程。 较高的值对于更快地适应动态环境（例如，移动物体）是有用的，但是可能在高度估计中引起更多噪声。

• min_horizontal_variance, max_horizontal_variance (double, default: pow(resolution / 2.0, 2), 0.5)

  高程图水平方差数据的最小值和最大值。

• enable_visibility_cleanup (bool, default: true)

  启用/禁用单独的线程，该线程通过光线跟踪从传感器帧中删除不再可见的地图中的元素。

• visibility_cleanup_rate (double, default: 1.0)

  The rate (in Hz) at which the visibility constraint is performed.

• scanning_duration (double, default: 1.0)

  The sensor's scanning duration (in s) which is used for the visibility cleanup. Set this roughly to the duration it takes between two consecutive full scans (e.g. 0.033 for a ToF camera with 30 Hz, or 3 s for a rotating laser scanner). Depending on how dense or sparse your scans are, increase or reduce the scanning duration. Smaller values lead to faster dynamic object removal and bigger values help to reduce faulty map cleanups.传感器的扫描持续时间（以s为单位），用于清除可见性。 将其大致设置为两次连续完整扫描之间的持续时间（例如，对于30Hz的ToF相机为0.033，对于旋转激光扫描仪为3s）。 根据扫描的密集程度或稀疏程度，增加或减少扫描持续时间。 较小的值可以更快地移除动态对象，更大的值有助于减少错误的映射清理。

• sensor_cutoff_min_depth, sensor_cutoff_max_depth (double, default: 0.2, 2.0)

  距离传感器测量长度的最小值和最大值。 超出此间隔的测量值将被忽略。

• sensor_model_normal_factor_a, sensor_model_normal_factor_b, sensor_model_normal_factor_c, sensor_model_lateral_factor (double)

  传感器噪声模型的数据。

---

---

基本用法——学习使用demo

使用simple_demo.launch学习

打开simple_demo.launch

<launch>

  <!-- Elevation mapping node -->
  <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/robots/simple_demo_robot.yaml" />
    <rosparam command="load" file="$(find elevation_mapping_demos)/config/elevation_maps/simple_demo_map.yaml" />
    <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/kinect_nguyen_et_al.yaml" />
  </node>

  <!-- Launch visualizations for the resulting elevation map -->
  <include file="$(find elevation_mapping_demos)/launch/visualization.launch" />

  <!-- Launch RViz with the demo configuration -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find elevation_mapping_demos)/rviz/elevation_map_visualization.rviz" />

</launch>

可以看到主要执行的node为elevation_mapping节点，需要传入的参数为

• simple_demo_robot.yaml

point_cloud_topic: "/points"  # 距离测量话题的名称
map_frame_id: "map"           # The id of the tf frame of the elevation map.
robot_base_frame_id: "base"     
robot_pose_with_covariance_topic: "/pose"  
robot_pose_cache_size: 200    # 机器人位姿缓存的大小
track_point_frame_id: "base"  # The elevation map is moved along with the robot following a track point. This is the id of the tf frame in which the track point is defined.
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

需要传入点云的话题名称/points，位姿的协方差矩阵/pose

• simple_demo_map.yaml

length_in_x: 2.5   # The size (in m) of the elevation map.
length_in_y: 2.5   # The size (in m) of the elevation map.


# 高程图中心的位置，在高程图框架中。 此参数设置生成的高程图与其发布的帧之间的平面位置偏移（map_frame_id）仅在未使用track_point_frame_id参数时才有用。
position_x: 0.0
position_y: 0.0
resolution: 0.01
min_variance: 0.000009
max_variance: 0.01

# 高程图中的每个单元格的高度值都存在不确定性。 根据现有高度分布的Mahalonobis距离和新测量值，输入数据与现有估计值融合，覆盖或忽略。 此参数确定马哈拉诺比斯距离的阈值，该阈值确定如何处理传入的测量。
mahalanobis_distance_threshold: 2.5

# 噪声添加到高于该特定位置的当前高程图的测量值。 仅当点落在马哈拉诺比斯距离阈值上时才执行此噪声添加过程。 较高的值对于更快地适应动态环境（例如，移动物体）是有用的，但是可能在高度估计中引起更多噪声。
multi_height_noise: 0.0000009

传入地图初始化相关参数

• kinect_nguyen_et_al.yaml

# Kinect (short-range mode) model taken from:
# Nguyen, C. V., Izadi, S., & Lovell, D. (2012).
# Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking.
sensor_processor/type: structured_light

# 距离传感器测量长度的最小值和最大值。 超出此间隔的测量值将被忽略。
sensor_processor/cutoff_min_depth: 0.35
sensor_processor/cutoff_max_depth: 3.0

# 传感器噪声模型的数据。
sensor_processor/normal_factor_a: 0.0012
sensor_processor/normal_factor_b: 0.0019
sensor_processor/normal_factor_c: 0.4
sensor_processor/normal_factor_d: 0
sensor_processor/normal_factor_e: 1
sensor_processor/lateral_factor: 0.001376915 # for theta = 45°: 0.835 * 0.00780 [mm/px] / 4.73 [mm]

传入kinect传感器噪声模型相关参数