【ROS】源码分析-服务发现原理

ShadowWalker

已于 2022-06-19 15:18:46 修改

阅读量1.5k

点赞数 3

分类专栏： ROS 文章标签：服务发现自动驾驶 c++ ROS

于 2022-06-19 14:18:11 首次发布

本文链接：https://blog.csdn.net/lanmo555/article/details/125356148

版权

ROS 专栏收录该内容

3 篇文章 3 订阅

订阅专栏

说明

本文通过NodeHandle源码作为入口，来分析PubNode、SubNode、Master之间是如何发现彼此的。

源码目录

ros_comm/clients/roscpp: ROS Node和底层的RPC通讯协议实现，都是c++代码。
ros_comm/tools/rosmaster: ROS Master的功能代码，都是python代码。
ros_tutorials/turtlsim：小海龟的PubNode和SubNode。

源码分析

本文以小海龟代码开始

PubNode：turtle_teleop_key
SubNode：turtle_node

Topic发布

咱们从日常编码的入口开始（这个是小海龟turtle_teleop_key的相关代码）：

int main(int argc, char** argv)
{
    ros::init(argc, argv, "teleop_turtle");
    ros::NodeHandle nh_;  //创建NodeHandler
    ros::Publisher twist_pub_;
    twist_pub_ = nh_.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1);//声明/发布本node的topic
    twist_pub_.publish(twist); //发布消息到topic
}

那咱们就从NodeHandle开始，先看下advertise的实现

(源码文件：ros_comm/clients/roscpp/include/ros/node_handle.h)
    template <class M>
    Publisher advertise(const std::string& topic, uint32_t queue_size, bool latch = false)
    {
      AdvertiseOptions ops;
      ops.template init<M>(topic, queue_size);
      ops.latch = latch;
      return advertise(ops);
    }
    
(源码文件：ros_comm/clients/roscpp/src/libros/node_handle.cpp)
Publisher NodeHandle::advertise(AdvertiseOptions& ops){
   TopicManager::instance()->advertise(ops, callbacks)
}

