(02)Cartographer源码无死角解析-(13) Node::AddTrajectory()→订阅话题与注册回调函数

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

一、前言

继续前面的内容继续简介，在 Node::AddTrajectory() 函数中，其调用了 LaunchSubscribers(options, trajectory_id) 函数，其主要的作用就是订阅话题与注册回调函数。其主要根据传感器的类型与数目注册回调函数，但是注意，同一类型的位姿传感器共用一个回调函数。如：

Node::HandleLaserScanMessage() //所有单线雷达topic回调函数
Node::HandleMultiEchoLaserScanMessage() //所有回声波雷达topoc回调函数
Node::HandlePointCloud2Message() //所有多线雷达topic回调函数
Node::HandleImuMessage() //所有IMU topic回调函数
Node::HandleOdometryMessage() //所有里程计 topic
Node::HandleNavSatFixMessage() //GPU
Node::HandleLandmarkMessage()//Landmar 回调函数
1
2
3
4
5
6
7

LaunchSubscribers() 函数初步看起来是比较复杂的，不过没有关系，一步步对其进行分析即可。在讲解之前，这里提及一下 C++11 中的 lambda 表达式，其编程规则如下：

[捕捉列表](参数)mutable—>返回值类型{ 函数体 } 
1

 
$1.[捕捉列表]$ 该列表总是出现在lambda表达式的起始位置，编译器根据[]来判断接下来的代码是否为lambda表达式，捕捉列表能够捕捉当前作用域中的变量，供lambda函数使用。

[val]： 表示以传值方式捕捉变量val
[=]： 表示以传值方式捕捉当前作用域中的变量，包括this指针。
[&val]： 表示以引用方式传递捕捉变量val。
[&]： 表示以引用方式传递捕捉当前作用域中的所有变量，包括this指针。
[this]： 表示以传值方式捕捉当前的this指针。
1
2
3
4
5

简单的说，就是你书写的这个函数需要那些变量，你可以通过指定具体的变量名，也可以通过=或者&号捕获作用域的所有变量等。
 
$2.(参数)$与普通函数参数列表使用相同。如果不需要传递参数，可以连同"()"一起省略。
 
$3.mutable$ 除了 mutable 还可以声明 exception，默认情况下，lambda函数总是一个const函数，捕捉的传值参数具有常性，mutable可以取消常性。使用mutable修饰符时，参数列表不能省略，即使参数为空。
 
$4.\to 返回值类型$ 使用追踪返回类型形式声明函数的返回值类型，没有返回值此部分可省略。返回值类型明确的情况下，也可省略，由编译器推导。
 
$5.函数体$ 在函数体内除了可以使用参数外，还能使用捕捉的变量，其余的变量都是部可以使用的。
 
$示例$

	//捕捉当前作用域的变量，没有参数，编译器推导返回值类型。
	int a = 1;
	int b = 2;
	[=]{return a + b; };

	auto fun1 = [&](int c){b = a + c; };   	//使用和仿函数差不多
	fun1(10);

	auto add1 = [&x, y]()->int{ x *= 2;//捕捉传递引用不具有常性
							return x + y; };
	cout << add1() << endl;

	//传值捕捉
	int x = 1;
	int y = 2;
	auto add0 = [x, y]()mutable->int{ x *= 2;//捕捉传递传值具有常性
									return x + y; };
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

上面都是列举的一些简单例子，有兴趣的朋友可以通过其他的资料查阅进行更加深入的了解。
 

二、语法讲解

首先 LaunchSubscribers() 函数调用了如下一段代码：

  // laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
1
2
3
4
5
6
7
8
9

关于 ComputeRepeatedTopicNames 的内容在上一篇博客中已经进行过讲解，其主要功能就是根据配置参数→传感器默认 Topic name 以及该类传感的数目 num_xxx 生成新的 Topics name。然后会进行循环，对每个新生成的 topic 都注册会调函数。

先来看看代码中的 subscribers_(订阅者)变量，从名字都能够看出来，其应该是存储 ROS 中的订阅者对象，定义如下：

std::unordered_map<int, std::vector<Subscriber>> subscribers_;
1

可以看到，其是一个 map 类型，key 其实就是 trajectory_id，value 为 Subscriber 类型，该类型在同样在 node.h 中定义：

  struct Subscriber {
    ::ros::Subscriber subscriber;
    std::string topic;
  };
1
2
3
4

