使用二维激光雷达和cartographer_ros实现实时SLAM

在前面已经完成了cartographer_ros的安装和demo的运行了。接下来，就要放到机器人上，实时进行SLAM了。

前一篇内容的链接如下：

Cartographer_ros的下载、配置及编译与问题处理_hxlanu的博客-CSDN博客_ros 编译

---

开始时基本的思路就是修改demo的文件。当时用的就是offline_backpack_2d.launch这个文件。这个能做到，但是要修改的很多，下面就来解决。（更推荐基于demo_revo_lds.launch这个文件进行修改，这个可以看后面问题部分。）

cartographer_ros的官方教程里提供了录制的二维激光雷达和IMU的bag包，如果像替换成其他雷达的数据，还需要做一些修改。

cartographer_ros提供的二维激光雷达数据包发布的数据topic包括/clock, /horizontal_laser_2d, /vertical_laser_2d和/imu。而二维激光雷达发布的激光扫描数据一般为/scan。另外，/horizontal_laser_2d和/vertical_laser_2d两个topic的数据类型都是sensor_msgs::MultiEchoLaserScan，而一般的雷达数据都是sensor_msgs::LaserScan类型，因此如果要使用二维激光雷达，要么进行数据类型转换后转发，要么更改cartographer_ros的设置一边接收sensor_msgs::laserscan类型。

---

数据类型转换法

先看sensor_msgs::MultiEchoLaserScan的数据

# Single scan from a multi-echo planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

Header header            # timestamp in the header is the acquisition time of 
                         # the first ray in the scan.
                         #
                         # in frame frame_id, angles are measured around 
                         # the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis
                         
float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

LaserEcho[] ranges       # range data [m] (Note: NaNs, values < range_min or > range_max should be discarded)
                         # +Inf measurements are out of range
                         # -Inf measurements are too close to determine exact distance.
LaserEcho[] intensities  # intensity data [device-specific units].  If your
                         # device does not provide intensities, please leave
                         # the array empty.

而sensor_msgs::LaserScan的数据则是

# Single scan from a planar laser range-finder
#
# If you have another ranging device with different behavior (e.g. a sonar
# array), please find or create a different message, since applications
# will make fairly laser-specific assumptions about this data

Header header            # timestamp in the header is the acquisition time of 
                         # the first ray in the scan.
                         #
                         # in frame frame_id, angles are measured around 
                         # the positive Z axis (counterclockwise, if Z is up)
                         # with zero angle being forward along the x axis
                         
float32 angle_min        # start angle of the scan [rad]
float32 angle_max        # end angle of the scan [rad]
float32 angle_increment  # angular distance between measurements [rad]

float32 time_increment   # time between measurements [seconds] - if your scanner
                         # is moving, this will be used in interpolating position
                         # of 3d points
float32 scan_time        # time between scans [seconds]

float32 range_min        # minimum range value [m]
float32 range_max        # maximum range value [m]

float32[] ranges         # range data [m] (Note: values < range_min or > range_max should be discarded)
float32[] intensities    # intensity data [device-specific units].  If your
                         # device does not provide intensities, please leave
                         # the array empty.

对比之后可以发现，主要区别就是ranges和intensities这两个变量的数据类型。

因此，只需在转发程序里将ranges和intensities的数据进行转换。具体转换部分如下：

float range;
float inten;
sensor_msgs::LaserEcho echo;
sensor_msgs::LaserEcho intens;

for(int i = 0; i < datalength; i++){ //datalength是激光的数据点数
    range = msg->ranges[i];//msg是激光数据topic的回调函数的消息指针
    inten = msg->intensities;
    echo.echoes.push_back(range);
    muls.ranges.push_back(echo);
    intens.echoes.push_back(inten);
    muls.intensities.push_back(intens);
}

注意，在数据转换的时候，消息的header和其他参数也需要相应的拷贝过去，一般直接拷贝就可以了。如果数据不全，则须按照激光雷达的参数进行设置。

Header 包含seq、stamp、frame_id，其中seq扫描顺序，stamp是时间戳，frame_id是雷达的名字。

其他参数的含义：

angle_min：第一个激光数据的角度

angle_max：最后一个激光数据的角度

angle_increment：相邻两个激光数据的角度差

time_increment： 测量时间间隔

scan_time ：扫描时间间隔

range_min：测距最小值

range_max：测距最大值

最后，转发的topic叫/horizontal_laser_2d

---

参数设置法

参数设置法主要通过在cartographer_ros的lua中进行设置。demo里的offline_backpack_2d.launch文件里加载的backpack_2d.lua中有这样两行代码：

  num_laser_scans = 0,

  num_multi_echo_laser_scans = 1,

