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();
...

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

ros 框架解析-订阅部分_第1张图片

Subscriber 相关类图

ros 框架解析-订阅部分_第2张图片

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;
}

ros 框架解析-订阅部分_第3张图片

然后建立连接的过程会在 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());
  }
}

ros 框架解析-订阅部分_第4张图片

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

通过 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 处理收到的整包的消息,并找到相应的回调信息

你可能感兴趣的:(C++,网络)