本人講解關于slam一系列文章匯總鏈接:史上最全slam從零開始，針對于本欄目講解(02)Cartographer源碼無死角解析-鏈接如下:
(02)Cartographer源碼無死角解析- (00)目錄_最新無死角講解：https://blog.csdn.net/weixin_43013761/article/details/127350885
?
$文末正下方中心提供了本人聯(lián)系方式，點擊本人照片即可顯示WX\to 官方認證$
?

一、前言

下面我們就正式開始講解代碼了，那么不用多說，首先我們是從 main 函數(shù)開始分析，那么問題來了，如何找到 main 函數(shù)呢？根據(jù)src/cartographer_ros/cartographer_ros/launch/lx_rs16_2d_outdoor.launch 文件，可以看到其有 node，名字name分別為：cartographer_node，cartographer_occupancy_grid_node 他們分別做什么的，先不去理會，就假設現(xiàn)在我們不知道。src/cartographer_ros/cartographer_ros/cartographer_ros/CMakeLists.txt 文件中搜索 cartographer_node、cartographer_occupancy_grid_node 可以看到看到如下內(nèi)容：

# 生成建圖節(jié)點
google_binary(cartographer_node
  SRCS
    node_main.cc
)
google_binary(cartographer_occupancy_grid_node
  SRCS
    occupancy_grid_node_main.cc
)

其上的 google_binary 是 google 自己封裝好的cmake(cmake是一門編程語言，可以定義函數(shù))函數(shù)，其會把 node_main.cc 生成二進制可執(zhí)行文件 cartographer_node，occupancy_grid_node_main.cc 生成二進制可執(zhí)行文件 cartographer_occupancy_grid_node。對于 .launch 文件中的節(jié)點 rviz 與 playbag 后續(xù)過程中進行講解。

node_main.cc 文件位于 src/cartographer_ros/cartographer_ros/cartographer_ros/ 文件夾下，那么下面我們就開始分析吧。
?

二、gflags與glog

1、gflags

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

DEFINE_bool 、 DEFINE_string、

其都是來自于 gflags 中，其是谷歌命令行解析工具。主要用于解析用命令行執(zhí)行可執(zhí)行文件時傳入的參數(shù)。與getops()不同的是，在gflags中flag可以分散的定義在各個文件之中，而不用定義在一起，這就意味著在我們可以在一個單獨的文件中只定義這個文件所需要用到的一些flag，鏈接了該文件應用都可以使用該文件中的flag，這樣就能非常方便的實現(xiàn)代碼的復用，如果不同的文件定義了相同的flag，則會產(chǎn)生錯誤，所以需要明確規(guī)范gflags的使用規(guī)范。

/**
 * note: gflags是一套命令行參數(shù)解析工具
 * DEFINE_bool在gflags.h中定義
 * gflags主要支持的參數(shù)類型包括bool, int32, int64, uint64, double, string等
 * 定義參數(shù)通過DEFINE_type宏實現(xiàn), 該宏的三個參數(shù)含義分別為命令行參數(shù)名, 參數(shù)默認值, 以及參數(shù)的幫助信息
 * 當參數(shù)被定義后, 通過FLAGS_name就可訪問到對應的參數(shù)
 */

如下源碼中的示例：

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

DEFINE_bool 表示闖入的參數(shù)為 bool類型， collect_metrics 表示命令行需要傳入的參數(shù)，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" />

表示再在執(zhí)行 cartographer_node 程序的時候，其傳入的兩個參數(shù)，分別為 -configuration_directory，-configuration_basename，其與 node_main.cc 文文件中的 configuration_directory，configuration_basename 是一一對應的。另外，runlaunch 指令還會把 remap(重映射)操作也當作參數(shù)傳入到 main 函數(shù)的 **argv 中。如下：

傳送進來的參數(shù)會經(jīng)過

  // note: 初始化glog庫
  google::InitGoogleLogging(argv[0]);
   
  // 使用gflags進行參數(shù)的初始化. 其中第三個參數(shù)為remove_flag
  // 如果為true, gflags會移除parse過的參數(shù), 否則gflags就會保留這些參數(shù), 但可能會對參數(shù)順序進行調(diào)整.
  google::ParseCommandLineFlags(&argc, &argv, true);

兩個函數(shù)的處理，處理之后變換成如下：

先來看看 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中未配置，使用默認值。將保存的狀態(tài)加載為凍結(jié)（非優(yōu)化）軌跡。
$(5)$ start_trajectory_with_default_topics→使用默認主題(話題)開始第一個軌跡
$(6)$ save_state_filename→保存軌跡文件的路徑

如果想引用這些參數(shù)，在參數(shù)的前面加入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"

?

2、glog

