ROS初始化分析

ROS通信模型

ROS节点构成了一个计算图(computation graph)，它是一个p2p的网络结构，节点之间的拓扑结构为mesh（网状）。ROS节点之间的通讯中，节点通过socket来实现建立连接，传输数据。ROS与master节点的通信则通过远程过程调用实现。ROS 中远程过程调用采用XML-RPC 实现。远程调用负责管理节点对计算图中信息的获取与更改，还有一些全局的设置， 但RPC不直接支持数据的流传输。数据的传输是通过 TCPROS与 UDPROS实现的。

每个ROS节点运行一个XML-RPC server，ROS的通讯模块在ros_comm中。整个通信模型如下：

上图中蓝线表示XML-RPC通信，黑线表示socket通信。每个ROS节点都拥有IP地址、端口号和XML-RPC服务器，IP地址和端口号用来标识不同的节点。如果ROS节点在同一台主机上，那么IP地址可能是相同的。

发布者节点通过一个名为registerPublisher的远程调用(XML-RPC)，注册它们的话题和消息类型到主节点。同样地，订阅者通过registerSubscriber远程调用，注册到主节点，这样主节点就获知了双方的URI、话题名称、消息类型。

订阅者注册时，主节点会回复注册同一话题的所有发布者，然后订阅者就可以通过socket直接与发布者通信，通信基于TCPROS协议传输消息，这就跟主节点无关了。当又有新的发布者注册同一个话题到主节点时，主节点执行publisherUpdate()给订阅者，回复一个新的发布者列表。

发布者和订阅者通信是基于TCPROS协议，过程与TCP三次握手类似。订阅者与发布者建立第一次连接，订阅者传输topic信息给发布者，然后再根据发布者返回的topic信息，建立第二次连接，发布者开始传输具体的数据。通信过程使用socket实现。

ros::init()

ros::init有三种重载形式，不过最后都会调用最简单的一种形式。

void init(const M_string& remappings, const std::string& name, uint32_t options)
{
  if (!g_atexit_registered)
  {
    g_atexit_registered = true;   // 是否已注册终止函数
    // 注册终止函数，在调用exit()或终止main函数时调用
    atexit(atexitCallback);
  }

  if (!g_global_queue)
  {
    g_global_queue.reset(new CallbackQueue);
  }
  //上面做了一些预处理，主要部分在下面：
  if (!g_initialized)   // 若未初始化
  {
    g_init_options = options;
    g_ok = true;

    ROSCONSOLE_AUTOINIT; //在console.h中的一段宏定义：Initializes the rosconsole library. 
    // Disable SIGPIPE
#ifndef WIN32     // 如果不是windows系统，则执行
    signal(SIGPIPE, SIG_IGN);
#endif
    // 重点是5个init
    network::init(remappings);//初始化网络,实现在network.cpp中
    master::init(remappings); //初始化master节点
    // name namespace is initialized by this_node
    this_node::init(name, remappings, options); //初始化当前节点
    file_log::init(remappings); //初始化日志文件
    param::init(remappings); //初始化ROS参数服务器
    g_initialized = true; //置上初始化标记
  }
}

g_global_queue的定义是CallbackQueuePtr g_global_queue，是一个boost共享指针：typedef boost::shared_ptr<CallbackQueue> CallbackQueuePtr。

network::init()

network::init函数主要就是给g_host和g_tcpros_server_port赋值。

void init(const M_string& remappings) //该函数在init.cpp中被调用
{
  //模块1
  M_string::const_iterator it = remappings.find("__hostname");
  if (it != remappings.end())
  {
    g_host = it->second;
  }
  else
  {
    it = remappings.find("__ip");
    if (it != remappings.end())
    {
      g_host = it->second;
    }
  }
  //模块2
  it = remappings.find("__tcpros_server_port");
  if (it != remappings.end())
  {
    try
    { // 尝试将对应元素的值（std::string）转化成uint16_t类型，g_tcpros_server_port初始值为0
      g_tcpros_server_port = boost::lexical_cast<uint16_t>(it->second);
    }
    catch (boost::bad_lexical_cast & )        // 如果上述类型转化发生异常，捕捉
    {
      throw ros::InvalidPortException("__tcpros_server_port [" + it->second + "] was not specified as a number within the 0-65535 range");
    }
  }
  //模块3，如果上面未能赋值，调用函数determineHost赋值
  if (g_host.empty())
  {
    g_host = determineHost();
  }
}

