这是为移动机器人的高程测绘而开发的ROS包。该软件专为配备姿态估计（例如 IMU 和里程计）和距离传感器（例如结构光（Kinect、RealSense）、激光测距传感器、立体摄像头）的机器人的（本地）导航任务而设计。所提供的高程图仅限于机器人周围，并反映了通过机器人运动（以机器人为中心的测绘）而聚集的姿态不确定性。该方法被开发用于明确处理机器人姿态估计的漂移。
这是研究代码，预计它会经常变化，并且不适用于任何特定用途。
作者：Péter Fankhauser
合著者：Maximilian Wulf
所属机构：ANYbotics
维护者：Maximilian Wulf，mwulf@anybotics.com，Magnus Gärtner，mgaertner @anybotics.com
该项目最初是在苏黎世联邦理工学院（自主系统实验室和机器人系统实验室）开发的。
这项工作是 ANYmal Research 的一部分，ANYmal Research 是一个致力于推动足式机器人技术的社区。
使用中的高程测绘软件的视频：
以下论文（可在此处获取）介绍了此软件中使用的以机器人为中心的高程测绘方法。如果您在学术背景下使用此作品，请引用以下出版物：
-
P. Fankhauser、M. Bloesch 和 M. Hutter， 《具有不确定定位的移动机器人的概率地形测绘》，载于《IEEE 机器人与自动化快报》（RA-L），第 3 卷，第 4 期，第 3019-3026 页，2018 年。（PDF）
@article{Fankhauser2018ProbabilisticTerrainMapping, author = {Fankhauser, P{\'{e}}ter and Bloesch, Michael and Hutter, Marco}, doi = {10.1109/LRA.2018.2849506}, title = {Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization}, journal = {IEEE Robotics and Automation Letters (RA-L)}, volume = {3}, number = {4}, pages = {3019--3026}, year = {2018} } -
P. Fankhauser、M. Bloesch、C. Gehring、M. Hutter 和 R. Siegwart， 《具有不确定性估计的以机器人为中心的高程测绘》，国际攀爬和行走机器人会议 (CLAWAR)，2014 年。（PDF）
@inproceedings{Fankhauser2014RobotCentricElevationMapping, author = {Fankhauser, P\'{e}ter and Bloesch, Michael and Gehring, Christian and Hutter, Marco and Siegwart, Roland}, title = {Robot-Centric Elevation Mapping with Uncertainty Estimates}, booktitle = {International Conference on Climbing and Walking Robots (CLAWAR)}, year = {2014} }
该软件基于机器人操作系统 ( ROS ) 构建，需要先安装。此外，以机器人为中心的高程测绘依赖于以下软件：
为了安装以机器人为中心的高程测绘，请将此存储库中的最新版本克隆到您的 catkin 工作区并使用 ROS 编译包。
cd catkin_workspace/src
git clone https://github.com/anybotics/elevation_mapping.git
cd ../
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build
使用以下工具构建测试
roscd elevation_mapping
catkin build --catkin-make-args run_tests -- --this
使用以下方法运行测试
rostest elevation_mapping elevation_mapping.test -t
为了让 Robot-Centric Elevation Mapping 与您的机器人一起运行，您需要调整一些参数。最简单的方法是从包中复制并调整您需要更改的所有参数文件elevation_mapping_demos（例如simple_demo示例）。这些具体是文件夹中的参数文件config和启动文件launch。
提供了一个运行示例，利用 Turtlebot3 模拟环境。此示例可用于测试高程测绘，作为进一步集成的起点。
首先，需要安装 Turtlebot3 模拟依赖项：
sudo apt install ros-melodic-turtlebot3*
可以使用以下命令启动高程测绘演示和 turtlebot3 模拟：
roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch
要使用键盘控制机器人，需要打开一个新的终端窗口（记得获取 ROS 环境）。然后运行
export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
可以通过按键a、w、d、向机器人发送速度输入x。要完全停止机器人，请按s。
.ply 发布为静态点云，elevation_mapping 订阅它并发布高程图。您可以通过 rviz 对其进行可视化。对于可视化，请选择/elevation_mapping/elevation_map_raw。
注意：您可能需要切换 grid_map_plugin 的可见性以将其可视化。
roslaunch elevation_mapping_demos ground_truth_demo.launch
虽然地面实况演示估算了地图框架中的高度，但简单演示设置了更现实的部署场景。在这里，elevation_map 配置为跟踪基准框架。首先，我们建议尝试一下并可视化其他已发布的主题，例如，/elevation_mapping/elevation_map_raw并将高度层更改为另一个层，例如elevation_inpainted。
这是主要的以机器人为中心的高程测绘节点。它使用距离传感器测量值以及机器人的姿势和协方差来生成带有方差估计的高程图。
-
/points（传感器消息/PointCloud2）距离测量。
-
/pose（geometry_msgs/PoseWithCovarianceStamped）机器人的姿势和协方差。
-
/tf（tf/tfMessage）變化樹。
-
elevation_map（grid_map_msgs/GridMap）整个（融合）海拔地图。它会定期发布（参见
fused_map_publishing_rate参数）或在trigger_fusion调用服务后发布。 -
elevation_map_raw（grid_map_msgs/GridMap）融合步骤之前的整个（原始）高程图。
-
trigger_fusion（std_srvs/空）触发整个海拔地图的融合过程并发布。例如，您可以使用以下命令从控制台触发地图融合步骤
rosservice call /elevation_mapping/trigger_fusion -
get_submap（grid_map_msgs/获取网格地图）获取请求位置和大小的融合高程子图。例如，您可以获取 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/获取网格地图）获取请求位置和大小的原始海拔子图。例如，您可以获取 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/空）启动清除整个地图以进行重置。使用以下命令触发地图清除
rosservice call /elevation_mapping/clear_map -
masked_replace（[grid_map_msgs/设置网格地图]）允许通过服务调用设置海拔地图的各个图层。图层蒙版可用于仅设置某些单元格，而不是整个地图。蒙版中包含 NAN 的单元格未设置，所有其他单元格均已设置。如果未提供图层蒙版，则整个地图将设置在两个地图的交集中。提供的地图的大小和位置可能与将要更改的地图不同。将海拔图层中标有蒙版的一些单元格设置为 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/触发器]）触发所有高程映射参数的重新加载，可用于在线重新配置参数。示例用法：
rosservice call /elevation_mapping/reload_parameters {} -
disable_updates（std_srvs/空）停止使用传感器输入更新海拔地图。使用以下方式触发更新停止：
rosservice call /elevation_mapping/disable_updates {} -
enable_updates（std_srvs/空）开始使用传感器输入更新海拔地图。触发更新时，请先输入
rosservice call /elevation_mapping/enable_updates {}
-
DEPRECATED point_cloud_topic（字符串，默认值：“/points”）距离测量主题的名称。请改用 input_sources。
-
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
不能使用空数组配置任何输入源：
input_sources: []
-
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（int，默认值：200，最小值：0）机器人姿势缓存的大小。
-
min_update_rate（双精度，默认值：2.0）根据新测量值或机器人姿势估计值更新高程图的最小更新率（以赫兹为单位）。
-
fused_map_publishing_rate（双精度，默认值：1.0）发布整个（融合）海拔地图的费率。
-
relocate_rate（双精度，默认值：3.0）检查海拔图以根据跟踪点进行重新定位的速率（以赫兹为单位）。
-
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）执行可见性清理的速率（以赫兹为单位）。
-
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>管道是 grid_map_filter 链，有关更多信息，请参阅 grid_map_demos/filters_demo.yaml 和ros/filters 。
-
postprocessor_num_threads（整数，默认值：1，最小值：1）用于异步后处理的线程数。线程越多，吞吐量越高，但资源使用量也越多。
-
scanning_duration（双精度，默认值：1.0）用于可见性清理的传感器扫描持续时间（以秒为单位）。将其大致设置为两次连续完整扫描之间的持续时间（例如，对于 30 Hz 的 ToF 相机，设置为 0.033 秒；对于旋转激光扫描仪，设置为 3 秒）。根据扫描的密集程度或稀疏程度，增加或减少扫描持续时间。较小的值会导致更快的动态对象移除，较大的值有助于减少错误的地图清理。
-
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（字符串）如果启用（
initialize_elevation_map：true），则在 的原点附近以 的高度偏移初始化一个大小为 ( , )initialization_method的平面 ( ：0) 。方差设置为。submap_length_xsubmap_length_yinit_submap_height_offsettarget_frame_init_submapinit_submap_variance -
increase_height_alpha（双精度，默认值：0.0，最小值：0.0，最大值：0.99）elevation = increase_height_alpha * previous_z + (1.0 - increase_height_alpha) * new_measured_z 凸组合参数，用于形成分布点之外的新融合高度观测值。对于尚未观察到的单元格，高度高于上限马哈拉诺比斯阈值的观测值会
scanning_duration触发高度估计的重新初始化。重新初始化被参数化为先前高度估计和观测值的凸组合：- 0.0：新的观察将用于初始化新模式，先前的数据被丢弃。
- 1.0：当前扫描中更高、超出分布的点的新观察结果不予考虑。先前的观察结果将保留为模式。
- 介于两者之间：值越高，对现有的先前估计的偏差越大。将形成高度和估计值与测量值之间的方差的凸组合，以初始化新的高斯高度分布。
查看变更日志
请使用问题跟踪器报告错误并请求功能。