(02)Cartographer源码无死角解析-(09) gflags与glog简介、及其main函数讲解

本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

下面我们就正式开始讲解代码了，那么不用多说，首先我们是从 main 函数开始分析，那么问题来了，如何找到 main 函数呢？根据src/cartographer_ros/cartographer_ros/launch/lx_rs16_2d_outdoor.launch 文件，可以看到其有 node，名字name分别为：cartographer_node，cartographer_occupancy_grid_node 他们分别做什么的，先不去理会，就假设现在我们不知道。src/cartographer_ros/cartographer_ros/cartographer_ros/CMakeLists.txt 文件中搜索 cartographer_node、cartographer_occupancy_grid_node 可以看到看到如下内容：

# 生成建图节点
google_binary(cartographer_node
  SRCS
    node_main.cc
)
google_binary(cartographer_occupancy_grid_node
  SRCS
    occupancy_grid_node_main.cc
)
1
2
3
4
5
6
7
8
9

其上的 google_binary 是 google 自己封装好的cmake(cmake是一门编程语言，可以定义函数)函数，其会把 node_main.cc 生成二进制可执行文件 cartographer_node，occupancy_grid_node_main.cc 生成二进制可执行文件 cartographer_occupancy_grid_node。对于 .launch 文件中的节点 rviz 与 playbag 后续过程中进行讲解。

node_main.cc 文件位于 src/cartographer_ros/cartographer_ros/cartographer_ros/ 文件夹下，那么下面我们就开始分析吧。
 

二、gflags与glog

1、gflags

首先在 node_main.cc 文件中，可以看到如下几个宏：

DEFINE_bool 、 DEFINE_string、
1

其都是来自于 gflags 中，其是谷歌命令行解析工具。主要用于解析用命令行执行可执行文件时传入的参数。与getops()不同的是，在gflags中flag可以分散的定义在各个文件之中，而不用定义在一起，这就意味着在我们可以在一个单独的文件中只定义这个文件所需要用到的一些flag，链接了该文件应用都可以使用该文件中的flag，这样就能非常方便的实现代码的复用，如果不同的文件定义了相同的flag，则会产生错误，所以需要明确规范gflags的使用规范。

/**
 * note: gflags是一套命令行参数解析工具
 * DEFINE_bool在gflags.h中定义
 * gflags主要支持的参数类型包括bool, int32, int64, uint64, double, string等
 * 定义参数通过DEFINE_type宏实现, 该宏的三个参数含义分别为命令行参数名, 参数默认值, 以及参数的帮助信息
 * 当参数被定义后, 通过FLAGS_name就可访问到对应的参数
 */
1
2
3
4
5
6
7

如下源码中的示例：

DEFINE_bool(collect_metrics, false,
            "Activates the collection of runtime metrics. If activated, the "
            "metrics can be accessed via a ROS service.");
1
2
3

DEFINE_bool 表示闯入的参数为 bool类型， collect_metrics 表示命令行需要传入的参数，false 代表默认值，后的剩下的字符串是备注, 在 .launch 中的的代码有如下：

      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename lx_rs16_2d_outdoor.lua"
          
    <remap from="points2" to="rslidar_points" />
    <remap from="scan" to="front_scan" />
    <remap from="odom" to="odom_scout" />
    <remap from="imu" to="imu" />
1
2
3
4
5
6
7
8

表示再在执行 cartographer_node 程序的时候，其传入的两个参数，分别为 -configuration_directory，-configuration_basename，其与 node_main.cc 文文件中的 configuration_directory，configuration_basename 是一一对应的。另外，runlaunch 指令还会把 remap(重映射)操作也当作参数传入到 main 函数的 **argv 中。如下：

传送进来的参数会经过

  // note: 初始化glog库
  google::InitGoogleLogging(argv[0]);
   
  // 使用gflags进行参数的初始化. 其中第三个参数为remove_flag
  // 如果为true, gflags会移除parse过的参数, 否则gflags就会保留这些参数, 但可能会对参数顺序进行调整.
  google::ParseCommandLineFlags(&argc, &argv, true);
1
2
3
4
5
6

两个函数的处理，处理之后变换成如下：

先来看看 DEFINE_xxx 的宏定义变量：

