Cartographer使用３D激光雷达建立２D导航图（概率栅格地图）_3d点云在线生成2d地图

引言：确切说应该是使用３ｄ激光雷达建立概率栅格地图，一开始我使用了官方demo_backpack_3d.launch经过修改一些参数，但在rviz中显示的图，只有黑色和灰色，也就是代表的障碍物体和未知区域，缺少白色区域就是已知区域（可通行区域）。当只有灰色和黑色的时候，这个地图是没有办法拿来做导航用的。所以这个问题必须解决。
声明：北科天汇３２线激光雷达

１.尝试过的方法

1.1 参考官方demo_backpack_3d.launch文件，直接建图

先贴配置图
1. my_demo_backpack_3d.launch

<launch>
  <param name="/use_sim_time" value="true" />

  <include file="$(find cartographer_ros)/launch/demo_my_robot_3d.launch" />//换成自己的配置launch文件

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>
1
2
3
4
5
6
7
8
9
10

如果是边跑边建图，则use_sim_time应该设置为false。我是先录制的bag包，然后播放生成的图。
2. demo_my_robot_3d.launch

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_backpack_3d.urdf" />//换成自己的机器人配置文件

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_robot_3d.lua"	//换成自己配置文件
      output="screen">
	<remap from="points2" to="/rfans_driver/rfans_points" />//换成自己的bag包数据中的点云话题
	
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />  
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

会看点云话题吗？用这个命令查看

# rosbag info *****.bag
1

看到这个topics中sensor_msgs/PointCloud2的这个话题类型了吗？前面的就是你自己数据的点云话题，我的点云话题是/rfans_driver/rfans_points,所以在launch文件中，把点云话题改成自己的

3. 配置lua文件
首先看一下使用官方backpack_3d.lua文件生成图的效果

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 2,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 320
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

看一下官方3d bag包跑出来的效果
看起来效果还可以吧
再看一下我建的图
我先说一下这张图的问题：１．为什么没有出现白色区域（已知区域）２．为什么有这么多的噪点
后来重新调整了lua文件
my_robot_3d.lua

include "map_builder.lua"
include "trajectory_builder.lua"


options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",	--imu
  published_frame = "base_link",   --laser
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans =0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 1.,
  odometry_sampling_ratio = 1,    --1.
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_2D.min_z = 0.1
TRAJECTORY_BUILDER_2D.max_z = 1.5
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1   	--1
TRAJECTORY_BUILDER_3D.min_range = 1.3			--houjia
TRAJECTORY_BUILDER_3D.max_range = 25.			--houjia
TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true		--houjia
--TRAJECTORY_BUILDER_3D.submaps.num_range_data=90.

MAP_BUILDER.use_trajectory_builder_3d = true
MAP_BUILDER.num_background_threads = 7
POSE_GRAPH.optimization_problem.huber_scale = 5e2
POSE_GRAPH.optimize_every_n_nodes = 60
POSE_GRAPH.constraint_builder.sampling_ratio = 0.03
POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 10
POSE_GRAPH.constraint_builder.min_score = 0.62
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.66

return options
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

出现了如下效果图，噪点没有了，至关重要的一个参数是TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true
这个参数的作用就是开启实时的闭环检测来进行前段的扫描匹配，所以生成的图效果非常好，就是计算量会增大，消耗更多CPU。

但是这个图还是不能用来导航啊，只有灰色和黑色区域（也就是障碍物和未知区域）。

现在剩下的问题就是要解决白色区域了

1.2在线或离线生成pbstream文件，再转成pgm，yaml文件

在线生成pbstream文件，还是启动上个方法的launch文件

# roslaunch cartographer_ros my_demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/***.bag
1

在rviz生成图了之后，启动一个新终端使用如下命令：

