cartographer使用笔记

一、Cartographer-ros的安装：

1.1 安装Cartographer

   参考官方文档安装即可。
   https://google-cartographer.readthedocs.io/en/latest/

1.2 安装cartographer-ros

   同样参考官方文档安装即可。
   https://google-cartographer-ros.readthedocs.io/en/latest/your_bag.html

   本人实际安装与官方有些许不同，按如下操作步骤：

1. 新建ros工作空间并初始化：

   mkdir carto_ws && cd carto_ws
   mkdir src && cd src
   catkin_init_workspace
   

2. 下载cartograher_ros：

   git clone https://github.com/cartographer-project/cartographer_ros.git
   

3. 编译：

   	catkin_make
   

二、使用官方数据集跑cartographer

   详情参考官方手册：
https://google-cartographer-ros.readthedocs.io/en/latest/demos.html

三、使用VLP-16跑cartographer

3.1 二维定位与建图：

一、准备文件：
   在cartographer_ros/configuration_files中，创建my_mapping_2d.lua
   在cartographer_ros/launch中，创建my_mapping_2d.launch
二、设定参数：
   修改my_mapping_2d.lua，如下所示。
   具体参数设置见官方文档：Lua configuration reference documentation

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 = false,
  publish_frame_projected_to_2d = false,
  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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 20.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
 
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
 
return options

三、修改launch文件：
   my_mapping_2d.launch如下所示：

<launch>

  <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_mapping_2d.lua"
      output="screen">
      
    <!-- 修改scan为自己雷达发布的话题 -->
    <remap from="echoes" to="scan" />	
    
    <!-- 修改imu为自己imu发布的话题，如果没有或者不使用imu，则注释掉下行 -->
    <remap from="imu" to="imu/data_own" />
  </node>

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

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

四、运行程序：

roslaunch cartographer_ros my_mapping_2d.launch

3.2 三维定位与建图：

一、准备文件：
   在cartographer_ros/configuration_files中，创建my_mapping_3d.lua
   在cartographer_ros/launch中，创建my_mapping_3d.launch
   在cartographer_ros/urdf中，创建my_car.urdf.xacro
二、设定参数：
   修改my_mapping_3d.lua，如下所示。
   具体参数设置见官方文档：Lua configuration reference documentation

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 = 0,
  num_multi_echo_laser_scans = 0,
  num_subdivisions_per_laser_scan = 1,
  num_point_clouds = 1, -- 这里设置成自己的雷达的数量，一般为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 -- 积累多少帧发布一次数据， 设置为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 = 100	-- 每次优化间隔的node数
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文件：
   my_mapping_3d.launch如下所示：

<launch>
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_car.urdf.xacro" />

  <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_mapping_3d.lua"
      output="screen">
    <remap from="points2" to="/velodyne_points" />
    <remap from="/imu" to="/imu/data_own" />

  </node>

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

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

三、创建自己的机器人模型：

<?xml version="1.0" ?>
<robot name="vdcar" xmlns:xacro="http://ros.org/wiki/xacro">
  <xacro:include filename="$(find vdcar)/urdf/common_properties.xacro"/>
  <!--xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_waffle2d.gazebo.xacro"/-->

  <link name="base_footprint"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_link" />
    <origin xyz="0 0 0.12065" rpy="0 0 0"/>
  </joint>

  <link name="base_link" />
  
<!--sure-->
  <joint name="imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin xyz="0.0583 -0.0434 0.2123" rpy="0 0 0"/>
    <axis xyz="0 0 1" />
  </joint>

  <link name="imu_link"/>
  
<!--sure-->  
  <joint name="velodyne_joint" type="fixed">
    <parent link="base_link"/>
    <child link="velodyne"/>
    <origin xyz="0.355 0 0.9188017" rpy="0 0 0"/>
    <axis xyz="0 0 1" />
  </joint>

  <link name="velodyne"/>
  
</robot>

五、运行程序：

roslaunch cartographer_ros my_mapping_3d.launch