注意看清楚，这里重新定义了一个 Subscriber，与 ::ros::Subscriber 不要搞混淆了。也就是说，如果往 subscribers_ 添加元素，其需要需要一个 trajectory_id 以及一个 Subscriber 对象。Subscriber 对象中有两个成员，分别为 ::ros::Subscriber subscriber 与 std::string topic。

源码中在添加元素时，使用花括号{}进行初始化，也就是 {::ros::Subscriber，std::string} 这种形式，源码如下：

    {SubscribeWithHandler<sensor_msgs::LaserScan>(
         &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
         this),
     topic}
1
2
3
4

topic 就是 std::string 类型，SubscribeWithHandler() 函数返回的实际就是 ::ros::Subscriber 类型，定义在 node.cc 中如下。
 

三、SubscribeWithHandler()

初步了解代码执行原理之后，下面看详细看看 SubscribeWithHandler() 函数的实现：

/**
 * @brief 在node_handle中订阅topic,并与传入的回调函数进行注册
 * 
 * @tparam MessageType 模板参数,消息的数据类型
 * @param[in] handler 函数指针, 接受传入的函数的地址
 * @param[in] trajectory_id 轨迹id
 * @param[in] topic 订阅的topic名字
 * @param[in] node_handle ros的node_handle
 * @param[in] node node类的指针
 * @return ::ros::Subscriber 订阅者
 */
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,  // kInfiniteSubscriberQueueSize = 0
      // 使用boost::function构造回调函数,被subscribe注册
      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表达式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}
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

首先第一参数，是一个函数指针，如下面的整个代码，其表示的是一个参数(参数名为handler)：

void (Node::*handler)(int, const std::string&,const typename MessageType::ConstPtr&),
1

其实也是比较好理解的，首先该函数必须是 Node 类中实现，返回值为 void 类型。该函数的第一个参数为 int 类型、第二个为 const std::string& 类型、第三个为 const typename MessageType::ConstPtr& 类型。

SubscribeWithHandler() 第二个参数为 const int trajectory_id，第三个参数为 const std::string& topic 都是比好好理解的，第四个 ::ros::NodeHandle* const node_handle 是前面提到的节点句柄，第五个为 Node* 类型，一般为this。

了解 SubscribeWithHandler() 的参数之后，可以看到其还是一个模板函数，回到之前的代码，来看看该模板函数其是如和调用的：

SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, this)
1
2

首先模板需要指定消息类型，这里 sensor_msgs::LaserScan 说明其为单线雷达消息类型，指定了消息类型之后，应该如何去订阅的话题呢？

根据对ROS的了解，订阅话题一般使用 NodeHandle::subscribe() 注册回调函数，同时其会返回一个 ROS::Subscriber 类型，NodeHandle::subscribe() 函数至少需要传入三个参数，分别为→订阅话题的名称、消息队列大小、回调函数。回到 SubscribeWithHandler() 函数如下：

  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,  // kInfiniteSubscriberQueueSize = 0
      // 使用boost::function构造回调函数,被subscribe注册
      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表达式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
1
2
3
4
5
6
7
8

现在能够看明白，通过 node_handle->subscribe 创建一个订阅者 ，消息类型为 MessageType(模板参数)，订阅的话题为 topic，消息队列大小为 kInfiniteSubscriberQueueSize，那么现在还缺少一个参数，那就是回调函数。前面传入了一个 函数指针 handler，那么我们如何去使用呢？

总的来说：

      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表达式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          })
1
2
3
4
5

其表达就是一个函数，先来看看其 lambda表达式 部分(在文章开头有初步的讲解)。该 lambda 函数需要使用的到 [node, handler, trajectory_id, topic] 四个变量，传递的参数为 const typename MessageType::ConstPtr& 类型，其函数内容就是调用了 node->*handler 函数(传递trajectory_id, topic, msg三个参数)。

注意，也就是说利用 lambda 表达式，原本的NodeHandle::subscribe()即node_handle->subscribe()函数注册的回调函数只能只能传入一个参数。但是现在却可以传入三个函数了，这就是 lambda 表达式捕获代码的优势，也就是源码中这样书写的主要原因。

然后再来看看 boost::function，个人感觉该函数在这里没有起到什么作用，或许可以加快程序的运行吧，本人尝试把 boost::function 注释掉之后，依然可以编译运行。不过通过查阅资料，大概的意思就是回调函数都喜欢与 boost::function 连用，其他暂未深入了解。
 

