cartographer 3D运行录制rosbag包

目录：

1、运行多线激光雷达；

2、运行IMU；

3、录制rosbag包；

4、配置cartographer；

5、查看地图；

1、运行多线激光雷达：

　　主要是测试雷达是否正在运行，确认雷达点云topic类型是否一致，确认雷达frame id（可统一修改为laser_link）。

2、运行IMU：

　　主要时测试IMU是否正常运行，可以通过rostopic list、rostopic info /imu、rosmsg info /sensor_msgs/Imu进行确认，同时确认IMU的frame id（可统一修改为imu_link）；

　　另外，确认IMU输出数据中的角速度的单位，是角度每秒还是弧度每秒，cartographer需要的是弧度每秒。

3、录制rosbag包：

　　运行激光雷达和IMU，录制rosbag包：rosbag record -O b3_01  /IMU /Laser；

4、配置cartographer包：建议配置官网最新版，因为发现网上流传版本比较老，至少源代码的结构已经发生了较大变化，算法功能也发生了较大变化

　　4.1、配置lua文件：主要修改以下几个参数

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu_link",  --修改为imu的frame id
  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 = 1,    --修改为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.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}
--修改为1，它表示累加几帧的point做为1帧，有些雷达是将360°的point分批次发布的，
--因此需要累加起来，方便去除运动畸变，这里用的雷达为一次发布360°的point，因此可以设定为1
TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1    

　　4.2、配置urdf文件：这里仅使用了一个激光雷达，注意IMU和lidar的link name和前面已运行的设备frame id对应起来　　

<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_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="laser_link">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

  <link name="base_link" />

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

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser_link" />
    <origin xyz="0. 0. -0.0475" rpy="0. 0. 0." />
  </joint>

</robot>

　　4.3、配置launch文件：这里使用offline节点或online节点都可以，offline节点可以建图，因此使用offline节点

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

  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_3d.rviz" />

  <node name="cartographer_offline_node" pkg="cartographer_ros"
      type="cartographer_offline_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basenames my_robot_3d.lua　　　　　　　　　　　　　　　<!-- 修改为自己的lua文件 -->
          -urdf_filenames $(find cartographer_ros)/urdf/my_robot_3d.urdf 　　<!-- 修改为自己的urdf文件 -->
          -bag_filenames $(arg bag_filenames)"
      output="screen">
    <remap from="/points2" to="/rfans_driver/rfans_points" />　　　　<!-- 将cartographer订阅的topic类型remap为雷达和IMU发布的topic类型 -->
    <remap from="/imu" to="/imu/data_raw" />　　　　　　　　　　　　　　<!-- 我觉得也可以将雷达和IMU发布的topic类型remap为cartographer订阅的类型 -->
  </node>

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

5、查看地图：上面修改的文件，如果是修改的/catkin_ws/src下的文件，需要rebuild，编译完成之后应该就可以运行自己录制的bag包了，也可以使用cartographer提供的check工具check一下bag包，通常情况下都没有问题，警告可以忽略

　　使用assets_writer_backpack_3d.launch可以将地图可视化，具体的配置方法和上面一样，通常也是lua文件和urdf文件配置，需要注意的是，如果想查看3D点云地图，需要安装points_cloud_viewer，同时在lua文件中增加如下语句：

    {
      action = "write_ply",
      filename = "points.ply",
    },
--    {
--      action = "color_points",
--      frame_id = "horizontal_vlp16_link", --因为没有这两个frame id，所以应该改为自己的frame id
--      color = { 255., 0., 0. },
--    },
--    {
--      action = "color_points",
--      frame_id = "vertical_vlp16_link",
--      color = { 0., 255., 0. },
--    },

　　运行相应的launch文件后，可以得到地图和轨迹的图片和点云：