从代码理解 cartographer 1

之前看了不少的cartographer的从总体上了解cartographer的文章。但是代码是怎么做的，代码怎么写的。我一点都不清楚。

所以这是一次再出发，我自己从代码层面去看cartographer。

在从代码层面上了解cartographer的，有知乎的一个 cartographer源码解读系列

在了解读系列之后，我心里还不是很清楚。所以我自己开始自己的解读过程的记录。

我自己了解cartographer的一个随笔过程。

在我写ROS程序的时候，我自己就是一个过程思维。每一步都经历了什么过程。

所以我在这一系列里面，尝试着解读这些问题。

首先是数据从哪里进去。 

如果有错误，希望大家指出。谢谢

---

官方流程图：

官方流程图还是很清晰明了地讲解了数据的输入有哪几个种类。

在官方流程图的最左边，首先说明了输入的数据雷达的范围数据，IMU数据以及Odom数据。

下面，我们就来了解清楚这些数据进行输入时，代码具体如何执行，以及Google的工程师，是如何设计了这些类。

这些都是我很好奇的地方。

---

第一部分：Cartographer订阅的各种话题的流程是什么。

在Cartographer的流程图里面，分别有三种数据，雷达数据，odom数据以及IMU数据。

这三个数据分别是雷达驱动所发出的，以及底盘驱动所发出来的Odom数据以及IMU数据。

Cartographer分别订阅了他们所发出来的话题。

那么Cartographer在线算法部分在订阅这些话题的主要代码是什么呢？

下面整理的数个步骤，就是我的理解。

我仅仅列出整个过程所需要的函数，其它的会倾向于省略。

第一步：

在ros运行的时候, 入口的函数 main, 这一主函数所在的 文件是 /src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 下:
其中在main中, 主要的运行主要部分的函数:

int main(int argc, char** argv) {
    ...
    cartographer_ros::Run(); // 调用了同一个文件中的run函数
    ...
}

在main函数当中，除了初始化其它对象，如glog和cartographer这些对象之外，就是运行Run函数。

Run函数就像是主要启动的一个函数。在这，可以看到工程师的一个想法就是，用一个函数来包含启动需要的一些主要步骤，那这就是Run函数。

Run函数的主要代码如下：

void Run() {
    ...
    //Node在/cartographer_ros/cartographer_ros/cartographer/node.h中定义；
    // 在该构造函数中订阅了很多传感器的topic。收集传感器数据
    // node_options 是为了node配置的一个类
    // map_builder 是 cartographer里面的一个类
    Node node(node_options, std::move(map_builder), &tf_buffer);
    ..
}

在Run函数当中，主要与订阅相关相关是实例化Node类。

ROS程序当中，主要是以节点的形式进行存在。所以工程师们想用一个类来代表这个ROS节点，进行处理ROS节点事务。

node_options，是另外一个类，是用来表示节点的选项的一个类。

map_builder，是另外一个类，是同来表示地图构造的，虽然看过他的函数，但仍然不知道他具体的东西。

tf_buffer，是tf树的一个缓冲区。

第二步：

Node类节点, 在 src/cartographer_ros/cartographer_ros/cartographer_ros/node.h 里面进行定义的.

根据ROS的用法习惯，如果这个类里面有订阅的相关代码，一定会拥有 ros::Subscriber 这个相关信息。

然后顺藤摸瓜，可以得出以下跟订阅相关的变量和函数：

namespace cartographer_ros {
class Node {    
private:
   struct Subscriber { // Subscriber 构成的新的订阅pair似的数据结构
       ::ros::Subscriber subscriber; // ros订阅用法的类
       std::string topic;  // 主题的名字
   }
   ....
   // 因为调用 AddTrajectory
  bool HandleStartTrajectory( 
      cartographer_ros_msgs::StartTrajectory::Request& request,
      cartographer_ros_msgs::StartTrajectory::Response& response);
  // 从名字看，就知道和订阅有关。
  void LaunchSubscribers(const TrajectoryOptions& options,
                         const cartographer_ros_msgs::SensorTopics& topics,
                         int trajectory_id);
  // 会调用 LaunchSubscribers
  int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics)
  .... 
 // 用服务的形式管理所有第一个 trajectory                      
  std::vector<::ros::ServiceServer> service_servers_;
  // 再根据 Subscriber， 会搜索到这个变量。用一个unordered_map来进行组织
  // int 类型，我看了后面知道这是一个 trajectory_id 
 // 后面是一个vector组成Subscriber
  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
  ...
}    
}

代码中列出了相关的代码和注释：

他们是从 ros::Subscriber 开始，一路顺延下去进行挖掘。能够得出这些代码都是订阅相关的。