这里实际可通过更改这两个参数的数字来设置cartographer_ros接收哪种数据。如果num_laser_scans为1则默认接收sensor_msgs::LaserScan数据的topic；如果num_multi_echo_laser_scans为1则默认接收sensor_msgs::MultiEchoLaserScan数据的topic。

如果采用的通过修改demo_revo_lds.launch和revo_lds.lua文件，则默认是使用sensor_msgs::LaserScan的topic数据类型。

---

遇到的问题

用前面的方法解决数据匹配后，如果直接运行，会发现还会遇到很多问题，接下来将一个一个的解决。

问题一

由于不再需要录制的数据包，所以对launch文件进行修改，删除bag部分，修改后的offline_backpack_2d.launch为：

<launch>
  <arg name="no_rviz" default="false"/>
  <arg name="rviz_config" default="$(find cartographer_ros)/configuration_files/demo_2d.rviz"/>
  <arg name="configuration_directory" default="$(find cartographer_ros)/configuration_files"/>
  <arg name="configuration_basenames" default="backpack_2d.lua"/>
  <arg name="urdf_filenames" default="$(find cartographer_ros)/urdf/backpack_2d.urdf"/>
  <arg name="launch_prefix" default=""/>

  <remap from="echoes" to="horizontal_laser_2d"/>
  <include file="$(find cartographer_ros)/launch/offline_node.launch">
    <arg name="bag_filenames" value="$(arg bag_filenames)"/>
    <arg name="no_rviz" value="$(arg no_rviz)"/>
    <arg name="rviz_config" value="$(arg rviz_config)"/>
    <arg name="configuration_directory" value="$(arg configuration_directory)"/>
    <arg name="configuration_basenames" value="$(arg configuration_basenames)"/>
    <arg name="urdf_filenames" value="$(arg urdf_filenames)"/>
    <arg name="launch_prefix" value="$(arg launch_prefix)"/>
  </include>
</launch>

注意，下面的这个语句

  <remap from="echoes" to="horizontal_laser_2d"/>

如果仍然采用sensor_msgs::MultiEchoLaserScan这个数据类型的话，保持不变；如果改用sensor_msgs::LaserScan，则需要把echoes改为scan。

问题二

如果通过前面的方法直接用demo中的launch文件进行运行的话，就会遇到缺少/imu这个topic的问题，解决方法就是修改lua文件。打开launch文件中对应的lua文件，找到num_subdivisions_per_laser_scan这个参数，并做如下修改：

首先找到

MAP_BUILDER.use_trajectory_builder_2d = true

删除后面的

TRAJECTORY_BUILDER_2D.num_accumulated_range_data = 10

然后参考revo_lds.lua，添加如下部分：

TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
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

修改后即可解决上述问题。

问题三

如果通过前面的方法直接用demo中的launch文件进行运行的话，就会遇到一下问题：

Ignored subdivision of a LaserScan message from sensor scan because previous subdivision time xxxxxxxxx is not before current subdivision time xxxxxxxxx

解决这个问题的办法还是修改lua文件。打开launch文件中对应的lua文件，找到num_subdivisions_per_laser_scan这个参数，并做如下修改：

num_subdivisions_per_laser_scan = 1,

同时，在launch文件中，还需要添加如下语句：

 <param name="use_sim_time" value="false" />

通过以上修改就可以解决前面的问题了。

问题四

如果运行过程中出现以下提示：

range_data_collator.cc:82] Dropped 1 earlier points.

这是由于激光的数据问题。可以通过在转发的时候给header.stamp赋予现在的时间即可。

sensor_msgs::LaserScan ls.header.stamp = ros::Time::now();

问题五

如果需要对sensor_msgs::laserscan进行转发，可能遇到一下问题：

segmentation fault

这个问题发生在

ls.ranges[i] = msg->ranges[i];

这是因为ls这个变量定义后，它的成员ranges需要确定大小。所以解决方法就是在给它赋值前进行初始化，初始化方法如下：

ls.ranges.resize(num);

ls.intensities.resize(num);

这里num是激光雷达的数据长度。

至此，可以应该就可以运行cartographer_ros。

---

 我主要是通过修改backpack_2d.launch文件和backpack_2d.lua文件实现。为了实现直接采用激光雷达数据，则需删除最后的rosbag的部分。修改后的launch文件如下：

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

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename revo_lds.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" />

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

修改后的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.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 12.
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

接下来就是运行了cartographer_ros了！

当然，lua里的参数还需要修改才能实现较好的效果，可以参考一下两个网址：

Cartographer_ros解读 - ColinQin - 博客园 (cnblogs.com)

Algorithm walkthrough for tuning — Cartographer ROS documentation (google-cartographer-ros.readthedocs.io)

Tuning methodology — Cartographer ROS documentation (google-cartographer-ros.readthedocs.io)

Configuration — Cartographer documentation (google-cartographer.readthedocs.io)

基于激光雷达定位和DWA的小型无人机穿越窄门演示