ros 框架解析-订阅部分

博客：http://www.coolsite.top/archives/334

前言

本文从 roscpp 源码的角度解析 ros 框架的订阅部分，目的是分析订阅相关的各个类的关系和作用，从而熟悉 ros 订阅功能底层调用过程，和 ros 节点之间的通信方式。

源码地址是： https://github.com/ros/ros_comm

Subscriber 对象创建的过程是怎样的？

...
ros::init();
ros::NodeHandle n;
Subscriber subscriber = n.subscribe("topic", queue_size, callback); // ??? 我干了什么 ???
ros::spin();
...

先来看订阅操作相关的主要的一些类：

Subscriber 相关类图

Subscription 相关类图

NodeHandle：节点句柄类，提供生成 Subscriber、Publisher 等对象的接口，并管理其生命期，用来与其他节点通信。

Subscriber：订阅器类，用来管理单个 topic 的订阅回调任务，对象由 NodeHandle 创建。

SubscriptionCallbackHelper：用来管理单个订阅回调，为不同的回调函数和参数提供统一抽象。

TopicManager：话题管理类，管理该节点所有的 topic 相关的订阅、发布信息，并与 master 通信。

Subscription：订阅类，用来管理单个 topic 的相关信息和该 topic 相关的发布者连接信息。

XMLRPCManager：XMLRPC 管理类，在单独线程中运行，管理节点的 XMLRPC 连接。

PublisherLink：抽象类，描述某话题的订阅节点所属的发布节点连接，负责接收该发布节点的消息。

TransportPublisherLink：PubslicationLink 子类，表示 Transport 类型的发布节点连接，与该节点通过网络进行通信。

IntraProcessPublisherLink：PubslicationLink 子类，表示该发布节点与订阅节点同一进程，消息通过进程内传输。

Transport：传输抽象类，用来抽象不同的传输层协议（TCP/UDP）。

TransportTCP：TCP 通信类，为 Transport 子类，封装 TCP 协议的操作系统接口，管理单个 socket fd。

PollSet：封装 poll 来管理 socket ，poll 由跨平台实现，linux 下为 poll/epoll ，所有接口线程安全。

Connection：连接类，用来抽象节点之间的连接，不依赖传输层协议类型，为上层提供方便的读写接口。

CallbackQueue：全局回调队列，保存所有接收到的消息与其相应的回调。

---

内部函数调用流程如下（去除了一些细节，保留主要流程）：

格式统一为 类名->方法 描述

• NodeHandle->subscribe 生成 Subscriber 对象
  • SubscribeOptions->initByFullCallbackType 根据入参生成话题相关的回调信息 (话题名、队列长度、回调函数)
  • TopicManager->subscribe 订阅操作，生成该 topic 的 Subscription 对象，保存到 TopicManager 中
    • Subscription->addCallback 将 topic 的回调信息加入该 Subscription 对象中
    • TopicManager->registerSubscribe 注册订阅信息
      • master::execute 通知 master 该节点信息和订阅信息，并拿到该 topic 所有相关的发布者的 uri 地址
      • Subscription->pubUpdate 根据拿到的发布者 uri，更新该话题的发布者信息，断开旧的不存在连接，建立新连接
        • Subscription->negotiateConnection 与发布节点通过 xmlrpc 连接进行协商，跳过和本节点相同地址的发布节点（自连接）
          • XmlRpcClient->executeNonBlock 向发布节点异步发送 xmlrpc 请求
          • XMLRPCManager->addAsyncConnection 异步建立与发布节点的 xmlrpc 连接
    • Subscription->addLocalConnection 如果发布节点在本地，则添加 IntraProcessPublisherLink 对象

挑一段源码看看（clients/roscpp/src/libros/subscription.cpp）：