四、LaunchSubscribers()注释

/**
 * @brief 订阅话题与注册回调函数
 * 
 * @param[in] options 配置参数
 * @param[in] trajectory_id 轨迹id  
 */
void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  // laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }

  // multi_echo_laser_scans的订阅与注册回调函数
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  
  // point_clouds 的订阅与注册回调函数
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, 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.
  // imu 的订阅与注册回调函数,只有一个imu的topic
  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())) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, kImuTopic,
                                                &node_handle_, this),
         kImuTopic});
  }

  // odometry 的订阅与注册回调函数,只有一个odometry的topic
  if (options.use_odometry) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, kOdometryTopic,
                                                  &node_handle_, this),
         kOdometryTopic});
  }

  // gps 的订阅与注册回调函数,只有一个gps的topic
  if (options.use_nav_sat) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
             &node_handle_, this),
         kNavSatFixTopic});
  }

  // landmarks 的订阅与注册回调函数,只有一个landmarks的topic
  if (options.use_landmarks) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, 
             trajectory_id, 
             kLandmarkTopic,
             &node_handle_, 
             this),
         kLandmarkTopic});
  }
}
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

 

五、MaybeWarnAboutTopicMismatch

讲解完 LaunchSubscribers() 函数之后，回到 Node::AddTrajectory() 其还执行了如下代码：

  // 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
  // 检查设置的topic名字是否在ros中存在, 不存在则报错
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

  // 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  return trajectory_id;
1
2
3
4
5
6
7
8
9
10
11
12

创建了一个定时器，间隔 kTopicMismatchCheckDelaySec 秒执行一次 MaybeWarnAboutTopicMismatch 函数，由于 由于oneshot=true, 所以只执行一次。进入 Node::MaybeWarnAboutTopicMismatch() 函数，主要功能检查设置的 topic 名字是否在ros中存在, 不存在则报错。

其主要执行步骤如下：

$(1):$ 首先使用ros的master的api进行topic名字的获取，如本人显示如下：

因为是订阅话题，所以需要订阅的话题存在于ros的master之中。对所有 master 中的话题进行循环，获得其全局名称。如 “blah” => “/namespace/blah”。

$(2):$ 循环遍历获取实际订阅的topic名字，如果设置的topic名字,在ros中不存在,则报错。注意实际订阅的topic名字按照 trajectory_id 存储于 subscribers_ 变量中，所以有两层循环，第一层是遍历轨迹，第二层遍历轨迹下的所有topic名字。

$(3):$ 如果报错，还会打印信息告诉订阅者那些 topic 可用。

源码注释如下：

// 检查设置的topic名字是否在ros中存在, 不存在则报错
void Node::MaybeWarnAboutTopicMismatch(
    const ::ros::WallTimerEvent& unused_timer_event) {

  // note: 使用ros的master的api进行topic名字的获取
  ::ros::master::V_TopicInfo ros_topics;
  ::ros::master::getTopics(ros_topics);

  std::set<std::string> published_topics;
  std::stringstream published_topics_string;

  // 获取ros中的实际topic的全局名称,resolveName()是获取全局名称
  for (const auto& it : ros_topics) {
    std::string resolved_topic = node_handle_.resolveName(it.name, false);
    published_topics.insert(resolved_topic);
    published_topics_string << resolved_topic << ",";
  }
  
  bool print_topics = false;
  for (const auto& entry : subscribers_) {
    int trajectory_id = entry.first;
    for (const auto& subscriber : entry.second) {

      // 获取实际订阅的topic名字
      std::string resolved_topic = node_handle_.resolveName(subscriber.topic);

      // 如果设置的topic名字,在ros中不存在,则报错
      if (published_topics.count(resolved_topic) == 0) {
        LOG(WARNING) << "Expected topic \"" << subscriber.topic
                     << "\" (trajectory " << trajectory_id << ")"
                     << " (resolved topic \"" << resolved_topic << "\")"
                     << " but no publisher is currently active.";
        print_topics = true;
      }
    }
  }
  // 告诉使用者哪些topic可用
  if (print_topics) {
    LOG(WARNING) << "Currently available topics are: "
                 << published_topics_string.str();
  }
}
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

 

六、结语

在 Node::AddTrajectory 函数的最后，还会执行如下代码：

  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
1
2
3

其是将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复。也就是说 subscribed_topics_ 存储的是当前已经订阅过的 topic 名字。