boost::lexical_cast用于string和数值之间的转换比如：将一个字符串”712”转换成整数712。

最后，如果之前未能给g_host成功赋值，会调用determineHost函数，它依次获取环境变量ROS_HOSTNAME和ROS_IP。如果没有设置这两个环境变量，就调用gethostname函数获取主机名，但不能取localhost。如果这样也不能获取到，会返回127.0.0.1。

master::init()

master::init处理master节点。

void init(const M_string& remappings)
{
  M_string::const_iterator it = remappings.find("__master");
  if (it != remappings.end())
  {
    g_uri = it->second;
  }
  //如果g_uri没有被赋值（即刚刚没找到相应节点）
  if (g_uri.empty())
  {
    char *master_uri_env = NULL;
    #ifdef _MSC_VER     // 根据编译器使用不同函数
      _dupenv_s(&master_uri_env, NULL, "ROS_MASTER_URI");
    #else
      master_uri_env = getenv("ROS_MASTER_URI");
    #endif

    if (!master_uri_env)
    {
      ROS_FATAL( "ROS_MASTER_URI is not defined in the environment. Either " \
                 "type the following or (preferrably) add this to your " \
                 "~/.bashrc file in order set up your " \
                 "local machine as a ROS master:\n\n" \
                 "export ROS_MASTER_URI=http://localhost:11311\n\n" \
                 "then, type 'roscore' in another shell to actually launch " \
                 "the master program.");
      ROS_BREAK();
    }
    g_uri = master_uri_env;

#ifdef _MSC_VER
    free(master_uri_env);
#endif
  }

  //对g_uri进行解析，把g_uri中去掉协议部分赋值给g_host，并将端口赋值给g_port。
  if (!network::splitURI(g_uri, g_host, g_port))
  {
    ROS_FATAL( "Couldn't parse the master URI [%s] into a host:port pair.", g_uri.c_str());
    ROS_BREAK();
  }
}

如果没有获得环境变量ROS_MASTER_URI，那么节点报错退出。最后把值赋给g_uri，splitURI函数是一个字符串的处理函数。 该函数使得rosmaster能够在系统中查找到节点，并且能够获得IP与端口号进行通信。rosmaster掌握了各节点的文本信息，包括名称、话题名，以及它们的端口号、URL，用线程池收集，用字典保存，开启XML-RPC服务器进行话题的传送。

this_node::init()

this_node::init初始化name命令空间。

void init(const std::string& name, const M_string& remappings, uint32_t options)
{
  ThisNode::instance().init(name, remappings, options);
}
// ThisNode是一个类，instance()是单例模式
static ThisNode& instance()
{
	static ThisNode singleton;
	return singleton;
}

// 主要实现在这里
void ThisNode::init(const std::string& name, const M_string& remappings, uint32_t options)
{
  char *ns_env = NULL;
#ifdef _MSC_VER
  _dupenv_s(&ns_env, NULL, "ROS_NAMESPACE");
#else
  ns_env = getenv("ROS_NAMESPACE");
#endif

  if (ns_env)	//如果获得环境变量
  {
    namespace_ = ns_env;    // namespace_是成员变量
#ifdef _MSC_VER
    free(ns_env);
#endif
  }
  if (name.empty()) {	// 节点名不能为空
    throw InvalidNameException("The node name must not be empty");
  }

  name_ = name;     // 节点名赋值，name_也是类成员变量，初始值为"empty"

  bool disable_anon = false;
  //在输入参数remappings查找键为"__name"的项
  M_string::const_iterator it = remappings.find("__name");
  if (it != remappings.end())
  {
    name_ = it->second;
    disable_anon = true;
  }
  //在输入参数remappings查找键为"__ns"的项
  it = remappings.find("__ns");
  if (it != remappings.end())
  {
    namespace_ = it->second;
  }
  // 这里可以看出ROS_NAMESPACE不是必要的
  if (namespace_.empty())
  {
    namespace_ = "/";
  }
  // 如果在上面赋值为 / ，最终就是 /，否则就是 /+namespace
  namespace_ = (namespace_ == "/")
    ? std::string("/") 
    : ("/" + namespace_)
    ;

  std::string error;
  // 对命名空间进行验证，这是个字符串处理函数
  // 检查首字符，首字符只能是~ / 或 alpha，逐个检查name中的每个字符是否为合法字符
  if (!names::validate(namespace_, error))
  {
    std::stringstream ss;
    ss << "Namespace [" << namespace_ << "] is invalid: " << error;
    throw InvalidNameException(ss.str());
  }

  // names must be initialized here, because it requires the namespace to already be known so that it can properly resolve names.
  // It must be done before we resolve g_name, because otherwise the name will not get remapped.
  // 将remappings映射为g_remappings和g_unresolved_remappings两个变量
  names::init(remappings);
  // 节点名不能含有 / 和 ~
  if (name_.find("/") != std::string::npos)
  {
    throw InvalidNodeNameException(name_, "node names cannot contain /");
  }
  if (name_.find("~") != std::string::npos)
  {
    throw InvalidNodeNameException(name_, "node names cannot contain ~");
  }
  // 简单的格式化操作
  name_ = names::resolve(namespace_, name_);
  // 如果初始化时的options选择的时匿名节点，那么在节点名后加时间戳，单位是纳秒
  if (options & init_options::AnonymousName && !disable_anon)
  {
    char buf[200];
    // ros::WallTime::now()返回当前时间的纳秒表示
    snprintf(buf, sizeof(buf), "_%llu", (unsigned long long)WallTime::now().toNSec());
    name_ += buf;
  }
  //把节点和名字联系起来
  ros::console::setFixedFilterToken("node", name_);
}

