用自己的雷达和IMU运行cartographer

  最近在做项目，需要用雷达和IMU去建图，根据系统算法的输入，目前可以使用IMU的系统只有cartographer，而且图优化的cartographer更适合于大范围场景的地图构建，非常适用于工厂AGV的作业。
  如果不是搞cartographer内部算法，只是想要运用自己的传感器来复现，其实不需要看内部算法，只需要配置launch文件、lua文件以及urdf文件就可以了！
  我刚开始跑cartographer的时候，是直接按照官网的方法直接运行数据集，那时候感觉效果真的非常棒，于是想用自己的传感器去试试，但是自己又不知道该从哪下手，后面就是从网上找各种方法，发现按照网上的方法配置以后效果非常差。其实原因主要有以下几点：

  1.传感器不适用

   一开始运行cartographer的时候并没有用IMU。此时用的是EAI的YDLIDAR—G2，只需要配置revo_lds.lua文件以及demo_revo_lds.launch文件。具体配置如下：
原revo_lds.launch文件

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "horizontal_laser_link",
  published_frame = "horizontal_laser_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 = false
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

修改后的revo_lds.lua文件

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "laser",
  published_frame = "laser",
  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 = false
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

  其实就是将坐标系改为自己的雷达坐标系就可以了。我的雷达坐标系是laser，如果不知道自己雷达坐标系，可以先启动雷达，然后另起终端输入:

rostopic echo /雷达话题

  其中的frame id就是雷达对应的坐标系。接着就是修改launch文件。
原demo_revo_lds.launch文件

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

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

  /use_sim_time对应是否离线还是在线跑系统（类似时间戳），默认的value=“true”，就是离线跑数据包的意思，因此后面加上了node name=“playbag” pkg=“rosbag” type=“play” args="–clock $(arg bag_filename)" bag包回放。如果要在线跑的话，只需将value改为false，并将bag包回放这句话删除就可以了。remap重映射话题，改为自己的雷达话题就可以了。因此修改后的launch文件如下。

<launch>
  <param name="/use_sim_time" value="false" />
  
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename agv_cyx.lua"
      output="screen">
    <remap from="scan" to="scan" />
  </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文件中用到的lua文件是自己修改后的lua文件。
   最后的建图效果如下：

  可以看出，单纯用雷达建出的效果非常差，重影非常多，根本用不起来。后面就参考了链接: Cartographer中对激光雷达运动畸变的处理方法分析.对代码进行了修改，矫正雷达数据反序以及频率不稳定的问题，但是发现建图效果还是一样的差。然后看到了这个博客: 用自己的雷达进行Cartographer建图或仿真.这个博客里面说到各种雷达跑cartographer的效果是不一样的，正好实验室有个思岚雷达，换上试了试，发现效果真的很棒！如下图：

   还真的是雷达的问题。

   2.urdf文件和lua文件配置不正确

  以上只是单纯的用雷达建图，为了进一步提升建图效果，需要加IMU，此时还需要配置urdf文件。一开始配置urdf的时候都是参考网上的步骤，但其实还是要依据自身传感器的情况去撰写urdf文件，假如想用机器人完成建图，那么就定义三个坐标系，分别是laser_joint、imu_joint以及base_link坐标系，base_link坐标系是机器人的底盘坐标系，这个可以随意定制，一般选取机器人中心就行。而laser_joint和imu_joint坐标系需要计算它们的外参，这个比较麻烦，目前我还没找到标定二维激光雷达和IMU外参的工具。我的方法就是将IMU放到它的正下方，这样的话相当于就是y轴差了1-2cm。坐标系的旋转情况根据各传感器的基准而定，具体可以参考传感器的出厂文件。比如思岚A2的传感器定义如下：

  因为我想要实现和数据集一样的效果，因此在demo_backpack_2d.urdf文件的基础上进行了修改，最后撰写的urdf文件如下：

<robot name="agv">
  <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_link">
    <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.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link">
  <!--link name="base_footprint"-->
    <visual>
    <geometry>
        <!--cylinder length="0.45" radius="0.45"/-->
        <box size="0.60 0.52 0.4"/> 
    </geometry>
    <origin rpy="0 0 0" xyz="0  0 .2"/>
    <material name="blue">
        <color rgba="0 1 .8 0.5"/>
    </material>
    </visual>
 </link>

  <joint name="imu_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="imu_link" />
    <origin rpy="0 3.1415926 0" xyz="0.15 0 0.02" />
  </joint>

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

</robot>

  其次要修改lua文件，也是在backpack_2d.lua文件的基础上进行修改。如下：

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

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_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.num_accumulated_range_data = 1


return options

  通样也要修改launch文件，首先是backpack_2d.launch:

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/cyx_rplidar.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 cyx_rplidar.lua"
      output="screen">
    <remap from="scan" to="/scan" />
    <remap from="imu" to="/imu/data" />
  </node>

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

  然后是demo_backpack_2d.launch：

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

  <include file="$(find cartographer_ros)/launch/cyx.launch" />

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

  这些文件的名字都是自己随意定义的，只要将urdf、lua、launch文件对应起来即可。每次修改文件后要记得重新编译：

catkin_make_isolated --install --use-ninja
source install_isolated/setup.bash

  值得注意的是，urdf里的坐标系一定要和lua文件对应，不对应的话会出现坐标系之间的警告，假如将urdf中的link name=laser改为了link name=laser_link，会出现如下错误：

  如果不知道自己的传感器的frame id的，可以通过rostopic echo /传感器数据进行查看。
  其次，lua文件中的tracking frame一定要是urdf中IMU的坐标系。假如写成了base_link坐标系，也会出错sensor_bridge.cc:136] 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.：

  只要注意以上问题，相信走到这一步绝大部分人都能够运行了，这些文件的具体参数修改暂且没有探究过，博主也只是对传感器的外参和坐标系名称进行了修改。后续有知道的小伙伴可以在下方评论区一块探讨。

  3.parent坐标系转换有误

  这个问题大部分人遇不到，困扰了我很久。因为我在网上并没有找到解决办法，后来想出来的。在上一步修改完urdf、lua、launch文件后，重新运行的时候出现了如下问题：

  根据错误提示，是因为IMU坐标系和雷达坐标系的父亲坐标系不是同一个坐标系，然后我就通过

rosrun tf view_frames

查看了坐标系变换，如下图：

而数据集中的tf变化是这样的：

  这就很奇怪了，我明明就已经在urdf文件中设置了它们各自的父亲坐标系了，为啥会出现问题，然后就是各种重新些urdf文件，差点怀疑人生，都已经感觉上天和我作对哈哈哈。暂停了一段时间后，这天我突然想到会不会是传感器本身驱动有问题。果真，雷达没有问题，而IMU驱动有问题！原来xsens系列IMU驱动的官方文件中就已经定义了IMU的坐标系为world！于是我就去修改了IMU的驱动文件，根据launch文件提示的内容：

  然后找到了xsens_mti_node.yaml文件，里面有一个publish_transfrom：

  这个参数将IMU的parent坐标系设为world，于是将其改为false，最终重新启动了IMU、lidar、cartographer，总算是成功了，效果如下：
  蓝色小车是通过urdf文件定义出来的，想要的可以去参考上面的urdf文件，开心！
  加了IMU后，最终的建图效果如下：

  very nice！几乎和数据集一样喽。
  如果这篇文章对你有帮助的话，点个赞加关注吧！