$(1)$ collect_metrics→默认为false，激活运行时度量的集合.如果激活, 可以通过ROS服务访问度量。.launch文件中没有配置，所以其使用的是默认值。
$(2)$ configuration_directory→在.launch中被被配置为 $(find cartographer_ros)/configuration_files。
$(3)$ configuration_basename→在.launch中被被配置为 lx_rs16_2d_outdoor.lua
$(4)$ load_frozen_state→默认为true，.launch中未配置，使用默认值。将保存的状态加载为冻结（非优化）轨迹。
$(5)$ start_trajectory_with_default_topics→使用默认主题(话题)开始第一个轨迹
$(6)$ save_state_filename→保存轨迹文件的路径

如果想引用这些参数，在参数的前面加入FLAGS_即可，比如引用上面的 cartographer_node，书写成 FLAGS_configuration_directory 即可。其变量本人打印如下：

FLAGS_configuration_directory→"/my_work/ROS/work/cartographer_detailed_comments_ws/install_isolated/share/cartographer_ros/configuration_files"
FLAGS_configuration_basename="lx_rs16_2d_outdoor.lua"
1
2

 

2、glog

Google glog 是一个应用级别的日志系统库.它提供基于 C++风格的流和各种辅助宏的日志 API.支持以下功能：

01.参数设置, 以命令行参数的方式设置标志参数来控制日志记录行为
02.严重性分级, 根据日志严重性分级记录日志 - INFO WARNING ERROR FATAL
03.可有条件地记录日志信息 - LOG_IF LOG_EVERY_N LOG_IF_EVERY_N LOG_FIRST_N
04.条件中止程序。丰富的条件判定宏, 可预设程序终止条件 - CHECK 宏
05.异常信号处理。程序异常情况, 可自定义异常处理过程
06.支持 debug 功能
07.自定义日志信息
08.线程安全日志记录方式
09.系统级日志记录
10.google perror 风格日志信息
11.精简日志字符串信息
1
2
3
4
5
6
7
8
9
10
11

其最终的结果不仅会在屏幕终端显示出来, 同时会将 log 日志写入到/tmp/…log…这个文件中，另外在 glog 中有如下宏定义，用于两个值的比较：

#define CHECK_EQ(val1, val2) CHECK_OP(_EQ, ==, val1, val2) //检查val1、val2是否相等
#define CHECK_NE(val1, val2) CHECK_OP(_NE, !=, val1, val2) //检查val1、val2是否不相等
#define CHECK_LE(val1, val2) CHECK_OP(_LE, <=, val1, val2) //检查val1是否小于等于val2
#define CHECK_LT(val1, val2) CHECK_OP(_LT, < , val1, val2) //.......
#define CHECK_GE(val1, val2) CHECK_OP(_GE, >=, val1, val2) //.......
#define CHECK_GT(val1, val2) CHECK_OP(_GT, > , val1, val2) //.......
1
2
3
4
5
6

这些语句能够很方便的把信息输入到日志中，如源码中的：

  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";
1
2
3
4

如果输入参数 configuration_directory 或 FLAGS_configuration_basename 为空，则会记录错误信息，并且终止程序。那么我们应该如何使用 glog，自定义一些我们想要的信息呢？源码中可以看到如下代码(位于node_main.cc)：

  // 使用ROS_INFO进行glog消息的输出
  cartographer_ros::ScopedRosLogSink ros_log_sink;
1
2

其中的 ScopedRosLogSink 的定义与实现如下(位于install_isolated/include/cartographer_ros/ros_log_sink.h)：

// Makes Google logging use ROS logging for output while an instance of this
// class exists.
/**
 * @brief 自定义的输出日志的方式: 使用ROS_INFO进行glog消息的输出
 */
class ScopedRosLogSink : public ::google::LogSink {
 public:
  ScopedRosLogSink();
  ~ScopedRosLogSink() override;

  void send(::google::LogSeverity severity, const char* filename,
            const char* base_filename, int line, const struct std::tm* tm_time,
            const char* message, size_t message_len) override;

  void WaitTillSent() override;