Google glog 是一個應用級別的日志系統(tǒng)庫.它提供基于 C++風格的流和各種輔助宏的日志 API.支持以下功能：

01.參數(shù)設置, 以命令行參數(shù)的方式設置標志參數(shù)來控制日志記錄行為
02.嚴重性分級, 根據(jù)日志嚴重性分級記錄日志 - 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.系統(tǒng)級日志記錄
10.google perror 風格日志信息
11.精簡日志字符串信息

其最終的結(jié)果不僅會在屏幕終端顯示出來, 同時會將 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) //.......

這些語句能夠很方便的把信息輸入到日志中，如源碼中的：

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

如果輸入?yún)?shù) configuration_directory 或 FLAGS_configuration_basename 為空，則會記錄錯誤信息，并且終止程序。那么我們應該如何使用 glog，自定義一些我們想要的信息呢？源碼中可以看到如下代碼(位于node_main.cc)：

  // 使用ROS_INFO進行glog消息的輸出
  cartographer_ros::ScopedRosLogSink ros_log_sink;

其中的 ScopedRosLogSink 的定義與實現(xiàn)如下(位于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_;
};

首先其繼承了 google::LogSink，只需要實現(xiàn) send() 、WaitTillSent()、WaitTillSent() 三個函數(shù)即可，在src/cartographer_ros/cartographer_ros/cartographer_ros/ros_log_sink.cc 中實現(xiàn)，如下：

/*
 * 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 <chrono>
#include <cstring>
#include <string>
#include <thread>

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

namespace cartographer_ros {

namespace {

/**
 * @brief 根據(jù)給定的文件全路徑名, 獲取文件名
 * 
 * @param[in] filepath 
 * @return const char* 返回文件名
 */
const char* GetBasename(const char* filepath) {
  // 找到 '/' 最后一次在filepath中出現(xiàn)的位置
  const char* base = std::strrchr(filepath, '/');
  // 找到'/',就將'/'之后的字符串返回；找不到'/', 就將整個filepath返回
  return base ? (base + 1) : filepath;
}

}  // namespace


