3D激光雷达+IMU 用cartographer建立２D导航图（概率栅格地图）

3D激光雷达+IMU 用cartographer建立２D导航图（概率栅格地图）

首先将３Ｄ激光点云转成２Ｄ
https://blog.csdn.net/geerniya/article/details/84880514
消息类型　senser_msgs/pointcloud2 转成　senser_msgs/laserscan

提取关键信息：

IMU topic：/BMI088
IMU frame_id：imu_link
lidar topic：/laser_scan
lidar frame_id：lidar

1.　修改/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files／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

将tracking_frame 修改为imu坐标系

2.　修改/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files／backpack_2d.urdf文件

<robot name="cartographer_backpack_2d">
  <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="lidar">  //雷达坐标系
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </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" />　//imu坐标系相对baselink位置
  </joint>

  <joint name="lidar_link" type="fixed">
    <parent link="base_link" />
    <child link="lidar" />　
    <origin xyz="0 0 0" />　　//雷达坐标系相对baselink位置
  </joint>

</robot>

主要修改imu和雷达坐标系相对位置关系

３.　修改/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files／backpack_2d.launch文件

<launch>
  <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 backpack_2d.lua"
      output="screen">
    <!--<remap from="echoes" to="horizontal_laser_2d" />-->
    <remap from="scan" to="/laser_scan" />
    <remap from="imu" to="/BMI088" />
  </node>

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

和用于话题的重映射，Cartographer需要的是points和imu，我们提供的是/points_raw和/imu_raw

4.　修改/catkin_ws/src/cartographer_ros/cartographer_ros/configuration_files／demo_backpack_2d.launch文件(可修改，也可不修改）

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

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

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

</launch>

修改成上面这样，可以直接边播包边建图

５.　建图
如果修改了４　运行下面代码

roslaunch cartographer_ros demo_backpack_2d.launch
rosbag play --clock 2021-07-14-16-54-552d.bag //包的名称

如果不修改４　运行下面代码

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=/root/projects/Downloads/2021-07-14-16-54-552d.bag

６. 保存结果
先保存成.pbstream格式文件

rosservice call /finish_trajectory 0
rosservice call /write_state "{filename: '/root/dataset/map0714.pbstream'}"

然后再保存成pgm和yaml

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=/root/dataset/map -pbstream_filename=/root/dataset/map0714.pbstream -resolution=0.05