cartographer跑自己的数据+3d建图篇(Lidar+imu)

接上篇2d建图(那个,参数解释,网上太多了,我真的懒得写了,大家自己去官网查吧,最近好忙)
老规矩,按步走,做好笔记防止以后忘记!!!(步骤和2d的差不多,只是配置不同)

(镭神雷达没有反射率这个数据么?建图过程中一直在报警告,找不到反射率什么的!!)
硬件:镭神32线半固态雷达,导远惯导系统.
软件:Ubuntu1604,ROS-Kinetic,cartographer框架
(建议:改动文件建议自己复制源文件进行新建文件改动，源文件作为保留用。)

1. 第一步,确定硬件之间的坐标转换关系.
   根据上面所说,我用到了一个多线雷达和IMU,所以在跑数据时必须要有这两个传感器的TF树变换关系.在cartographer中,用的是URDF文件进行构建该关系,当然,后来我在实车测试时屏蔽了这个文件,直接用车上的tf变换关系也是可以的,只要加入imu到激光雷达的变化即可.cartographer的urdf文件定义在~/catkin_cartographer/install_isolated/share/cartographer_ros/urdf$中,名字自己要记住,
   如下我的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.0 0.0"/>
      <geometry>
        <cylinder length="0.07" radius="0.05"/>
      </geometry>
    </visual>
  </link>

  <link name="lidar_mid">
    <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="lidar_left">
    <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="lidar_base">
    <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="lidar_rear">
    <visual>
      <origin xyz="0.0 0.0 0.0" />
      <geometry>
        <cylinder length="0.07" radius="0.05" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>

    <joint name="imu2mid" type="fixed">
    <parent link="lidar_mid"/>
    <child link="imu" />
    <origin xyz="0 -4.0 -0.2" rpy="0 3.14 0"/>
  </joint>

  <joint name="left2mid" type="fixed">
    <parent link="lidar_mid" />
    <child link="lidar_left" />
    <origin xyz="-0.85 -0.42 0" rpy="0 0.03 0" />
  </joint>

  <joint name="base2mid" type="fixed">
    <parent link="lidar_mid" />
    <child link="lidar_base" />
    <origin xyz="0.85 -0.42 0" rpy="0 0 -1.57" />
  </joint>

  <joint name="rear2mid" type="fixed">
    <parent link="lidar_mid" />
    <child link="lidar_rear" />
    <origin xyz="0 -4.6 0" rpy="-3.14159 -3.14159 0" />
  </joint>


</robot>

要强调的是,里面的参数都是多雷达标定后得到的,要根据实际中你车上的坐标去改.

2.第二步,准备数据,这个不麻烦,自己准备3d数据包即可,实在找不到的,博主这里附上自己的3d数据包,自行下载.

链接: https://pan.baidu.com/s/1Hu1qd4SQ2KKGqXbUU6rMxA 提取码: 4asr

3. 第三步,配置文件条理,这个步骤其实不复杂,只是每在配置文件夹里改一次,就要去源文件夹里做一下相同的修改.先贴路径,
   配置文件夹路径:
   
   源文件夹路径:

~/catkin_cartographer/src/cartographer_ros/cartographer_ros/

这两个文件夹里最主要的东西都是一样的,configuration_files是存储.lua的文件夹,launch是存储启动命令的文件夹,urdf是存储小车模型的文件夹.
**注意:**配置文件夹里的urdf决定了你的小车的模型,即TF树,源文件夹里的urdf你可以不用管.
而urdf文件在上面有说过了.

4. 第四步,开始配置:
   首先,进入配置文件夹里的configuration_files文件夹,新建my_robot_3d.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 = "imu",
  published_frame = "lidar_mid",
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_pose_extrapolator = false,
  use_odometry = false,
  use_nav_sat = false,#这里选择是否用GPS,博主的图建出来效果不错,定位也可以,后面也尝试了加入GPS建图定位,很nice
  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.,
  fixed_frame_pose_sampling_ratio = 1.,
  imu_sampling_ratio = 1.,
  landmarks_sampling_ratio = 1.,
}

TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1

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

保存退出,进入同级目录的launch文件夹下,新建demo_my_robot_3d.launch文件(可复制backpack_3d.launch,然后修改),内容如下

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/minibus.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"#加载3d的配置文件
      output="screen">
    <remap from="points2" to="/minibus/front/lslidar_point_cloud" />#这里是你自己3d点云雷达的话题
    <!--<remap from="points2_2" to="vertical_laser_3d" />-->
  </node>

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

然后,在该文件夹里找到demo_backpack_3d.launch文件,用gedit打开修改成:

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

  <include file="$(find cartographer_ros)/launch/demo_my_robot_3d.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>

保存退出到同级目录中,进入urdf文件夹,检查minibus.urdf文件是否存在,且里面坐标变换关系是否正确.至此,配置文件夹里的改动结束.

5. 第五步, 配置源文件夹里的文件.
   步骤按照配置文件夹里的配置步骤来,懒的话,直接复制过去也是可以的.
6. 第六步, 检查两个文件夹里做了相同的配置之后,保存退出到工作空间路径:
   (不要问我源文件夹和配置文件夹是什么,上面已经讲过了,往上翻翻就知道了)

cd ~
cd ~/catkin_cartographer/
catkin_make_isolated --install --use-ninja
source devel_isolated/setup.bash

7. 尝试建图
8. 命令:

cd ~/catkin_cartographer/
source devel_isolated/setup.bash
roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=~/2d.bag

效果如下,其实,我懒得重新建图了,下面这是我跑纯定位模式时的地图,红色点云部分就是点云匹配上后给出的效果.

9. 保存地图
   这里统一默认将地图保存为.pbstream的格式，当你觉得图建的不错 时候，另起终端，输入如下两条命令。路径0是cartographer默认的建图ID

cd catkin_cartographer/
source devel_isolated/setup.bash
rosservice call /finish_trajectory 0 //结束路径0 的建图
rosservice call /write_state "{filename: '${HOME}/Downloads/mymap.pbstream'}" //保存地图

至此，完工，建好的图也被保存在了Home/Download/路径下，名字为mymap.pbstream。