//该方法是订阅节点主动请求master，注册信息并获取发布节点信息，与之建立连接
bool TopicManager::registerSubscriber(const SubscriptionPtr& s, const string &datatype)
{
  //填入相关请求参数
  XmlRpcValue args, result, payload;
  args[0] = this_node::getName();
  args[1] = s->getName();
  args[2] = datatype;
  args[3] = xmlrpc_manager_->getServerURI();

  // 向master发送注册节点请求
  if (!master::execute("registerSubscriber", args, result, payload, true))
  {
    return false;
  }

  // 解析master响应的消息，内容是该 topic 相关的发布节点uri
  vector<string> pub_uris;
  for (int i = 0; i < payload.size(); i++)
  {
    if (payload[i] != xmlrpc_manager_->getServerURI())
    {
      pub_uris.push_back(string(payload[i]));
    }
  }

  bool self_subscribed = false;
  PublicationPtr pub;
  const std::string& sub_md5sum = s->md5sum();
  // Figure out if we have a local publisher
  // 找出哪些节点是本地节点，这部分节点无需建立连接，无需网络通信
  {
    boost::recursive_mutex::scoped_lock lock(advertised_topics_mutex_);
    V_Publication::const_iterator it = advertised_topics_.begin();
    V_Publication::const_iterator end = advertised_topics_.end();
    for (; it != end; ++it)
    {
      pub = *it;
      const std::string& pub_md5sum = pub->getMD5Sum();

      if (pub->getName() == s->getName() && !pub->isDropped())
	{
      // 消息冲突！ md5sum 来自该topic的对应消息的 md5
	  if (!md5sumsMatch(pub_md5sum, sub_md5sum))
	    {
	      ROS_ERROR("md5sum mismatch making local subscription to topic %s.",
			s->getName().c_str());
	      ROS_ERROR("Subscriber expects type %s, md5sum %s",
			s->datatype().c_str(), s->md5sum().c_str());
	      ROS_ERROR("Publisher provides type %s, md5sum %s",
			pub->getDataType().c_str(), pub->getMD5Sum().c_str());
	      return false;
	    }

	  self_subscribed = true;
	  break;
	}
    }
  }

  // 更新发布节点信息，并与新节点建立连接，因为这里s是刚创建，所以 pub_urls 都是新节点
  s->pubUpdate(pub_uris);
  if (self_subscribed)
  {
    // 发布节点在本地，无需建立连接
    s->addLocalConnection(pub);
  }

  return true;
}

然后建立连接的过程会在 XMLRPCManager 线程中进行：

• XMLRPCManager->serverThreadFunc 在单独线程中处理 xmlrpc 连接
  • PendingConnection->check 检查该 xmlrpc 连接是否成功
    • Subscription->pendingConnectionDone 与发布节点的 xmlrpc 连接建立成功，把临时连接 PendingConnection 删除
      • XMLRPCManager->validateXmlrpcResponse 验证该节点与与该发布节点协商是否成功，并拿到回复
      • TransportTCP->connect 生成 TransportTCP 对象，与发布节点建立 TCP 连接（或者 UDP 连接，取决于 master）
        • socket 生成套接字
        • setNonBlock 设置非阻塞套接字
        • getaddrinfo 解析域名
        • connect 建立连接
        • TransportTCP->initializeSocket
          • setKeepAlive
          • PollSet->addSocket 把 socket 注册到 poll 中，并绑定回调事件
          • PollSet->addEvent 注册 POLLRDHUP 事件，监测连接关闭事件
      • Connection->initialize 初始化 Connection 对象，传入 TransportTCP 对象
        • 绑定具体的可读、可写等事件 onReadable、onWritable 等到 Transport
      • TransportPublisherLink->initialize 初始化 PublisherLink 对象，传入该 Connection 对象
        • 绑定读写消息头事件到 Connection，为了分包
      • ConnectionManager->addConnection 该 Connection 对象加入管理器
      • Subscription->addPublisherLink 把该 PublisherLink 对象加入本对象（Subscription）