＃ rosservice call /finish_trajectory 0　	//停止接受传感器数据
＃ rosservice call /write_state  /home/路径/***.pbstream		//生成pbstream文件
1
2

接下来就是生成pgm文件

＃ roslaunch cartographer_ros assets_writer_ros_map.launch bag_filenames:=${HOME}/Downloads/2020-04-29-11-25-22.bag pose_graph_filename:=${HOME}/Downloads/2020-04-29-11-25-22.bag.pbstream
1

把bag文件路径和pbstream文件路径改成你自己的，生成图如下：

但是在路线闭环的情况下，就会出现激光雷达的波纹信息，如下图

虽然这种方法有白色可通行区域，但这个雷达波纹和行车轨迹看起来很恶心啊。
离线的方法我就不说了，效果是一样的。离线生成启动的launch文件是

roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/2020-05-13-16-37-40.bag
1

一样把launch文件换成自己的点云话题！运行完之后会直接生成pbstream文件

２.最终方案！使用３d点云数据直接在rviz上生成２d图（导航图，栅格图）

这种方法是需要里程计/odom信息，不用imu信息
先贴自己配置图
demo_zch_2d.launch

<launch>
  <param name="/use_sim_time" value="true" />
  <!-- <node pkg="tf" type="static_transform_publisher" name="base_laser_broadcaster" args="0 0 0 0 0 0  laserbase_footprint /base_footprint 20" /> -->
  <include file="$(find cartographer_ros)/launch/zch_rslidar_2d.launch" />
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory
              $(find cartographer_ros)/configuration_files
          -configuration_basename zch_rslidar_2d.lua"//你自己的lua文件
      output="screen">
      <remap from="/odom" to="/odom" />		//你自己的里程计话题
      <remap from="/points2" to="/rfans_driver/rfans_points" />　　//你自己的点云话题
  </node>
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/rslidar_2d.rviz" />
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

zch_rslidar_2d.launch

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_backpack_3d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />
</launch>
1
2
3
4
5
6
7

zch_rslidar_2d.lua

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "base_link",
  published_frame = "base_link",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,
  lookup_transform_timeout_sec = 0.2,
  submap_publish_period_sec = 0.3,
  pose_publish_period_sec = 5e-3,
  trajectory_publish_period_sec = 30e-3,
  rangefinder_sampling_ratio = 0.5,
  odometry_sampling_ratio = 1.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35

TRAJECTORY_BUILDER_2D.min_range = 0.6
TRAJECTORY_BUILDER_2D.max_range = 30.
TRAJECTORY_BUILDER_2D.min_z = -0.3
TRAJECTORY_BUILDER_2D.max_z = 1.5
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

POSE_GRAPH.optimize_every_n_nodes = 35		--houjia  35
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e3
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e3

--POSE_GRAPH.optimization_problem.huber_scale = 1e2		
--POSE_GRAPH.constraint_builder.min_score = 0.65		
return options
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

我后知后觉直接２d建图，它不香吗？看效果建图如下：
哇，真漂亮！

运行命令如下：

# roslaunch cartographer_ros demo_zch_2d.launch
1

再开启一个终端

# rosbag play --clock 2020-06-01-14-22-23.bag
1

保存地图命令

＃　rosrun map_server map_saver -f ***//你起的名字，例如my_map
1

我的urdf文件也给你们参考吧
my_backpack_3d.urdf

<robot name="cartographer_backpack_3d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>

   <link name="imu">
    <visual>
      <origin xyz="0 0 0"/>
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>
  <link name="laser">
    <visual>
      <origin xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>
  
  <link name="base_link" />

  <joint name="imu_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu" />
    <origin xyz="0 0 0" rpy="0 0 ０"/>
  </joint>

  <joint name="laser_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>      

<!--  <joint name="imu2lidar" type="fixed">
    <parent link="laser" />
    <child link="imu" />
    <origin xyz="0 0 0"/>
  </joint> -->

</robot>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48

再给大家推荐一个修图软件　kolourpaint
祝大家顺利！