 private:
  bool will_die_;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

首先其继承了 google::LogSink，只需要实现 send() 、WaitTillSent()、WaitTillSent() 三个函数即可，在src/cartographer_ros/cartographer_ros/cartographer_ros/ros_log_sink.cc 中实现，如下：

/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "cartographer_ros/ros_log_sink.h"

#include 
#include 
#include 
#include 

#include "glog/log_severity.h"
#include "ros/console.h"

namespace cartographer_ros {

namespace {

/**
 * @brief 根据给定的文件全路径名, 获取文件名
 * 
 * @param[in] filepath 
 * @return const char* 返回文件名
 */
const char* GetBasename(const char* filepath) {
  // 找到 '/' 最后一次在filepath中出现的位置
  const char* base = std::strrchr(filepath, '/');
  // 找到'/',就将'/'之后的字符串返回；找不到'/', 就将整个filepath返回
  return base ? (base + 1) : filepath;
}

}  // namespace


/**
 * @brief 在构造函数中调用AddLogSink(), 将ScopedRosLogSink类注册到glog中
 */
ScopedRosLogSink::ScopedRosLogSink() : will_die_(false) { AddLogSink(this); }
ScopedRosLogSink::~ScopedRosLogSink() { RemoveLogSink(this); }

/**
 * @brief 重载了send()方法, 使用ROS_INFO进行glog消息的输出
 * 
 * @param[in] severity 消息级别
 * @param[in] filename 全路径文件名
 * @param[in] base_filename 文件名
 * @param[in] line 消息所在的文件行数
 * @param[in] tm_time 消息的时间
 * @param[in] message 消息数据本体
 * @param[in] message_len 消息长度
 */
void ScopedRosLogSink::send(const ::google::LogSeverity severity,
                            const char* const filename,
                            const char* const base_filename, 
                            const int line,
                            const struct std::tm* const tm_time,
                            const char* const message,
                            const size_t message_len) {
  const std::string message_string = ::google::LogSink::ToString(
      severity, GetBasename(filename), line, tm_time, message, message_len);
  switch (severity) {
    case ::google::GLOG_INFO:
      ROS_INFO_STREAM(message_string);
      break;

    case ::google::GLOG_WARNING:
      ROS_WARN_STREAM(message_string);
      break;

    case ::google::GLOG_ERROR:
      ROS_ERROR_STREAM(message_string);
      break;

    case ::google::GLOG_FATAL:
      ROS_FATAL_STREAM(message_string);
      will_die_ = true;
      break;
  }
}

// WaitTillSent()会在每次send后调用, 用于一些异步写的场景
void ScopedRosLogSink::WaitTillSent() {
  if (will_die_) {
    // Give ROS some time to actually publish our message.
    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  }
}

}  // namespace cartographer_ros
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101

上面你的代码逻辑也比较建档，核心的语句为：

  const std::string message_string = ::google::LogSink::ToString(
      severity, GetBasename(filename), line, tm_time, message, message_len);
      
  ROS_xxxx_STREAM(message_string);
1
2
3
4

::google::LogSink::ToString 函数会把输入的参数如 severity，line，message 等汇聚成一条字符串信息，然后根据信息等级通过 ROS_xxxx_STREAM 输出。这样输出，最终就保持了ROS 格式，如下就是一条打印信息：

[ INFO] [1667016236.198881881, 1606808687.533274071]: I1029 12:03:56.000000 24790 collated_trajectory_builder.cc:115] points2 rate: 19.29 Hz 5.18e-02 s +/- 5.36e-04 s (pulsed at 1.79% real time)
1

含有信息安全级别、时间戳、文件名，消息长度等。每次消息发送完成之后会调用 void ScopedRosLogSink::WaitTillSent() 函数，如果发生致命(FATAL)错误，则会通过延时，给ROS一些时间来发布我们的消息。
 

三、main函数

在理解了 gflags 与 glog 之后，再来看 main 函数就比较简单了，主要步骤如下：

$(1)$ 初始化glog库，然后解析命令行参数判断 configuration_directory 与 configuration_basename 两个参数是否输入，如果却是则报错

$(2)$ 调用 ::ros::init(argc, argv, “cartographer_node”); 初始化一个节点，再调用 ::ros::start(); 启动ROS相关的线程，网络等(一般不需要在自己的代码中显式调用)

$(3)$ 创建一个 cartographer_ros::ScopedRosLogSink 类对象，使用 ROS_INFO 进行glog消息的输出。

