【开源项目分享】用于机器人崎岖地形导航的高程图创建及Gazebo仿真

简介

该项目是由苏黎世联邦理工学院（自治系统实验室和机器人系统实验室）开发，用于创建以机器人为中心的高程图，以实现崎岖地形的自主导航任务。

项目演示

https://www.bilibili.com/video/BV1p5411t7Um/

【开源项目分享】Outdoor Terrain Mapping with ANYmal

https://www.bilibili.com/video/BV19k4y197iT/

【开源项目分享】用于机器人崎岖地形导航的高程图创建演示

下载安装

请参考elevation_mapping

elevation_mapping节点简介

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

订阅话题：

/points (sensor_msgs/PointCloud2)        距离传感器所测量的数据

/pose (geometry_msgs/PoseWithCovarianceStamped)    机器人姿态和姿态协方差

/tf (tf/tfMessage)         坐标信息

发布话题：

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: ‘’”

以velodyne32线激光和summit_xl_omni仿真为例

因为summit_xl比较难git下来，可以通过https://download.youkuaiyun.com/download/Travis_X/12321738下载。

创建vlp_32.launch文件

<launch>

  <!-- Elevation mapping node -->
  <node pkg="elevation_mapping" type="elevation_mapping" name="elevation_mapping" output="screen">
    <rosparam command="load" file="$(find elevation_mapping)/config/sensor_processors/velodyne_HDL-32E.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>

创建yaml文件

# Velodyne_HDL-32E
# TODO

#sensor_processor/type: laser
#sensor_processor/min_radius: 0.018
#sensor_processor/beam_angle: 0.0006
#sensor_processor/beam_constant: 0.0015

point_cloud_topic: "/velodyne_points"
map_frame_id: "map"
robot_base_frame_id: "base_link"
robot_pose_with_covariance_topic: "/ground_truth_pose"
robot_pose_cache_size: 200
track_point_frame_id: "/velodyne"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0

length_in_x: 20.0
length_in_y: 20.0
position_x: 0.0
position_y: 0.0
resolution: 0.20
min_variance: 0.000009
max_variance: 0.01
mahalanobis_distance_threshold: 2.5
multi_height_noise: 0.0000009

sensor_frame_id: "/velodyne"
sensor_processor/type: laser
sensor_processor/min_radius: 0.018
sensor_processor/beam_angle: 0.0006
sensor_processor/beam_constant: 0.0015

启动gazebo仿真环境和elevation_mapping

roslaunch summit_xl_gazebo summit_xl_omni.launch
rosrun elevation_mapping_demos ground_truth_pose_publisher.py 
 roslaunch elevation_mapping_demos vlp_32.launch 

演示效果

graph