cartographer接入2D雷达laser+imu实时建图

前言

最近也是需要稍微看一下cartographer，然后用来跑2D建图，且需要融入imu数据

这篇文章也是自己在搞的过程中记录下来，以免自己以后忘记了，也希望能给小伙伴们们提供帮助

另外需要注意的是，我这里使用的carto launch是demo_revo_lds.launch，默认情况下是用来跑bag的，也没有设置imu的接口

1 接入laser

接入laser相对简单点，而且不了解cartographer的人也可以接入，总的来说就是改topic、frame_id、加入urdf，那么下面我们就来进行修改吧

1.1 修改laser启动文件

一般来说，laser的启动文件里面都是有直接的参数可以修改laser的topic和frame_id的，我这拿镭神的lsn10作为例子吧，我们直接打开启动雷达的launch文件，如下：

<launch>
    <node name="lsn10" pkg="lsn10" type="lsn10" output="screen" >
        <param name="scan_topic" value="scan"/>           #设置激光数据topic名称
        <param name="frame_id" value="laser_link"/>       #激光坐标
        <param name="serial_port" value="/dev/ttyUSB1" />  #雷达连接的串口
        <param name="baud_rate" value="230400" />         #雷达连接的串口波特率
	<param name="angle_disable_min" value="0.0"/>           #雷达裁剪角度开始值 
	<param name="angle_disable_max" value="0.0"/>           #雷达裁剪角度结束值 
	<param name="min_range" value="0.0"/>                   #雷达点云距离最小值
	<param name="max_range" value="100.0"/>                 #雷达点云距离最大值
    </node>
    
  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lsn10)/rviz/default.rviz" output="screen"/>      #雷达点云配置 -->

</launch>

我用的雷达不是很多，但是感觉差不多都是这样子吧，可以简单明了的找到需要改的两个参数scan_topic和frame_id。这两分别对应

• scan_topic horizontal_laser_2d
• frame_id horizontal_laser_link

修改后如下

<launch>
    <node name="lsn10" pkg="lsn10" type="lsn10" output="screen" >
        <param name="scan_topic" value="horizontal_laser_2d"/>           #设置激光数据topic名称
        <param name="frame_id" value="horizontal_laser_link"/>       #激光坐标
        <param name="serial_port" value="/dev/ttyUSB1" />  #雷达连接的串口
        <param name="baud_rate" value="230400" />         #雷达连接的串口波特率
	<param name="angle_disable_min" value="0.0"/>           #雷达裁剪角度开始值 
	<param name="angle_disable_max" value="0.0"/>           #雷达裁剪角度结束值 
	<param name="min_range" value="0.0"/>                   #雷达点云距离最小值
	<param name="max_range" value="100.0"/>                 #雷达点云距离最大值
    </node>
    
  <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lsn10)/rviz/default.rviz" output="screen"/>      #雷达点云配置 -->

</launch>

1.2 修改demo_revo_lds.launch文件

修改这个文件第一个是use_sim_time参数要改成false，第二个就是加入urdf，我直接使用了官方的，也就是tf变换，直接看成品吧

<launch>
  <param name="/use_sim_time" value="false" />
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.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 revo_lds.lua"
      output="screen">
    <remap from="scan" to="horizontal_laser_2d" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
<!--
  <node name="playbag" pkg="rosbag" type="play"
      args="-clock $(arg bag_filename)" />
-->
</launch>

这里面主要修改的就是launch下的前三个，两个param和一个node，然后屏蔽掉rosbag，需要注意直接屏蔽rosbag是会报错的，因为--clock用了两个扛，所以要么直接删除，要么删去一个扛，像我这样

到此为止，启动laser的launch和demo_revo_lds.launch即可实现用雷达建图了，下面再讲一下imu怎么加入

2 接入imu

接入imu其实也不难，就是没有了解过carto的参数会不知道除了什么问题，下面跟着我的步骤来改吧

2.1 修改revo_lds.lua

这个文件在launch上一级的configuration_files下面，launch加载的时候也可以看到。
这里需要改的地方只有三个

• tracking_frame base_link
• published_frame base_link
• TRAJECTORY_BUILDER_2D.use_imu_data true