advertise的核心代码在TopicManager中：通过master::execute("registerPublisher"向master注册自己和topic。

(源码文件：ros_comm/clients/roscpp/src/libros/topic_manager.cpp)
bool TopicManager::advertise(const AdvertiseOptions& ops, const SubscriberCallbacksPtr& callbacks) {
  XmlRpcValue args, result, payload;
  args[0] = this_node::getName();
  args[1] = ops.topic;
  args[2] = ops.datatype;
  args[3] = xmlrpc_manager_->getServerURI(); //本PubNode的URL地址
  master::execute("registerPublisher", args, result, payload, true);
  //后续对payload并没有做任何的解析处理
}

进入master.cpp进一步查看：

这里使用通讯层工具XmlRpc::XmlRpcClient向master-server发起网络请求，并获取到结果payload，所以master.cpp就是一个与master-server通讯的工具类。
从上文的Master服务启动分析得出Master使用python的SimpleXMLRPCServer启动了http服务器，以接受PubNode&SubNode的服务注册和参数服务处理rosmaster.master_api.ROSMasterHandler，这里是与之呼应连接的。
如何发现Master在master.cpp中的代码ros::getDefaultMasterURI()可以得知是约定：本机地址+默认端口号11311。

(源码文件：ros_comm/clients/roscpp/src/libros/master.cpp)
bool execute(const std::string& method, const XmlRpc::XmlRpcValue& request, XmlRpc::XmlRpcValue& response, XmlRpc::XmlRpcValue& payload, bool wait_for_master){

  std::string master_host = getHost();
  uint32_t master_port = getPort();
  XmlRpc::XmlRpcClient *c = XMLRPCManager::instance()->getXMLRPCClient(master_host, master_port, "/");
  bool printed = false;
  bool slept = false;
  bool ok = true;
  bool b = false;

  b = c->execute(method.c_str(), request, response);
  XMLRPCManager::instance()->validateXmlrpcResponse(method, response, payload)
}

(源码文件：ros_comm/clients/roscpp/src/libros/init.cpp)
const std::string& getDefaultMasterURI() {
  static const std::string uri = "http://localhost:11311";
  return uri;
}

然后看下一MasterNode中是如何registerPublisher的：

register_publisher: 记录pubNode信息：id、topic、pubisherServerURL
_notify_topic_subscribers : 通知订阅者该topic的pubisherServer的信息。

(源码文件：ros_comm/tools/rosmaster/src/rosmaster/master_api.py)
    @apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
    def registerPublisher(self, caller_id, topic, topic_type, caller_api):
        try:
            self.ps_lock.acquire()
            # 1.记录pubNode信息：id、topic、pubServerURL
            self.reg_manager.register_publisher(topic, caller_id, caller_api)
            # don't let '*' type squash valid typing
            if topic_type != rosgraph.names.ANYTYPE or not topic in self.topics_types:
                self.topics_types[topic] = topic_type
            pub_uris = self.publishers.get_apis(topic)
            sub_uris = self.subscribers.get_apis(topic)
            # 2.通知订阅者该topic的pubisherServer的信息
            self._notify_topic_subscribers(topic, pub_uris, sub_uris)
            mloginfo("+PUB [%s] %s %s",topic, caller_id, caller_api)
            # 3.返回订阅者(subscriber)的信息给发布者(publisher)
            sub_uris = self.subscribers.get_apis(topic)            
        finally:
            self.ps_lock.release()
        return 1, "Registered [%s] as publisher of [%s]"%(caller_id, topic), sub_uris

Topic订阅

还是从NodeHandle开始，先看下subscribe的实现

(源码文件：ros_comm/clients/roscpp/include/ros/node_handle.h)
  template<class M, class T>
  Subscriber subscribe(const std::string& topic, uint32_t queue_size, void(T::*fp)(M), T* obj, 
                       const TransportHints& transport_hints = TransportHints())
  {
    SubscribeOptions ops;
    ops.template initByFullCallbackType<M>(topic, queue_size, boost::bind(fp, obj, boost::placeholders::_1));
    ops.transport_hints = transport_hints;
    return subscribe(ops);
  }
  
(源码文件：ros_comm/clients/roscpp/src/libros/node_handle.cpp)
Subscriber NodeHandle::subscribe(SubscribeOptions& ops) {
   TopicManager::instance()->subscribe(ops)
}

subscribe的核心代码也是在TopicManager中：通过master::execute("registerSubscriber"向master注册自己和topic，master::execute具体实现与上面的master::execute("registerPublisher"是完全一样的（使用通讯层工具XMLRPCManager向master-server发起网络请求，并获取到结果payload）。差异的地方是subscriber对于master返回的结果payload进行了解析，从而得知该topic的publisher的serverURL地址。

(源码文件：ros_comm/clients/roscpp/src/libros/topic_manager.cpp)
bool TopicManager::subscribe(const SubscribeOptions& ops) {
    registerSubscriber(s, ops.datatype)
}
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();//本SubNode的URL地址
  
  //想master注册subNode，并获取pubServerURL
  master::execute("registerSubscriber", args, result, payload, true));
  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]));
    }
  }
  s->pubUpdate(pub_uris);
}

然后看下一MasterNode中是如何registerSubscriber的：

register_subscriber: 记录subNode信息：id、topic、pubisherServerURL
pub_uris: 返回发布者(publisher)的信息给订阅者(subscriber)

@apivalidate([], ( is_topic('topic'), valid_type_name('topic_type'), is_api('caller_api')))
    def registerSubscriber(self, caller_id, topic, topic_type, caller_api):
        try:
            self.ps_lock.acquire()
            # 1.记录subNode信息：id、topic、subServerURL
            self.reg_manager.register_subscriber(topic, caller_id, caller_api)
            if not topic in self.topics_types and topic_type != rosgraph.names.ANYTYPE:
                self.topics_types[topic] = topic_type
            mloginfo("+SUB [%s] %s %s",topic, caller_id, caller_api)
            # 2.返回发布者(publisher)的信息给订阅者(subscriber)
            pub_uris = self.publishers.get_apis(topic)
        finally:
            self.ps_lock.release()
        return 1, "Subscribed to [%s]"%topic, pub_uris

结论汇总

publisher和subscriber向master注册实现逻辑都是在TopicManager(源码文件：ros_comm/clients/roscpp/src/libros/topic_manager.cpp)中。
与master通讯工具类是master::execute(maser.cpp)，底层的RPC通讯框架是XmlRpc::XmlRpcClient(源码文件：ros_comm/xmlrpcpp/src/XmlRpcClient.cpp)，数据协议是XML，网络协议是HTTP。
服务发现过程：通过向master注册自己的ServerURL
- Publisher注册： master记录id、topic、publisherServerURL，并通知Subscriber该topic的publisherServerURL信息。
- Subscriber注册：master记录id、topic、subscriberServerURL，并主动获取该topic的publisherServerURL信息。

在这里插入图片描述