在后面，会讲解每个函数都做了什么，会解释每一个函数当中与订阅相关的代码。

第三步：

刚才我们从定义上面知道了这些相关的变量是哪些，这是很有用的，因为，初始化函数里面的东西很多，找到相关的代码就OK了。

但真正的流程是从Node的初始化函数开始的。

主要与订阅数据相关的代码如下：

Node::Node(
    const NodeOptions& node_options,    // 选项
    std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,    // 地图匹配
    tf2_ros::Buffer* const tf_buffer)     // tf 树
    : node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
    ...
    // node_constants.h 里面定义了下面的常量
    // constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
    // 将 HandleStartTrajectory 作为服务的回调函数, 那么问题来了, 哪里调用了这个服务? 
   // 这个我还不确切知道. 但以我自己的感觉, 是在Mapbuilder那边,开始添加trajectory的时候.
    service_servers_.push_back(node_handle_.advertiseService(
      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); 
   ...   
}

从代码可以看出，在初始化函数里面

vector service_servers_ 进行了服务的管理，它压入一个新增的名为kStartTrajectoryServiceName的服务。

这个服务的回调函数是HandleStartTrajectory。 

所以这个服务被调用的时候，就会去启动handleStartTrajectory这个函数。

大家有没一个问题？为什么用一个服务呢？启动订阅不是一次就可以启动，然后数据源源不断了吗？为什么需要一个服务，可以让其它部件多次调用呢？

我个人的猜测，这和Trajectory是有关系的。每次开始一个Trajectory，可能都会调用这个。

至于是不是这样，我不确切知道的哦。只是猜测。

言归正传，知道哪里调用了 HandleStartTrajectory 被调用了。

那么我们还得了解下 HandleStartTrajectory 做了什么。

第四步：

HandleStartTrajectory 的主要代码如下，因为它是一个ROS规定的回调函数，它有它自己的参数格式。

request 表达的就是调用这个服务的请求的数据结构。

response 就是自定义的回复的格式。

bool Node::HandleStartTrajectory(
    ::cartographer_ros_msgs::StartTrajectory::Request& request,
    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
  carto::common::MutexLocker lock(&mutex_);
  TrajectoryOptions options;
  // 判断从ROS消息里面读取出options, 并且,这个options 是否是有效的
  if (!FromRosMessage(request.options, &options) ||
      !ValidateTrajectoryOptions(options)) {
          // 无效的option
    const std::string error = "Invalid trajectory options.";
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = error;
  } else if (!ValidateTopicNames(request.topics, options)) { // 判断是否无效的topic name
      // 无效的topic name
    const std::string error = "Invalid topics.";
    LOG(ERROR) << error;
    response.status.code = cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT;
    response.status.message = error;
  } else {
      // 如果都成功, 所以就会添加这个主题
    response.trajectory_id = AddTrajectory(options, request.topics);
    response.status.code = cartographer_ros_msgs::StatusCode::OK;
    response.status.message = "Success.";
  }
  return true;
}   

代码中明确表示了它的逻辑就是，判断 当前情况为正常情况之后,

就使用了AddTrajectory 来添加第一个(我不确定这是第一个阿,只是看名字才说是第一个)  trajector. 

在调用 AddTrajectory 的时候,可以看到, 是通过 request.topics 来定义需要调用的哪些话题.

我们又进了一步，那就是  AddTrajectory

第五步 

AddTrajectory 里面的代码如下：

int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  // 根据options, 得出一个SensorID的set
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics); 
  // 根据sensor id 和 option, 得出 trajectory id.    
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  // Extrapolator 是一个推算位置的类, 因为我看过后面, 跟我想要解决的数据流入没关系
  AddExtrapolator(trajectory_id, options);
  // sensor_samplers_ 同样也是一个抽样的类, 跟数据流入没关系
  AddSensorSamplers(trajectory_id, options);
  // LaunchSubscribers 订阅所有话题的主要函数
  LaunchSubscribers(options, topics, trajectory_id);
  is_active_trajectory_[trajectory_id] = true;
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

代码里面有相关的注释，列出这段代码，主要是为了让大家看到 LaunchSubscribers 的调用，这是与订阅话题相关的。 

其它的语句的话，我清楚他们的作用，但我仅有的知识是

Extrapolator 是一个推算位置的类, 在概率机器人里面，有提到过两种方法来推算机器人的位置，采样法和根据当前的速度推算的方法。

明显这个Extrapolator是根据速度来推算下一个位置的类

sensor_samplers_ ： 我也不清楚这个，名字显示是传感器的抽样。但是我对LocalSLAM下面的东西还不算太清楚。所以不知道它的作用。具体是哪里。