直接看一下code吧

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_pose_extrapolator = true,
  use_odometry = false,
  use_nav_sat = false,
  use_landmarks = false,
  num_laser_scans = 1,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 0,
  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.,
}

MAP_BUILDER.use_trajectory_builder_2d = true

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1

POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65

return options

2.2修改imu启动文件

根laser是一样的，需要修改topic和frame，然后就可以直接接上了，我这里那lpmsbe1作为参考吧

• imu_topic imu
• frame_id imu_link

里面没有直接设置topic的接口，我们可以使用remap进行修改，然后frame_id直接进行设置即可，下面来看code吧

<launch>
    <!-- IG1 Sensor node -->
    <remap from="/imu/data" to="/imu" />
    <node name="lpms_be1" pkg="lpms_ig1" type="lpms_be1_node" output="screen">
        <param name="port" value="0001" type="string" />
        <param name="baudrate" value="115200" type="int" />
        <param name="frame_id" value="imu_link" type="string" />
    </node>

    <!-- imudata rad to deg conversion node -->
    <node name="imudata_deg" pkg="lpms_ig1" type="imudata_rad_to_deg_node" />

    <!-- Plots -->
    <node name="plot_imu_gyro" pkg="rqt_plot" type="rqt_plot" 
        args="/angular_vel_deg" />

    <node name="plot_imu_euler" pkg="rqt_plot" type="rqt_plot"
        args="/rpy_deg" />
</launch>

到此为止就全部搞定了，然后启动laser、imu、cartographer的demo_revo_lds.launch即可

3 发生过的报错

过程中发现了几个问题，但是有一个问题没有在网上查到直接的资料，问题如下

[FATAL] [1658375430.748902123]: F0721 11:50:30.000000 11409 sensor_bridge.cc:126] Check failed: sensor_to_tracking->translation().norm() < 1e-5 The IMU frame must be colocated with the tracking frame. Transforming linear acceleration into the tracking frame will otherwise be imprecise.
[ WARN] [1658375200.196875179]: W0721 11:46:40.000000 10794 ordered_multi_queue.cc:155] Queue waiting for data: (0, imu)

上面这个问题是我在设置了imu接口在graph中直接可以接上了的报错，这个时候我还没有更改tracking_frame 和published_frame 这两个参数，后来我直接将两个参数从horizontal_laser_link改到base_link后就不会报错了

4 实验发现

最开始我用的laser是比较便宜的，然后直接去建图，直接就胡成一团了，在加入了imu后，我发现imu对于建图的校准是非常有效的
但carto毕竟是laser主导的算法，其建图效果还是受到laser的影响，后来我们发现一个很大的问题！！
不同的雷达，对于建图的效果那影响不是一星半点的，下面进行总结一下吧，对于我用过的几个雷达的效果（暂时只用过镭神家的雷达进行测试）

• lsn10 —— 这款雷达相对来说比较便宜，然后有网口版和usb版的，我测试的时候用了usb版的，单laser直接不行，开始就直接糊了，后来加了imu后，发现imu对于转向的校准的帮助非常大，然后效果还可以，但是有时候往前走的时候停下来再往前走，算法中就会判定为没有走，然后我们发现停下来再转动，就是原地转圈，再走就会避免一些这样的情况
• lsm10 —— 这个稍微贵上一丢丢吧，也有串口网口版本，在我手上测试的是串口的，只用laser的时候，发现laser点云会抖动，可能一秒钟抖一下的，然后测试carto的时候，最开始的时候效果就只是不准确，到后面完全就用不了了，但是比lsn10稍微好点点，同样的加入imu后的效果也是会好点，但是有时候吧，他就出问题
• lsn301 —— 这个我也不知道有没有串口版本的，我测试的时候用的是网口版，然后没有加imu测试的，那个效果贼好，要是使用不严格的话，完全不需要加imu都可以，我们当时测试的场地有两个，大概都在10000平米的样子，那个效果真的好的没话说，根前面两个比，还是没有加imu的情况。只不过在极少数情况下转弯会没那么精确，可能就差一点点吧，但是不碍事。

然后我们的测试方向现在就有两个了，一个是判断是不是网口和串口对于建图效果的影响，第二个的话是不是雷达的影响。这个实验的话，我们后续再补上吧，这篇博客先这样吧

这篇好文章是转载于：学新通技术网