话题订阅与注册回调-Node类-cartographer
Kong Liangqian Lv6
 2021-08-31 15:59:22  

上回说道添加轨迹函数Node::AddTrajectory中的订阅话题与注册回调函数，在此之前计算了一个由topicname和sensor类型组合成的一个集合expected_sensor_ids，轨迹IDtrajectory_id，新添加了一个位姿估计器，和一个传感器采样器

1
2
// 订阅话题与注册回调函数
  LaunchSubscribers(options, trajectory_id);

订阅话题与注册回调函数

在整个LaunchSubscribers函数里面，会根据配置信息对相应的topic进行订阅并且注册回调函数。对于有多个可能设备的topic，通过ComputeRepeatedTopicNames来获取topic名字。并且订阅并进行回调

1
2
3
4
5
6
7
8
9
// 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});
}

对于只有单个设备的例如里程计，简单if判断即可

1
2
3
4
5
6
7
8
// 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});
}

订阅者定义

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

订阅者为key为轨迹ID，value是一个Subscriber的一个vector。Subscriber由一个ros提供的订阅者和订阅的topic组合而成

1
2
3
4
5
6
7
8
9
struct Subscriber {
    ::ros::Subscriber subscriber;

    // ::ros::Subscriber::getTopic() does not necessarily return the same
    // std::string
    // it was given in its constructor. Since we rely on the topic name as the
    // unique identifier of a subscriber, we remember it ourselves.
    std::string topic;
  };

SubscribeWithHandler

SubscribeWithHandler返回ros的一个Subscriber

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
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
if (options.use_odometry) {
  subscribers_[trajectory_id].push_back(
      {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                trajectory_id, kOdometryTopic,
                                                &node_handle_, this),
       kOdometryTopic});
}

这里push_back的就是一个Subscriber的一个结构体

即就是std::unordered_map<int, std::vector<Subscriber>> subscribers_;这里的Subscriber

参数列表

1
2
3
4
5
6
7
8
9
10
11
/**
 * @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 订阅者
 */

注意，函数的形参中定义了一个函数指针

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

这个函数指针的返回值为void，指向Node类里面的一个函数，函数指针的名字为handler，接受三个参数。

再看参数是如何传入进来的

1
2
3
4
5
6
7
if (options.use_odometry) {
  subscribers_[trajectory_id].push_back(
      {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                trajectory_id, kOdometryTopic,
                                                &node_handle_, this),
       kOdometryTopic});
}

函数Node::HandleOdometryMessage是一个Node的一个函数，并且他的参数和函数指针所定义的保持一致

1
2
3
void Node::HandleMultiEchoLaserScanMessage(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg)

在这里的模板类型为nav_msgs::Odometry

注册回调

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
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);
          }));
}

对于topic的回调是通过node_handle->subscribe<MessageType>来实现的，topic为订阅的消息名字，缓冲区数据队列长度，回调函数。

按正常的ros格式来说，这里需要一个函数的名字。这里函数的类型为boost:function ,<>里面的是函数的返回值void和参数，后面的括号是函数里面的内容，在这里通过lambda表达式来进行一个函数的传递

lambda 表达式

lambda表达式是匿名的

1
[函数对象的参数 = &]()(->返回值类型){函数体}

[]是不可以少的，=表示这里的参数用值拷贝使用，&表示这里的参数是通过引用的方式进行使用

1
2
3
[node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }

前面[]表示这里需要使用的参数，是以值拷贝的方式来使用，()表示此函数需要用的参数

(node->*hander)表示函数指针的使用

 Comments