这个函数主要是对节点的名字进行了注册，在name这个命名空间内有一组映射g_remappings，包含了所有注册的节点名。

file_log::init()

file_log::init利用节点的名称来创造日志文件，确定日志存放目录和日志名称。

void init(const M_string& remappings)
{
  std::string log_file_name;
  M_string::const_iterator it = remappings.find("__log");
  //在remappings中找到键为"__log"的项
  if (it != remappings.end())
  {
    log_file_name = it->second;		//如果找到了，将对应的值赋值给log_file_name
  }

  {
    // Log filename can be specified on the command line through __log
    // If it's been set, don't create our own name
    if (log_file_name.empty())
    {
      // Setup the logfile appender
      // Can't do this in rosconsole because the node name is not known
      pid_t pid = getpid();		//获取当前进程号
      std::string ros_log_env;
      //获取"ROS_LOG_DIR"
      if ( get_environment_variable(ros_log_env, "ROS_LOG_DIR"))
      {
        log_file_name = ros_log_env + std::string("/");
      }
      else		//如果不存在"ROS_LOG_DIR"这个环境变量
      {			//获取"ROS_HOME"的环境变量值
        if ( get_environment_variable(ros_log_env, "ROS_HOME"))
        {
          log_file_name = ros_log_env + std::string("/log/");
        }
        else		//如果不存在环境变量"ROS_HOME"
        {
          // 如果没有设置以上环境变量，日志最终放在 ~/.ros/log
          if( get_environment_variable(ros_log_env, "HOME") )
          {
            std::string dotros = ros_log_env + std::string("/.ros/");
            fs::create_directory(dotros);
            log_file_name = dotros + "log/";
            fs::create_directory(log_file_name);
          }
        }
      }

      // log_file_name是完整路径，这里是取 文件名=节点名_
      for (size_t i = 1; i < this_node::getName().length(); i++)
      {
        if (!isalnum(this_node::getName()[i]))
        {
          log_file_name += '_';
        }
        else
        {
          log_file_name += this_node::getName()[i];
        }
      }
      // 变成了  节点名_pid_log
      char pid_str[100];
      snprintf(pid_str, sizeof(pid_str), "%d", pid);  //将pid以整形变量的形式写入pid_str
      log_file_name += std::string("_") + std::string(pid_str) + std::string(".log");
    }
    // 赋值
    log_file_name = fs::system_complete(log_file_name).string();
    g_log_directory = fs::path(log_file_name).parent_path().string();
  }
}

param::init()

param::init初始化ROS参数服务器。