第六步:

LaunchSubscribers 的 代码如下

// 在这儿，订阅了一堆东西。
void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const cartographer_ros_msgs::SensorTopics& topics,
                             const int trajectory_id) {
  // ComputeRepeatedTopicNames 作用: 确保传进去的topic,出来之后是唯一的.
  for (const std::string& topic : ComputeRepeatedTopicNames(
           topics.laser_scan_topic, options.num_laser_scans)) {
    // SubscribeWithHandler 的作用: 
    // 使用'node_handle'为'trajectory_id'订阅'topic'，并在'node'上调用'handler'来处理消息。 返回订阅者。          
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
                                 options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           topics.point_cloud2_topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    std::string topic = topics.imu_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, topic,
                                                &node_handle_, this),
         topic});
  }

  if (options.use_odometry) {
    std::string topic = topics.odometry_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, topic,
                                                  &node_handle_, this),
         topic});
  }
  if (options.use_nav_sat) {
    std::string topic = topics.nav_sat_fix_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  if (options.use_landmarks) {
    std::string topic = topics.landmark_topic;
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
}

这些代码的目的就很明确了，根据配置所配置的名称，来进行订阅话题。

在这些代码里面，工程师们的c++用法以及不清楚具体的目的，因为可能不确定里面到底输出的是什么东西，以及输出东西的含义。

我进行我的解读是这样的。它有一个相关的函数是这样的：

std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
                                                   const int num_topics) {
  CHECK_GE(num_topics, 0);
  if (num_topics == 1) {
    return {topic};
  }
  std::vector<std::string> topics;
  topics.reserve(num_topics);
  for (int i = 0; i < num_topics; ++i) {
    topics.emplace_back(topic + "_" + std::to_string(i + 1));
  }
  return topics;
}

从函数里面可以看到，如果输入的名字是一样的，但是有多个雷达，会加上 _<num> 这样的格式。

所以，这儿应该只是生成话题。然后进行了订阅。

然而这写topics是如何来的呢？

有一定的相关的默认配置，应该也是可以通过配置文件来进行配置。

默认的选项是这样的

cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
  cartographer_ros_msgs::SensorTopics topics;
  topics.laser_scan_topic = kLaserScanTopic;
  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
  topics.point_cloud2_topic = kPointCloud2Topic;
  topics.imu_topic = kImuTopic;
  topics.odometry_topic = kOdometryTopic;
  topics.nav_sat_fix_topic = kNavSatFixTopic;
  topics.landmark_topic = kLandmarkTopic;
  return topics;
}

kLaserScanTopic 这些名字，定义在node_constants.h 里面。

所以,其实,我们开始提出的问题: 数据在哪进行订阅， 代码中如何体现

看到这里就解决了.

第七步：

第七步起始与主线关系不大，订阅的在上面已经讲清楚，这儿是我在上面有个函数每看明白，所以列出来了。

函数是 SubscribeWithHandle 这个函数：

template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int trajectory_id, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node) {
  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

里面的

boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          })

函数，之前一直不明白。我知道boost::Function对象。但是没见过例子有 [] 这样的用法。

首先很明确的是，这个一定是返回一个函数对象。

对象的参数信息也可以从nodehandle->subscribe里面得出：

Subscriber ros::NodeHandle::subscribe(
    const std::string &     topic,
    uint32_t     queue_size,
    const boost::function< void(const boost::shared_ptr< M const > &)> &     callback,
    const VoidConstPtr &     tracked_object = VoidConstPtr(),
    const TransportHints &     transport_hints = TransportHints() 
)    

不懂的时候，我喜欢看看boost文档，然后在boost::function里面发现这样一个定义

boost::function1<int, int> f2(f); // f 是一个函数对象

这让我想起来， c++ lambda的用法是[]开头的。

所以上面的函数对象的意义就明了了。

这是一个用lambda初始boost::function的用法。

OK.想明白,真简单.

没想明白,这都啥跟啥

 其实在这里: 

1. 虽然看过后面的一些文章, 但我不确切明白 Trajectory 是什么. 

这些是另外的问题, 其实我现在是想知道, 数据开始流入了, LocalSLAM如何工作.

---

看一个理论和大项目的方法，大多都推荐，先看个大概的理论框架是什么跟什么。

我是看了一些从顶层介绍的文章，然后还是云里雾里。

所以，我还是习惯从底层慢慢看。因为，本身可能数学基础就不太好，没有那么好的抽象思维。

根据我自己编程的习惯就是，数据怎么来，怎么处理，得出什么结果这样开始再看一次到底怎么做。

沿着这中思路，下一章写什么？

等我写好了下一章的草稿。。。

我再来补这里。