cartographer之运行自己的bag包

 要注意的是：在carto src目录下改变文件之后，不仅仅是代码，配置文件及launch文件之后，需要重新编译一下，系统会将改变后的文件安装到指定目录，才会生效。

参考carto的3D 的demo，从launch文件看起：

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

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

可以看到在启动该launch文件后，还会再启动carto的launch文件夹中 $(findcartographer_ros)/launch/backpack_3d.launch，那么我们接下来看该launch又开启了什么

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_3d.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_3d.lua"
      output="screen">
    <remap from="points2_1" to="horizontal_laser_3d" />
    <remap from="points2_2" to="vertical_laser_3d" />
    <!--remap from="points2" to="vertical_laser_3d" /-->
    <remap from="imu" to="imu" />
  </node>

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

由此，明了启动顺序与文件。

1.先是启动/urdf/backpack_3d.urdf，它是机器人描述文件，结合ros 下robot_state_publisher节点，能够发布出机器人关节的静态TF，因此我们知道，这个文件结合robot_state_publisher是提供静态TF,那么针对不同机器人，不同IMU、Lidar的布置形式，其对应的TF是不同的。

因此，如果需要跑自己的数据包，就需要搞清楚自己数据包的机器的布置。

我照着backpack_3d.urdf写了一个自己的urdf文件，my_fang_3d.urdf

<robot name="cartographer_TYWL_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" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>

  <link name="velodyne">
    <visual>
      <origin xyz="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" />
    <origin xyz="0.5 -0.1 0.6" rpy="0 0 0" />
  </joint>

  <joint name="velodyne_link_joint" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0.5 0.0 0.7" rpy="0. 0 0" />
  </joint>
</robot>

 添加自己的IMU，名称需要和录制的包imu话题的frame_id一致，base_link不需要改；主要还要改的就是imu_link_joint的参数，指代的实际传感器与base的静态TF。

2.通过看原demo可以发现，

它加载了/configuration_files -configuration_basename backpack_3d.lua"，因此我自己写了一个新的.lua文件。robot_3d.lua：

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "imu", --"base_link"
  published_frame = "base_link",  --"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,  --这里选择是否用GPS,
  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 = 160

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

3. 根据自己数据包设置ros remap映射，新建一个demo_robot_3D.launch

<launch>

  <!--加载小车模型-->
  <!-- <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/minibus.urdf" /> -->
    <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_fang_3d.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 robot_3d.lua" 
      output="screen">
    <!--这里是你自己3d点云雷达的话题-->
    <!-- <remap from="points2" to="/minibus/front/lslidar_point_cloud" />  -->
    <!--<remap from="points2_2" to="vertical_laser_3d" />-->
    <remap from="/imu" to="/imu/data" />
      <remap from="/points2" to="/velodyne_points" />
  </node>

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

4.新增一个最上层的launch文件，demo_fang_3d.launch

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

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

以上步骤，讲述了新增自己数据包的launch文件，若按照方式设置完，即可进行建图，效果如下显示： 

---

然后就是保存地图的环节，与上一个博客所示

(1) 结束轨迹：

rosservice call /finish_trajectory 0

（2）生成pbstream文件

rosservice call /write_state "{filename: '/home/fang/cartographer_ws/src/bags/2021-10-01-11-14-31.bag.pbstream', include_unfinished_submaps: 1}"

 （3）生成地图

roslaunch cartographer_ros assets_writer_backpack_3d.launch bag_filenames:='/home/fang/cartographer_ws/src/bags/2021-10-01-11-14-31.bag' pose_graph_filename:=/home/fang/cartographer_ws/src/bags/2021-10-01-11-14-31.bag.pbstream'

此处调用了assets_writer_backpack_3d.launch文件，然后发现报错了，进入该文件看，可以发现

<launch>
  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename assets_writer_backpack_3d.lua
          -urdf_filename $(find cartographer_ros)/urdf/backpack_3d.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

文件调用了carto默认的backpack_3d.urdf，因而新增一个assets_writer_fang_3d.launch

<launch>
  <node name="cartographer_assets_writer" pkg="cartographer_ros" required="true"
      type="cartographer_assets_writer" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename assets_writer_backpack_3d.lua
          -urdf_filename $(find cartographer_ros)/urdf/my_fang_3d.urdf
          -bag_filenames $(arg bag_filenames)
          -pose_graph_filename $(arg pose_graph_filename)"
      output="screen">
  </node>
</launch>

这一次就能够正常生成地图