void init(const M_string& remappings)
{
  M_string::const_iterator it = remappings.begin();
  M_string::const_iterator end = remappings.end();
  for (; it != end; ++it)//依次遍历remappings变量的所有元素
  {
    const std::string& name = it->first;//提取键
    const std::string& param = it->second;//提取值

    if (name.size() < 2)    //跳过键的长度小于2的元素
    {
      continue;
    }
    if (name[0] == '_' && name[1] != '_')//如果键以“__”开头
    {
      //为name赋予一个本地名称，用符号"~"代替“__”
      std::string local_name = "~" + name.substr(1);
      bool success = false;

      try
      {
        int32_t i = boost::lexical_cast<int32_t>(param);    //尝试将param转化成整型
        //将local_name规整化
        ros::param::set(names::resolve(local_name), i);
        success = true;//将成功标志置上
      }
      catch (boost::bad_lexical_cast&)
      {

      }
      if (success)    //如果成功标志已被置上，则越过后续过程
      {
        continue;     //此时，即param成功被转化为整型
      }

      try
      {
        //没能转化为整型，尝试将param转化成浮点型
        double d = boost::lexical_cast<double>(param);
        //将local_name规整化
        ros::param::set(names::resolve(local_name), d);
        success = true;   //将成功标志置上
      }
      catch (boost::bad_lexical_cast&)
      {

      }

      if (success)    //如果成功标志已被置上，则越过后续过程
      {
        continue;    //此时，即param成功被转化为浮点型
      }
      // 处理param为布尔型或其他的情况
      if (param == "true" || param == "True" || param == "TRUE")
      {
        ros::param::set(names::resolve(local_name), true);
      }
      else if (param == "false" || param == "False" || param == "FALSE")
      {
        ros::param::set(names::resolve(local_name), false);
      }
      else
      {
        ros::param::set(names::resolve(local_name), param);
      }
    }
  }

  XMLRPCManager::instance()->bind("paramUpdate", paramUpdateCallback);
}

Parameter是ROS系统运行所定义的全局变量，它由master节点的parameter server基于XML-RPC进行维护。它是全局可见的，因此可以运行时进行修改。

ros::NodeHandle()

ros::NodeHandle的构造函数会调用ros::start()。当最后一个ros::NodeHandle销毁时，将调用ros::shutdown()。如果想自定义节点生存期，可以用这两个函数。检查关闭节点的两种方法是ros::ok()和ros::isShuttingDown()。

NodeHandle类是非常重要的一个类，可以说是ROS程序的核心，发布、订阅就是它完成的。NodeHandle类中有几个私有成员变量：

std::string namespace_;
// 回调接口指针，主要用于advertise, subscribe, advertiseService和createTimer函数
CallbackQueueInterface* callback_queue_;
NodeHandleBackingCollection* collection_;

NodeHandle构造函数：

NodeHandle::NodeHandle(const std::string& ns, const M_string& remappings)
  : namespace_(this_node::getNamespace())
  , callback_queue_(0)	// 空指针
  , collection_(0)		// 空指针
{
  std::string 　tilde_resolved_ns;
  // 如果给句柄命名并以'~'开头，它就将节点名加上'/'加在话题名之前
  if (!ns.empty() && ns[0] == '~')
    tilde_resolved_ns = names::resolve(ns);
  else
    tilde_resolved_ns = ns;

  construct(tilde_resolved_ns, true);
  //remappings所对应的是一个维护节点名称字符串的哈希表
  initRemappings(remappings);
}

construct函数：

void NodeHandle::construct(const std::string& ns, bool validate_name)
{
  if (!ros::isInitialized())     // 若没有调用 ros::init，报错: 必须先init()然后创建NodeHandle
  {
    ROS_FATAL("You must call ros::init() before creating the first NodeHandle");
    ROS_BREAK();
  }
  collection_ = new NodeHandleBackingCollection;
  unresolved_namespace_ = ns;
  // if callback_queue_ is nonnull, we are in a non-nullary constructor

  if (validate_name)
    namespace_ = resolveName(ns, true);
  else
      namespace_ = resolveName(ns, true, no_validate());
    
  ok_ = true;
  boost::mutex::scoped_lock    lock(g_nh_refcount_mutex);
   //此参数为全局变量，初始为0
  if (g_nh_refcount == 0 && !ros::isStarted())
  {
    g_node_started_by_nh = true;
    ros::start();	// 启动ros
  }
  ++g_nh_refcount;
}

若此时没有调用ros::init，报严重错误然后终止，所以 ros::init()必须在NodeHandle创建之前。然后创建NodeHandleBackingCollection指针。接下来是一些赋值，g_nh_refcount为初始为0的全局变量，即全局引用计数，如果此时没有启动ros，将调用ros::start()，然后将g_nh_refcount加１。

ros::start()比较复杂，它是ROS架构的核心，也是理解ROS源码的核心。

NodeHandle类的析构函数只调用了destruct():