/**
 * @brief 在構(gòu)造函數(shù)中調(diào)用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 消息所在的文件行數(shù)
 * @param[in] tm_time 消息的時間
 * @param[in] message 消息數(shù)據(jù)本體
 * @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后調(diào)用, 用于一些異步寫的場景
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

上面你的代碼邏輯也比較建檔，核心的語句為：

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

::google::LogSink::ToString 函數(shù)會把輸入的參數(shù)如 severity，line，message 等匯聚成一條字符串信息，然后根據(jù)信息等級通過 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)

含有信息安全級別、時間戳、文件名，消息長度等。每次消息發(fā)送完成之后會調(diào)用 void ScopedRosLogSink::WaitTillSent() 函數(shù)，如果發(fā)生致命(FATAL)錯誤，則會通過延時，給ROS一些時間來發(fā)布我們的消息。
?

三、main函數(shù)

在理解了 gflags 與 glog 之后，再來看 main 函數(shù)就比較簡單了，主要步驟如下：

$(1)$ 初始化glog庫，然后解析命令行參數(shù)判斷 configuration_directory 與 configuration_basename 兩個參數(shù)是否輸入，如果卻是則報錯

$(2)$ 調(diào)用 ::ros::init(argc, argv, “cartographer_node”); 初始化一個節(jié)點，再調(diào)用 ::ros::start(); 啟動ROS相關的線程，網(wǎng)絡等(一般不需要在自己的代碼中顯式調(diào)用)

$(3)$ 創(chuàng)建一個 cartographer_ros::ScopedRosLogSink 類對象，使用 ROS_INFO 進行glog消息的輸出。

$(4)$ 調(diào)用 cartographer_ros 命名空間中的 Run() 函數(shù)，然后等待結(jié)束ROS相關的線程, 網(wǎng)絡等→ros::shutdown()。

代碼注釋如下：

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

  // note: 初始化glog庫
  google::InitGoogleLogging(argv[0]);
   
  // 使用gflags進行參數(shù)的初始化. 其中第三個參數(shù)為remove_flag
  // 如果為true, gflags會移除parse過的參數(shù), 否則gflags就會保留這些參數(shù), 但可能會對參數(shù)順序進行調(diào)整.
  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節(jié)點的初始化,可以理解為創(chuàng)建一個節(jié)點
  ::ros::init(argc, argv, "cartographer_node"); 

  // 一般不需要在自己的代碼中顯式調(diào)用
  // 但是若想在創(chuàng)建任何NodeHandle實例之前啟動ROS相關的線程, 網(wǎng)絡等, 可以顯式調(diào)用該函數(shù).
  ::ros::start();

  // 使用ROS_INFO進行glog消息的輸出
  cartographer_ros::ScopedRosLogSink ros_log_sink;

  // 開始運行cartographer_ros
  cartographer_ros::Run();

  // 結(jié)束ROS相關的線程, 網(wǎng)絡等
  ::ros::shutdown();
}

?

四、Run()函數(shù)

主函數(shù)main雖然比較簡單，但是其調(diào)用的 cartographer_ros::Run(); 函數(shù)還是比較復雜，主要邏輯如下：

$(1)$ 關鍵字 constexpr 定義一個常量且初始 kTfBufferCacheTimeInSeconds = 10，調(diào)用 tf2_ros::Buffer 創(chuàng)建一個緩存 tf(不懂沒關系，后面會講解) 的類對象，其緩存 kTfBufferCacheTimeInSeconds(10) 秒內(nèi)的 tf。然后再調(diào)用
tf2_ros::TransformListener 開啟監(jiān)聽 tf 的獨立線程。

$(2)$ 創(chuàng)建 NodeOptions node_options 與 TrajectoryOptions trajectory_options 兩個文件，分別用來存儲來自 .lua 文件的相關參數(shù)，具體是如何加載的，加載了那些參數(shù)下一篇博客為大家介紹。

$(3)$ 構(gòu)建一個 MapBuilder 類對對象 map_builder，其繼承于 MapBuilderInterface。MapBuilder 包含前端(TrajectoryBuilders,scan to submap) 與 后端(用于查找回環(huán)的PoseGraph) 。該為后續(xù)分析的重點。

$(4)$ Node類的初始化, 將ROS的topic傳入SLAM, 也就是MapBuilder，這樣接收到的數(shù)據(jù)，就能夠處理然后傳遞給 MapBuilder，從而運轉(zhuǎn)起 Cartographer 的核心算法。

$(5)$ 根據(jù)輸入?yún)?shù)決定是否加載pbstream文件，是否使用默認topic 開始軌跡。然后執(zhí)行 ros::spin();→單線程,回調(diào)函數(shù)執(zhí)行有一定次序，不能并發(fā)。

$(6)$ ros線程結(jié)束時，結(jié)束所有處于活動狀態(tài)的軌跡，當所有的軌跡結(jié)束時, 再執(zhí)行一次全局優(yōu)化，如果save_state_filename非空, 就保存pbstream文件。

代碼注釋如下：

namespace cartographer_ros {
namespace {

void Run() {
  constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  // 開啟監(jiān)聽tf的獨立線程
  tf2_ros::TransformListener tf(tf_buffer);

  NodeOptions node_options;
  TrajectoryOptions trajectory_options;

  // c++11: std::tie()函數(shù)可以將變量連接到一個給定的tuple上,生成一個元素類型全是引用的tuple

  // 根據(jù)Lua配置文件中的內(nèi)容, 為node_options, trajectory_options 賦值
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

  // MapBuilder類是完整的SLAM算法類
  // 包含前端(TrajectoryBuilders,scan to submap) 與 后端(用于查找回環(huán)的PoseGraph) 
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
  
  // c++11: std::move 是將對象的狀態(tài)或者所有權從一個對象轉(zhuǎn)移到另一個對象, 
  // 只是轉(zhuǎn)移, 沒有內(nèi)存的搬遷或者內(nèi)存拷貝所以可以提高利用效率,改善性能..
  // 右值引用是用來支持轉(zhuǎn)移語義的.轉(zhuǎn)移語義可以將資源 ( 堆, 系統(tǒng)對象等 ) 從一個對象轉(zhuǎn)移到另一個對象, 
  // 這樣能夠減少不必要的臨時對象的創(chuàng)建、拷貝以及銷毀, 能夠大幅度提高 C++ 應用程序的性能.
  // 臨時對象的維護 ( 創(chuàng)建和銷毀 ) 對性能有嚴重影響.

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

  // 如果加載了pbstream文件, 就執(zhí)行這個函數(shù)
  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(); //單線程,回調(diào)函數(shù)執(zhí)行有一定次序，不能并發(fā)。

  // 結(jié)束所有處于活動狀態(tài)的軌跡
  node.FinishAllTrajectories();

  // 當所有的軌跡結(jié)束時, 再執(zhí)行一次全局優(yōu)化
  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

?

五、結(jié)語

通過該篇博客。針對 src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 文件進行了講解，但是這僅僅是開始而已。接下來我們先講解一下如何加載 .lua 配置文件的。

?
?
?文章來源地址http://www.zghlxwxcb.cn/news/detail-424412.html

到了這里，關于(02)Cartographer源碼無死角解析-(09) gflags與glog簡介、及其main函數(shù)講解的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！