// 该函数在订阅节点与发布节点的 xmlrpc 连接建立完成之后，会调用
void Subscription::pendingConnectionDone(const PendingConnectionPtr& conn, XmlRpcValue& result)
{
  boost::mutex::scoped_lock lock(shutdown_mutex_);
  if (shutting_down_ || dropped_)
  {
    return;
  }

  {
    // 线程安全，pending_connections_ 会被主线程、xmlrpc线程操作
    boost::mutex::scoped_lock pending_connections_lock(pending_connections_mutex_);
    pending_connections_.erase(conn);
  }

  TransportUDPPtr udp_transport;

  std::string peer_host = conn->getClient()->getHost();
  uint32_t peer_port = conn->getClient()->getPort();
  std::stringstream ss;
  ss << "http://" << peer_host << ":" << peer_port << "/";
  std::string xmlrpc_uri = ss.str();
  udp_transport = conn->getUDPTransport();

  XmlRpc::XmlRpcValue proto;
  // 验证 xmlrpc 的 requestTopic 回复是否有效
  if(!XMLRPCManager::instance()->validateXmlrpcResponse("requestTopic", result, proto))
  {
  	ROSCPP_LOG_DEBUG("Failed to contact publisher [%s:%d] for topic [%s]",
              peer_host.c_str(), peer_port, name_.c_str());
  	closeTransport(udp_transport);
  	return;
  }

  // 验证回复的消息是否合法
  if (proto.size() == 0)
  {
  	ROSCPP_LOG_DEBUG("Couldn't agree on any common protocols with [%s] for topic [%s]", xmlrpc_uri.c_str(), name_.c_str());
  	closeTransport(udp_transport);
  	return;
  }

  if (proto.getType() != XmlRpcValue::TypeArray)
  {
  	ROSCPP_LOG_DEBUG("Available protocol info returned from %s is not a list.", xmlrpc_uri.c_str());
  	closeTransport(udp_transport);
  	return;
  }
  if (proto[0].getType() != XmlRpcValue::TypeString)
  {
  	ROSCPP_LOG_DEBUG("Available protocol info list doesn't have a string as its first element.");
  	closeTransport(udp_transport);
  	return;
  }

  // TCP连接
  std::string proto_name = proto[0];
  if (proto_name == "TCPROS")
  {
    if (proto.size() != 3 ||
        proto[1].getType() != XmlRpcValue::TypeString ||
        proto[2].getType() != XmlRpcValue::TypeInt)
    {
    	ROSCPP_LOG_DEBUG("publisher implements TCPROS, but the " \
                "parameters aren't string,int");
      return;
    }
    
    //拿到发布节点的地址信息
    std::string pub_host = proto[1];
    int pub_port = proto[2];
    ROSCPP_CONN_LOG_DEBUG("Connecting via tcpros to topic [%s] at host [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);

    //创建新的socket，并注册到 PollManager
    TransportTCPPtr transport(boost::make_shared<TransportTCP>(&PollManager::instance()->getPollSet()));
    // 建立socket连接
    if (transport->connect(pub_host, pub_port))
    {
      ConnectionPtr connection(boost::make_shared<Connection>());
      TransportPublisherLinkPtr pub_link(boost::make_shared<TransportPublisherLink>(shared_from_this(), xmlrpc_uri, transport_hints_));

      // 初始化连接，注册事件回调
      connection->initialize(transport, false, HeaderReceivedFunc());
      pub_link->initialize(connection);
		
      ConnectionManager::instance()->addConnection(connection);

      boost::mutex::scoped_lock lock(publisher_links_mutex_);
      addPublisherLink(pub_link);

      ROSCPP_CONN_LOG_DEBUG("Connected to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
    }
    else
    {
      ROSCPP_CONN_LOG_DEBUG("Failed to connect to publisher of topic [%s] at [%s:%d]", name_.c_str(), pub_host.c_str(), pub_port);
    }
  }
  else if (proto_name == "UDPROS")
  {
    //忽略UDP
  }
  else
  {
  	ROSCPP_LOG_DEBUG("Publisher offered unsupported transport [%s]", proto_name.c_str());
  }
}

至此订阅节点与发布节点连接便建立完成。

通过 NodeHandle 的 subscribe 接口生成 Subscriber 对象，实际上主要是订阅节点与发布节点建立网络连接的过程，首先由 TopicManager 生成 Subscription 管理该 topic 信息，其次通过 xmlrpc 通信把当前节点信息和 topic 信息告知 master 节点，同步会接收到 master 发来的该 topic 相关的所有发布节点信息，根据这些信息来更新该 topic 所属的 Subscription 对象管理的发布节点信息。如果旧的发布节点失效，则断开，如果有新的发布节点，则与其建立网络连接。

订阅节点与发布节点建立连接之前有一个协商的过程，是通过发送 xmlrpc 的 “RequestTopic” 请求，如果其回复协商成功，则调用 TransportTCP/TransportUDP 提供的 API 接口与其建立连接，并生成 Connection 和 TransportPublisherLink 对象，用来描述新的连接和新的发布节点。协商的过程是异步的，也就是说订阅器 Subscriber 初始化好之后并不一定会马上与发布节点建立连接，而是会在 XMLRPCManager 线程中去异步执行，直到协商完成才去建立连接。

建立连接的过程是同步的，connect 系统调用前 socket 为阻塞 fd，当 connect 返回连接建立成功后才将 fd 设置为非阻塞，Transport 会调用 PollSet 提供的方法将该 socket 注册到 poll 中，因为 poll 是在一个单独的线程中运行，所以之后的网络事件都在该线程中处理。

如果是本地连接，也就是订阅节点和发布节点同属一个进程，则无需建立连接，也无需序列化，直接通过内存共享该消息，该实现在 IntroProcessPublisherLink。

subscriber 怎么收到消息并触发回调的？

void callback(const message::ConstPtr& msg) {
    do something... // ??? msg 哪来的 ???
}

收消息的流程如下（主要的函数调用流程，省略了一些细节）：

• PollManager->threadFunc 在单独线程中处理网络事件
  • PollSet->update 等待并处理到来的事件
    • io::poll_socket 调用跨平台的 poll 接口等待网络消息，linux 下为 poll/epoll
    • Transport->socketUpdate 根据消息事件的不同类型，分发事件
      • Connection->onReadable 收消息是属于可读事件，处理该事件
        • Connection->readTransport
          • Transport->read 读网络事件，保存到 buffer
            • read 系统调用
          • Connection->onHeaderLengthRead 读取消息头长度
          • Connection->onHeaderRead 读取消息头
            • TransportPublisherLink->onHeaderReceived 解析消息头
            • TransportPublisherLink->onMessageLength 读取消息长度
            • TransportPublisherLink->onMessage 处理消息，根据消息大小分包
              • TransportPublisherLink->handleMessage 把消息转交给该 topic 所属的订阅类
                • Subscription->handleMessage 处理收到的整包的消息，并找到相应的回调信息