$(4)$ 调用 cartographer_ros 命名空间中的 Run() 函数，然后等待结束ROS相关的线程, 网络等→ros::shutdown()。

代码注释如下：

int main(int argc, char** argv) {

  // note: 初始化glog库
  google::InitGoogleLogging(argv[0]);
   
  // 使用gflags进行参数的初始化. 其中第三个参数为remove_flag
  // 如果为true, gflags会移除parse过的参数, 否则gflags就会保留这些参数, 但可能会对参数顺序进行调整.
  google::ParseCommandLineFlags(&argc, &argv, true);

  /**
   * @brief glog里提供的CHECK系列的宏, 检测某个表达式是否为真
   * 检测expression如果不为真, 则打印后面的description和栈上的信息
   * 然后退出程序, 出错后的处理过程和FATAL比较像.
   */
  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  // ros节点的初始化,可以理解为创建一个节点
  ::ros::init(argc, argv, "cartographer_node"); 

  // 一般不需要在自己的代码中显式调用
  // 但是若想在创建任何NodeHandle实例之前启动ROS相关的线程, 网络等, 可以显式调用该函数.
  ::ros::start();

  // 使用ROS_INFO进行glog消息的输出
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  // 开始运行cartographer_ros
  cartographer_ros::Run();

  // 结束ROS相关的线程, 网络等
  ::ros::shutdown();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

 

四、Run()函数

主函数main虽然比较简单，但是其调用的 cartographer_ros::Run(); 函数还是比较复杂，主要逻辑如下：

$(1)$ 关键字 constexpr 定义一个常量且初始 kTfBufferCacheTimeInSeconds = 10，调用 tf2_ros::Buffer 创建一个缓存 tf(不懂没关系，后面会讲解) 的类对象，其缓存 kTfBufferCacheTimeInSeconds(10) 秒内的 tf。然后再调用
tf2_ros::TransformListener 开启监听 tf 的独立线程。

$(2)$ 创建 NodeOptions node_options 与 TrajectoryOptions trajectory_options 两个文件，分别用来存储来自 .lua 文件的相关参数，具体是如何加载的，加载了那些参数下一篇博客为大家介绍。

$(3)$ 构建一个 MapBuilder 类对对象 map_builder，其继承于 MapBuilderInterface。MapBuilder 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 。该为后续分析的重点。

$(4)$ Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder，这样接收到的数据，就能够处理然后传递给 MapBuilder，从而运转起 Cartographer 的核心算法。

$(5)$ 根据输入参数决定是否加载pbstream文件，是否使用默认topic 开始轨迹。然后执行 ros::spin();→单线程,回调函数执行有一定次序，不能并发。

$(6)$ ros线程结束时，结束所有处于活动状态的轨迹，当所有的轨迹结束时, 再执行一次全局优化，如果save_state_filename非空, 就保存pbstream文件。

代码注释如下：

namespace cartographer_ros {
namespace {

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  // 开启监听tf的独立线程
  tf2_ros::TransformListener tf(tf_buffer);

  NodeOptions node_options;
  TrajectoryOptions trajectory_options;

  // c++11: std::tie()函数可以将变量连接到一个给定的tuple上,生成一个元素类型全是引用的tuple

  // 根据Lua配置文件中的内容, 为node_options, trajectory_options 赋值
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

  // MapBuilder类是完整的SLAM算法类
  // 包含前端(TrajectoryBuilders,scan to submap) 与 后端(用于查找回环的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
  
  // c++11: std::move 是将对象的状态或者所有权从一个对象转移到另一个对象, 
  // 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能..
  // 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象, 
  // 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.
  // 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

  // 如果加载了pbstream文件, 就执行这个函数
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

  ::ros::spin(); //单线程,回调函数执行有一定次序，不能并发。

  // 结束所有处于活动状态的轨迹
  node.FinishAllTrajectories();

  // 当所有的轨迹结束时, 再执行一次全局优化
  node.RunFinalOptimization();

  // 如果save_state_filename非空, 就保存pbstream文件
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }
}

}  // namespace
}  // namespace cartographer_ros
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

 

五、结语

通过该篇博客。针对 src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 文件进行了讲解，但是这仅仅是开始而已。接下来我们先讲解一下如何加载 .lua 配置文件的。