void NodeHandle::destruct()
{
  delete collection_;

  boost::mutex::scoped_lock lock(g_nh_refcount_mutex);

  --g_nh_refcount;

  if (g_nh_refcount == 0 && g_node_started_by_nh)
  {
    ros::shutdown();
  }
}

析构函数很简单，释放collection_，引用计数减１，如果变成了０，就关闭ROS。

ros::start()

ros::start()函数位于init.cpp中，它初始化了ROS中重要的管理者，这些管理者均为单例模式：

TopicManager::instance()->start();
ServiceManager::instance()->start();
ConnectionManager::instance()->start();
PollManager::instance()->start();
XMLRPCManager::instance()->start();

TopicManager的start函数从其他三个类中得到实例，建立进程间通信需要这三个类的协助。

void TopicManager::start()
{
  boost::mutex::scoped_lock shutdown_lock(shutting_down_mutex_);
  shutting_down_ = false;
 
  poll_manager_ = PollManager::instance();//负责处理发布消息的缓冲队列中的消息
  connection_manager_ = ConnectionManager::instance();//负责建立进程间的网络通信连接
  xmlrpc_manager_ = XMLRPCManager::instance();//负责通知rosmaster
 
  xmlrpc_manager_->bind("publisherUpdate", boost::bind(&TopicManager::pubUpdateCallback, this, _1, _2));
  xmlrpc_manager_->bind("requestTopic", boost::bind(&TopicManager::requestTopicCallback, this, _1, _2));
  xmlrpc_manager_->bind("getBusStats", boost::bind(&TopicManager::getBusStatsCallback, this, _1, _2));
  xmlrpc_manager_->bind("getBusInfo", boost::bind(&TopicManager::getBusInfoCallback, this, _1, _2));
  xmlrpc_manager_->bind("getSubscriptions", boost::bind(&TopicManager::getSubscriptionsCallback, this, _1, _2));
  xmlrpc_manager_->bind("getPublications", boost::bind(&TopicManager::getPublicationsCallback, this, _1, _2));
}

ConnectionManager位于connection_manager.cpp，它实实在在地执行了进程间TCP的通信，是真正的执行者。

void ConnectionManager::start()
{
  poll_manager_ = PollManager::instance();
  poll_conn_ = poll_manager_->addPollThreadListener(boost::bind(&ConnectionManager::removeDroppedConnections, 
								this));
 
  // Bring up the TCP listener socket
  tcpserver_transport_ = boost::make_shared<TransportTCP>(&poll_manager_->getPollSet());
  // 在这个函数中将调用socket中的listen函数开始监听TCP连接请求
  if (!tcpserver_transport_->listen(network::getTCPROSPort(), 
				    MAX_TCPROS_CONN_QUEUE, 
				    boost::bind(&ConnectionManager::tcprosAcceptConnection, this, _1)))
  {
    ROS_FATAL("Listen on port [%d] failed", network::getTCPROSPort());
    ROS_BREAK();
  }
 
  // Bring up the UDP listener socket
  udpserver_transport_ = boost::make_shared<TransportUDP>(&poll_manager_->getPollSet());
  if (!udpserver_transport_->createIncoming(0, true))
  {
    ROS_FATAL("Listen failed");
    ROS_BREAK();
  }
}

PollManager是用来管理发布器的消息发布的，所利用的是信号与槽的机制，在publish部分使用。

void PollManager::start()
{
  shutting_down_ = false;
  thread_ = boost::thread(&PollManager::threadFunc, this);
}

XMLRPCmanager的start函数中有一个循环在不断轮转，如下所示，它不断地将新注册的话题与发布器信息添加到处理队列中，而xmlrpc存在一个处理器，也在不断发布处理队列的消息给rosmaster。

void XMLRPCManager::serverThreadFunc()
{
  disableAllSignalsInThisThread();
 
  while(!shutting_down_)
  {
    {
      boost::mutex::scoped_lock lock(added_connections_mutex_);
      S_ASyncXMLRPCConnection::iterator it = added_connections_.begin();
      S_ASyncXMLRPCConnection::iterator end = added_connections_.end();
      for (; it != end; ++it)
      {
        (*it)->addToDispatch(server_.get_dispatch());
        connections_.insert(*it);
      }
 
      added_connections_.clear();
    }
   。。。。

nodehandle是客户端，server端是在rosmaster启动时，通过rosgraph的ThreadingXMLRPCServer启动的，这样便实现了rosmaster与nodehandle的联系。