ROS1源码阅读 -- 初始化

三千猫饼

已于 2023-02-22 17:34:47 修改

阅读量155

点赞数 1

文章标签：机器人

于 2023-02-22 17:33:47 首次发布

本文链接：https://blog.csdn.net/weixin_39427234/article/details/129166336

版权

1. 概述

ROS作为一个开源的机器人分布式框架也广泛应用于自动驾驶领域，因此了解ROS的底层处理逻辑能够让我们更好的使用框架进行开发。

ROS有三种通信模式：基于订阅者和发布者的话题通信、基于客户端和服务端的服务通信和基于RPC的参数服务器通信方式；在开发中我们使用最广泛的应该算是基于订阅者和发布者的话题通信了，因此接下来我们从一个简单的话题通信模型入手，对通信建立和数据的传输部分的源码进行梳理。

2. 分析模型搭建

为了方便梳理，我们首先建立一个简单的话题通信的模型，模型需要实现一个发布者节点(talker_node)和一个订阅者节点(listener_node)，两个节点通过topic /example/message进行数据传输。

简单的订阅发布模型

3. 模型实现

3.1 发布者节点

发布者节点中创建一个发布者，以1Hz的频率向topic /example/message发布数据。

#include<iostream>
#include<ros/ros.h>
#include<std_msgs/String.h>

int main(int argc, char* argv[])
{
    // step 1：初始化ROS节点
    ros::init(argc, argv, "example");
    
    // step 2: 实例化node handle
    ros::Nodehandle nh;
    
    // step 3： 创建ROS发布者
    ros::Publisher pub = nh.advertise("/example/message", 1);
    
    // step 4： 消息定义&发布
    ros::Rate rate(1);    
    std_msgs::String msg;
    while(ros::ok())
    {
        msg_data.data = "hello world";
        rate.sleep();
        ros::spinOnce();
    }    
    
    return 0;
}

3.2 订阅者节点的实现

#include<iostream>
#include<ros/ros.h>
#include<std_msgs/String.h>

void print(const std_msgs::StringConstPtr& msg)
{
    ROS_INFO("Listener recived message: %s", msg->data.c_str());
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "listener_node");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe<std_msgs::String>("/example/message", 1, print);
    ros::spin();
    return 0;    
}

4. 源码分析

首先从发布者节点入手，分析ros初始化过程即代码运行到ros::init(int argc, char* argv[]);处的框架内部调用。用户调用的ros::init在源码中的声明如下所示：

/** @brief ROS initialization function.
 *
 * This function will parse any ROS arguments (e.g., topic name
 * remappings), and will consume them (i.e., argc and argv may be modified
 * as a result of this call).
 *
 * Use this version if you are using the NodeHandle API
 *
 * \param argc
 * \param argv
 * \param name Name of this node.  The name must be a base name, ie. it cannot contain namespaces.
 * \param options [optional] Options to start the node with (a set of bit flags from \ref ros::init_options)
 * \throws InvalidNodeNameException If the name passed in is not a valid "base" name
 *
 */
ROSCPP_DECL void init(int &argc, char **argv, const std::string& name, uint32_t options = 0);

ros::init的主要作用是解析通过main函数传入的一些参数(如果有的话)，然后就是执行节点相关的一些对象的初始化。首先外层的init会解析main函数的传入的参数，并将结果放在string的一个Map中，然后通过解析出来的节点名和命名空间的Map调用内部的init，在内部init中会创建并初始化该节点的一个单例的实例出来，然后还会对相关组件进行初始化，如network、master等；内部init具体执行逻辑如下：

void init(const M_string& remappings, const std::string& name, uint32_t options)
{
  options |= init_options::NoRosout;

  if (!g_atexit_registered)
  {
    g_atexit_registered = true;
    atexit(atexitCallback);
  }

  if (!g_global_queue)
  {
    g_global_queue.reset(new CallbackQueue);
  }

  if (!g_initialized)
  {
    g_init_options = options;
    g_ok = true;

    ROSCONSOLE_AUTOINIT;
    // Disable SIGPIPE
#ifndef WIN32
    signal(SIGPIPE, SIG_IGN);
#else
    WSADATA wsaData;
    WSAStartup(MAKEWORD(2, 0), &wsaData);
#endif
    check_ipv6_environment();
    network::init(remappings);
    master::init(remappings);
    // names:: namespace is initialized by this_node
    // DBG：该接口会创建一个ThisNode单例并初始化，并且在this_node命名空间里面还定义了一些api用以获取node的相关信息(name、namespace...)
    this_node::init(name, remappings, options);
    file_log::init(remappings);
    XmlRpc::XmlRpcValue params, result, param_server;
    params[0] = this_node::getName();
    params[1] = "/param_server";
    bool redis_ready = false;
    // DBG: 通过execute可以通过RPC与master节点进行通信，将节点的信息注册给master节点
    bool ret = master::execute("getParam", params, result, param_server, true);
    if (ret && std::string(param_server) == "redis"){
      // redis_manager init
      ROS_INFO_STREAM("using redis as param server");
      redis_ready = RedisManager::instance()->init();
      ROS_WARN_COND(!redis_ready,"Rosmaster's /param_server is redis while RedisManager initialized failed,will switch to rosmaster");
    }
    if (!ret || !redis_ready){
      ROS_INFO_STREAM("using rosmaster as param server");
      param::init(remappings);
    }

    g_initialized = true;
  }
}

至此，节点的初始化工作完成，通过RPC将该节点注册到master节点以便被其他节点发现。