Cartographer建图和纯定位

（一）cartographer在ros下安装参考：

要求：

64-bit, modern CPU (e.g. 3rd generation i7)
16 GB RAM
Ubuntu 18.04 (Bionic), 20.04 (Focal)
gcc version 6.3.0, 7.5.0, 9.3.0

参考安装指南：https://google-cartographer-ros.readthedocs.io/en/latest/compilation.html

1.下载两个文件：

cartographer
cartographer_ros

2.建立工作空间

建立cartographer_ws/src，并把以上两个文件放入到src下面：

3.安装和编译ninja

sudo apt-get update
sudo apt-get install -y python-wstool python-rosdep ninja-build stow

4.安装依赖

sudo rosdep init
rosdep update
rosdep install --from-paths src --ignore-src --rosdistro=${ROS_DISTRO} -y

如果失败，参照：Ubuntu下多版本sophus、opencv使用: 3. 鱼香ROS一键安装程序包依赖

wget http://fishros.com/install -O fishros && . fishros
rosdepc install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO

5.安装 abseil-cpp

src/cartographer/scripts/install_abseil.sh

同时卸载方式为：

sudo apt-get remove ros-${ROS_DISTRO}-abseil-cpp

6.编译

catkin_make_isolated --install --use-ninja

7.官方数据集下载链接

2D数据集
 3D数据集

8.官方demo

建图：
2D

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=${HOME}/Downloads/cartographer_paper_deutsches_museum.bag

3D：

roslaunch cartographer_ros demo_backpack_3d.launch bag_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag

纯定位
下载2D数据集：

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-05-14-44-52.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_2d/b2-2016-04-27-12-31-41.bag

一个用来定位一个用来建立地图，

roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag
roslaunch cartographer_ros demo_backpack_2d_localization.launch \
   load_state_filename:=${HOME}/Downloads/b2-2016-04-05-14-44-52.bag.pbstream \
   bag_filename:=${HOME}/Downloads/b2-2016-04-27-12-31-41.bag

下载3D数据集，操作相同：

wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-13-54-42.bag
wget -P ~/Downloads https://storage.googleapis.com/cartographer-public-data/bags/backpack_3d/b3-2016-04-05-15-52-20.bag

roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag
roslaunch cartographer_ros demo_backpack_3d_localization.launch \
   load_state_filename:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag.pbstream \
   bag_filename:=${HOME}/Downloads/b3-2016-04-05-15-52-20.bag

（二）2d cartographer 解决cartographer纯定位问题

1.使用话题的类型是pointcloud2，

backpack_2d.launch:

2.lua文件的修改

backpack_2d.lua:
修改以下几句：

  num_laser_scans = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1,

添加几句：

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

参照：https://www.codeleading.com/article/77834618087/
完整的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 = 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.,
}

MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 1
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true

return options

3.urdf的修改：

backpack_2d.urdf: 为激光雷达的frame_id，imu暂且不管

  <joint name="velodyne_joint" type="fixed">   与base_link链接情况，暂且理解base_link为车体坐标系
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0.1007 0 0.0558" />
  </joint>

完整的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="velodyne">
    <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" />
  </joint>

  <joint name="velodyne_joint" type="fixed">
    <parent link="base_link" />
    <child link="velodyne" />
    <origin xyz="0.1007 0 0.0558" />
  </joint>
  
</robot>

4.修改完成之后启动建图节点：

roslaunch cartographer_ros demo_backpack_2d.launch bag_filename:=/home/xxx/a.bag

5.保存地图：

完成轨迹, 不接受进一步的数据：
新开终端：
注意要进行source，不然无法启用rosservice

source  ~/cartographer/devel_isolated/setup.bash
rosservice call /finish_trajectory 0

序列化保存其当前状态

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

将pbstream转换为pgm和yaml

rosrun cartographer_ros cartographer_pbstream_to_ros_map -map_filestem=${
    
    HOME}/Downloads/mymap -pbstream_filename=${
    
    HOME}/Downloads/mymap.pbstream -resolution=0.05

或者

roslaunch cartographer_ros assets_writer_ros_map.launch bag_filenames:=/home/xxxxx/a.bag pose_graph_filename:=/home/xxxx/a.bag.pbstream -resolution=0.05

6.纯定位功能（bag已经解决任意点启动功能，有待实车实验）

demo_backpack_2d_localization.launch：
添加：

<remap from="points2" to="/velodyne_points" />

注释：

  <!-- <node name="playbag" pkg="rosbag" type="play"
</launch> -->改由手动rosbag play xx.bag -s 50

7.启动定位节点（已经完成由传感器直接输入）：

roslaunch cartographer_ros demo_backpack_2d_localization.launch load_state_filename:=/home/xxxx/a.bag.pbstream

启动bag包：

rosbag play xx.bag -s 50

-s 50表示在bag第50s的时候启动

8.cartographer编译

catkin_make_isolated --install --use-ninja

9.纯定位，取消更新地图部分

occupancy_grid_node_main.cc中的 occupancy_grid_publisher_.publish(*msg_ptr);注释掉

10.加入IMU的数据，需要修改的部分

backpack_2d.launch和demo_backpack_2d_localization.launch添加： <remap from="imu" to="/pa_gs02/imu/data" />
backpack_2d.lua：修改为TRAJECTORY_BUILDER_2D.use_imu_data = true