Cartographer实时建图（个人定制版）

1. Carto实时建图方案

1. 首先启动雷达节点，获得scan
2. 然后启动机器人节点，获得odom和tf
3. 启动键盘控制节点
4. 启动cartographer建图节点
5. 结束建图，关闭传感器数据接收
6. 保存为后缀名为.pbstream
7. 将pbstream转化成pgm文件

在先前已经完成了Cartographer离线建图，离线建图方案是，先在底盘主机中录制bag包，包括/scan、/odom、/tf话题，然后将bag包拷贝到从机中，也就是自己的电脑，然后用自己的电脑启动cartographer建图节点，建完图后将得到的pgm、pbstream等文件再拷贝到底盘主机中。这样就完成了离线建图任务，参考链接如下：

Cartographer + Navigation 建图导航（个人定制版）

下面开始梳理Cartographer的实时建图问题。

2. 回顾离线建图命令

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=～/floor_12.bag

rosservice call /finish_trajectory 0

rosservice call /write_state "{filename: '${HOME}/Downloads/floor_12.pbstream'}"

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=/home/lyp/Downloads/floor_22 -pbstream_filename=/home/lyp/Downloads/floor_22.pbstream -resolution=0.05

注意bag使用绝对路径。

3. 修改相关配置文件

3.1 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" />
  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />
</launch>

修改：
1. 删除掉rviz。原因是底盘中没有接显示器，不能启动rviz界面。对于如何看到rviz界面，可以设置主从机单独打开rviz。
2. 删除掉了bag播放节点。原因是实时建图，直接接入机器人控制节点、激光雷达节点（/odom、/scan、/tf），不需要再播放bag。

3.2 demo_backpack_2d.launch（实时）

<launch>
  <param name="/use_sim_time" value="false" />
  <include file="$(find cartographer_ros)/launch/backpack_2d.launch" />

只需要关注launch/backpack_2d.launch文件。

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="scan" to="scan" />
  </node>

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

只需要关注backpack_2d.urdf和backpack_2d.lua文件

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="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" />

  <joint name="laser_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="laser" />
    <origin rpy="0 0 0" xyz="0.14 0 0.03" />
  </joint>
</robot>

不用修改，但是需要注意底盘主机中的cartographer参数是否正确。

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 = "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.num_accumulated_range_data = 4

return options

不用修改，需要注意底盘主机中cartographer/configuration_files/backpack_2d.lua参数是否正确。

4. 实时建图流程（个人定制）

（1）底盘启动机器人底盘节点，获取/odom和/tf话题。注意/cmd_vel需要remap成/smooth_cmd_vel话题，同时也需要启动速度平滑节点，修改速度平滑节点的launch文件。<remap from="/cmd_vel" to="/smooth_cmd_vel" />

roslaunch base_controller lp_robot.launch

（2）底盘启动激光雷达节点，获取/scan话题

roslaunch pavo_ros pavo_scan.launch

（3）底盘启动键盘控制节点，调节到合适的速度值

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

（4）底盘启动速度平滑节点，需要修改launch文件的订阅的话题。

roslaunch yocs_velocity_smoother standalone.launch

（5）自己的电脑从机上打开rviz，选择配置好的rviz，cartographer_ros/configuration_files/demo_2d.rviz

rviz

（6）底盘启动建图节点

roslaunch cartographer_ros demo_backpack_2d.launch

（7）机器人运动一段距离，跑遍室内的环境后，结束建图的数据接收，底盘终端运行命令

rosservice call /finish_trajectory 0

（8）保存成pbstream文件，底盘终端运行命令

rosservice call /write_state "{filename: '${HOME}/Downloads/floor_24.pbstream'}"

（9）将pbstream文件转化成pgm文件，底盘终端运行，完成建图

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=/home/lanpu/Downloads/floor_24 -pbstream_filename=/home/lanpu/Downloads/floor_24.pbstream -resolution=0.05

本文中列出一些的配置文件很多都不用修改，只需要将demo_backpack_2d.launch文件中的bag播放节点删掉，注释掉rviz启动节点。本文中列出的配置文件目的是要逐个检查一遍，防止出现实时建图与离线建图参数配置不一致，因为离线建图是在从机中，实时建图是在主机中。

5. 注意事项

1. 如果出现建图漂移特别严重、不能生成pgm文件等问题，极有可能是参数文件没有配置正确。（我的就出现了离线建图很好，但是实时建图总是漂移严重，后来将离线建图 的相关配置文件拷贝到底盘中再实时建图就可以了。这就说明之前的参数不对）
2. 修改carto_ws/src/文件夹下的cartographer和cartographer_ros文件夹下的配置文件，需要再重新编译。编译过程中会将配置文件拷贝到编译文件夹install_isolated中。修改cartographer的lua、launch、urdf等都需要重新编译。
3. 编译命令，在/carto_ws文件夹下运行catkin_make_isolated --install --use-ninja