Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)

Cartographer使用3d激光雷达建2d图

  • 1.尝试过的方法
    • 1.1 参考官方demo_backpack_3d.launch文件,直接建图
    • 1.2在线或离线生成pbstream文件,再转成pgm,yaml文件
  • 2.最终方案!使用3d点云数据直接在rviz上生成2d图(导航图,栅格图)

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

1.尝试过的方法

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>

如果是边跑边建图,则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>

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

# rosbag info *****.bag

Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)_第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

看一下官方3d bag包跑出来的效果
Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)_第2张图片看起来效果还可以吧
再看一下我建的图
我先说一下这张图的问题:1.为什么没有出现白色区域(已知区域)2.为什么有这么多的噪点
后来重新调整了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

出现了如下效果图,噪点没有了,至关重要的一个参数是TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = true
Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)_第3张图片这个参数的作用就是开启实时的闭环检测来进行前段的扫描匹配,所以生成的图效果非常好,就是计算量会增大,消耗更多CPU。

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

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

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

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

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

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

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

接下来就是生成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

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

Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)_第4张图片
但是在路线闭环的情况下,就会出现激光雷达的波纹信息,如下图

Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)_第5张图片
虽然这种方法有白色可通行区域,但这个雷达波纹和行车轨迹看起来很恶心啊。
离线的方法我就不说了,效果是一样的。离线生成启动的launch文件是

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

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

2.最终方案!使用3d点云数据直接在rviz上生成2d图(导航图,栅格图)

这种方法是需要里程计/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>

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>

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

我后知后觉直接2d建图,它不香吗?看效果建图如下:
Cartographer使用3D激光雷达建立2D导航图(概率栅格地图)_第6张图片哇,真漂亮!

运行命令如下:

# roslaunch cartographer_ros demo_zch_2d.launch

再开启一个终端

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

保存地图命令

# rosrun map_server map_saver -f ***//你起的名字,例如my_map

我的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 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>

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

你可能感兴趣的:(Cartographer)