配置Cartographer_ros进行2D激光雷达的realtime_slam!

slam优化方法简介

2D激光slam主要以优化方式分类，可以分为两种，一种是滤波方法，另一种是图优化方法。滤波方法是结合上一时刻的姿态信息和当前时刻的传感器测得的姿态信息进行匹配，通过卡尔曼滤波、扩展卡尔曼滤波、粒子滤波等算法，把传感器的测量误差降低，以期达到更好的测量状态。滤波这类方法在建图范围小，移动机器人运行里程少时，测量的误差会比较小，由于每次累计误差的存在，在大范围下、长距离下，误差仍会很大，而且计算的内存消耗会很大。
另一种优化方式是图优化（Graph）,图优化的思想是将机器人在各个时态的位姿抽象成点、观测产生的约束抽象成边，在运动过程中将这些位姿和约束关系记录下来，之后调整机器人的位姿和约束关系，利用最小二乘法求得在约束下误差最小位姿。

cartographer_ros介绍

cartographer是来自谷歌2016年的开源的ros系统下的激光slam算法。cartographer主要采用了CSM（Correlation Scan Matcher）和梯度优化方法融合的帧间匹配方法、采用Scan-to-Map的回环检测方法。该算法来自谷歌的大牛们，源码中大量采用C++11的编码方式，读起来很费劲，但绝对简洁。算法目前官方也一直在维护，是目前开源激光SLAM算法中效果非常好的一种，值得学习。

配置cartographer_ros文件进行realtime_slam

cartographer_ros的安装可以参见官方git.
realtime-slam是指在线建图，不是跑数据包（xxx.bag）。

启用激光雷达

我采用的是rplidar，启用端口是"/dev/ttyUSB002"(可以在teminal中输入命令lsusb查看具体端口)。
我的rplidar的launch文件如下，各个激光雷达厂家都有自己的雷达包，大家可以根据各自的包配置下自己的launch文件。

<launch>
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  <param name="serial_port"         type="string" value="/dev/ttyUSB002"/>
  <param name="serial_baudrate"     type="int"    value="115200"/>
  <param name="frame_id"            type="string" value="laser"/>
  <param name="inverted"            type="bool"   value="false"/>
  <param name="angle_compensate"    type="bool"   value="true"/>
  </node>
</launch>

cartographer中launch的配置

该launch：
1.包含了启动cartographer节点，包含了启动节点所需的lua配置文件,我用的是xiaoqiang机器人，配置文件为xiaoqiang_rplidar_2d.lua，具体lua编写之后介绍；
2.包含了建立栅格地图cartograoher_occupancy_grid_node节点所需的信息；
3.包含了建图可视化的rviz信息

<launch>
<include file="$(find cartographer_ros)/launch/rplidar_2d.launch" />
<node name="cartographer_node" pkg="cartographer_ros"
    type="cartographer_node" args="
        -configuration_directory
            $(find cartographer_ros)/configuration_files
        -configuration_basename xiaoqiang_rplidar_2d.lua"
    output="screen">
    <remap from="/odom" to="/xqserial_server/Odom" />
    <!--remap from="/laserbase_footprint" to="/base_footptint" />
    <remap from="/laser_baselink" to="/base_link" /-->
</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/rplidar_2d.rviz" />
</launch>

上述代码的rplidar_2d.launch，该launch中包含urdf文件对移动机器人的描述，具体为：

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

lua文件配置

include "map_builder.lua"
include "trajectory_builder.lua"

options = {
  map_builder = MAP_BUILDER,
  trajectory_builder = TRAJECTORY_BUILDER,
  map_frame = "map",
  tracking_frame = "laser",
  published_frame = "odom",
  odom_frame = "odom",
  provide_odom_frame = true,
  publish_frame_projected_to_2d = false,
  use_odometry = true,
  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 = 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.min_range = 0.15
TRAJECTORY_BUILDER_2D.max_range = 16.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1)

POSE_GRAPH.constraint_builder.min_score = 0.65
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7

return options

include的两个lua文件为官方包里就有的,map_builder主要是建立全局栅格地图，trajectory主要是建立轨迹优化的接口，导引到对应的2D和#3D建图模块中去。

lua的配置主要是对options的选项进行配置：

map_frame:tf中全局地图的tf名称，一般都为map；
tracking_frame: SLAM算法中追寻的坐标系的ID。
provide_odom_frame、published_frame的选择可以从cartographer源码中判断：

if (trajectory_state.trajectory_options.provide_odom_frame) {
        std::vector<geometry_msgs::TransformStamped> stamped_transforms;

        stamped_transform.header.frame_id = node_options_.map_frame;
        stamped_transform.child_frame_id =
            trajectory_state.trajectory_options.odom_frame;
        stamped_transform.transform =
            ToGeometryMsgTransform(trajectory_state.local_to_map);
        stamped_transforms.push_back(stamped_transform);

        stamped_transform.header.frame_id =
            trajectory_state.trajectory_options.odom_frame;
        stamped_transform.child_frame_id =
            trajectory_state.trajectory_options.published_frame;
        stamped_transform.transform = ToGeometryMsgTransform(
            tracking_to_local * (*trajectory_state.published_to_tracking));
        stamped_transforms.push_back(stamped_transform);

        tf_broadcaster_.sendTransform(stamped_transforms);
      } else {
        stamped_transform.header.frame_id = node_options_.map_frame;
        stamped_transform.child_frame_id =
            trajectory_state.trajectory_options.published_frame;
        stamped_transform.transform = ToGeometryMsgTransform(
            tracking_to_map * (*trajectory_state.published_to_tracking));
        tf_broadcaster_.sendTransform(stamped_transform);
      }

简单点就是：
如果provide_odom_frame=true,那么tf转换为：
map->odom->publshed_frame
如果provide_odom_frame=false，那么tf转换为：
map->published_frame。
TRAJECTORY_BUILDER_2D的配置是轨迹优化中对应激光雷达的参数的配置，POSE_GRAPH是进行全局图优化的配置。
大家可以依据自己的雷达类型进行配置吧，不详述了，具体可见：创客智造或者Google官方的解释。

效果展示

按着做下来，启动相应launch，就可以喝着咖啡、欣赏自己的建图成果了。
自己建的图之后补，在小车上，给大家展示下跑官方museum.bag的效果图吧。