2023年01月03日 | ROS机器人操作系统的实现原理解析

目的

本文介绍（Robot Operang System）的实现原理，从最底层分析ROS代码是如何实现的。

1、序列化

把的内容（也就是消息message）序列化是通信的基础，所以我们先研究序列化。

尽管笔者从事机器人学习和研发很长时间了，但是在研究ROS的过程中，“序列化”这个词还是这辈子第一次听到。

所以可想而知很多人在看到“把一个消息序列化”这样的描述时是如何一脸懵逼。

但其实序列化是一个比较常见的概念，你虽然不知道它但一定接触过它。

下面我们先介绍“序列化”的一些常识，然后解释ROS里的序列化是怎么做的？

1.1　什么是序列化？

“序列化”（SerializaTIon ）的意思是将一个对象转化为字节流。

这里说的对象可以理解为“面向对象”里的那个对象，具体的就是存储在内存中的对象数据。

与之相反的过程是“反序列化”（DeserializaTIon ）。

虽然挂着机器人的羊头，但是后面的介绍全部是计算机知识，跟机器人一丁点关系都没有，序列化就是一个纯粹的计算机概念。

序列化的英文Serialize就有把一个东西变成一串连续的东西之意。

形象的描述，数据对象是一团面，序列化就是将面团拉成一根面条，反序列化就将面条捏回面团。

另一个形象的类比是我们在对话或者打电话时，一个人的思想转换成一维的语音，然后在另一个人的头脑里重新变成结构化的思想，这也是一种序列化。

面对序列化，很多人心中可能会有很多疑问。

首先，为什么要序列化？或者更具体的说，既然对象的信息本来就是以字节的形式储存在内存中，那为什么要多此一举把一些字节数据转换成另一种形式的、一维的、连续的字节数据呢？

如果我们的程序在内存中存储了一个数字，比如25。那要怎么传递25这个数字给别的程序节点或者把这个数字永久存储起来呢？

很简单，直接传递25这个数字（的字节表示，即0X19，当然最终会变成二进制表示11001以高低电平传输存储）或者直接把这个数字（的字节表示）写进硬盘里即可。

所以，对于本来就是连续的、一维的、一连串的数据（例如字符串），序列化并不需要做太多东西，其本质是就是由内存向其它地方拷贝数据而已。

所以，如果你在一个序列化库里看到memcpy函数不用觉得奇怪，因为你知道序列化最底层不过就是在操作内存数据而已（还有些库使用了流的ostream.rdbuf()->sputn函数）。

可是实际程序操作的对象很少是这么简单的形式，大多数时候我们面对的是包含不同数据类型（int、double、string）的复杂数据结构（比如vector、list），它们很可能在内存中是不连续存储的而是分散在各处。比如ROS的很多消息都包含向量。

数据中还有各种指针和引用。而且，如果数据要在运行于不同架构的计算机之上的、由不同语言所编写的节点程序之间传递，那问题就更复杂了，它们的字节顺序endianness规定有可能不一样，基本数据类型（比如int）的长度也不一样（有的int是4个字节、有的是8个字节）。

这些都不是通过简单地、原封不动地复制粘贴原始数据就能解决的。这时候就需要序列化和反序列化了。

所以在程序之间需要通信时（ROS恰好就是这种情况），或者希望保存程序的中间运算结果时，序列化就登场了。

另外，在某种程度上，序列化还起到统一标准的作用。

我们把被序列化的东西叫object（对象），它可以是任意的数据结构或者对象：结构体、数组、类的实例等等。

把序列化后得到的东西叫archive，它既可以是人类可读的文本形式，也可以是二进制形式。

前者比如JSON和XML，这两个是应用里最常用的序列化格式，通过记事本就能打开阅读；

后者就是原始的二进制文件，比如后缀名是bin的文件，人类是没办法直接阅读一堆的0101或者0XC9D23E72的。

序列化算是一个比较常用的功能，所以大多数编程语言（比如、、等）都会附带用于序列化的库，不需要你再去造轮子。

以C++为例，虽然标准STL库没有提供序列化功能，但是第三方库Boost提供了[ 2 ]谷歌的protobuf也是一个序列化库，还有Fast-CDR，以及不太知名的Cereal，Java自带序列化函数，python可以使用第三方的ckle模块实现。

总之，序列化没有什么神秘的，用户可以看看这些开源的序列化库代码，或者自己写个小程序试试简单数据的序列化，例如这个例子，或者这个，有助于更好地理解ROS中的实现。

1.2　ROS中的序列化实现

理解了序列化，再回到ROS。我们发现，ROS没有采用第三方的序列化工具，而是选择自己实现，代码在roscpp_core项目下的roscpp_serializaTIon中，见下图。这个功能涉及的代码量不是很多。

为什么ROS不使用现成的序列化工具或者库呢？可能ROS诞生的时候（2007年），有些序列化库可能还不存在（protobuf诞生于2008年），更有可能是ROS的创造者认为当时没有合适的工具。

1.2.1　serialization.h

核心的函数都在serialization.h里，简而言之，里面使用了标准库的memcpy函数把消息拷贝到流中。

下面来看一下具体的实现。

序列化功能的特点是要处理很多种数据类型，针对每种具体的类型都要实现相应的序列化函数。

为了尽量减少代码量，ROS使用了模板的概念，所以代码里有一堆的mplate。

从后往前梳理，先看Stream这个结构体吧。在C++里结构体和类基本没什么区别，结构体里也可以定义函数。

Stream翻译为流，流是一个计算机中的抽象概念，前面我们提到过字节流，它是什么意思呢？

在需要传输数据的时候，我们可以把数据想象成传送带上连续排列的一个个被传送的物体，它们就是一个流。

更形象的，可以想象磁带或者图灵机里连续的纸带。在文件读写、使用串口、网络Socket通信等领域，流经常被使用。例如我们常用的输入输出流：

cout<<"helllo"; 由于使用很多，流的概念也在演变。想了解更多可以看这里。

struct Stream
{
  // Returns a pointer to the current position of the stream
  inline uint8_t* getData() { return data_; }
  // vances the stream, checking bounds, and returns a pointer to the position before it was advanced.
  // 	hrows StreamOverrunException if len would take this stream past the end of its buffer
  ROS_FORCE_INLINE uint8_t* advance(uint32_t len)
{
    uint8_t* old_data = data_;
    data_ += len;
    if (data_ > end_)
    {
      // Throwing directly here causes a significant speed hit due to the extra code generated for the throw statement
      throwStreamOverrun();
    }
    return old_data;
  }
  // Returns the amount of space left in the stream
  inline uint32_t getLength() { return static_cast(end_ - data_); }
  
protected:
  Stream(uint8_t* _data, uint32_t _count) : data_(_data), end_(_data + _count) {}


private:
  uint8_t* data_;
  uint8_t* end_;
};

注释表明Stream是个基类，输入输出流IStream和OStream都继承自它。

Stream的成员变量data_是个指针，指向序列化的字节流开始的位置，它的类型是uint8_t。

在Ubuntu系统中，uint8_t的定义是typedef unsigned char uint8_t;

所以uint8_t就是一个字节，可以用size_of()函数检验。data_指向的空间就是保存字节流的。

输出流类OStream用来序列化一个对象，它引用了serialize函数，如下。

struct OStream : public Stream
{
  static const StreamType stream_type = stream_types::Output;
  OStream(uint8_t* data, uint32_t count) : Stream(data, count) {}
  /* Serialize an item to this output stream*/
  template
  ROS_FORCE_INLINE void next(const T& t)
{
    serialize(*this, t);
  }
  template
  ROS_FORCE_INLINE OStream& operator<<(const T& t)
  {
    serialize(*this, t);
    return *this;
  }
};

输入流类IStream用来反序列化一个字节流，它引用了deserialize函数，如下。

struct ROSCPP_SERIALIZATION_DECL IStream : public Stream
{
  static const StreamType stream_type = stream_types::Input;
  IStream(uint8_t* data, uint32_t count) : Stream(data, count) {}
  /* Deserialize an item from this input stream */
  template
  ROS_FORCE_INLINE void next(T& t)
{
    deserialize(*this, t);
  }
  template
  ROS_FORCE_INLINE IStream& operator>>(T& t)
  {
    deserialize(*this, t);
    return *this;
  }
};

自然，serialize函数和deserialize函数就是改变数据形式的地方，它们的定义在比较靠前的地方。它们都接收两个模板，都是内联函数，然后里面没什么东西，只是又调用了Serializer类的成员函数write和read。所以，serialize和deserialize函数就是个二道贩子。

// Serialize an object.  Stream here should normally be a ros::OStream
template
inline void serialize(Stream& stream, const T& t)
{
  Serializer::write(stream, t);
}
// Deserialize an object.  Stream here should normally be a ros::IStream
template
inline void deserialize(Stream& stream, T& t)
{
  Serializer::read(stream, t);
}

所以，我们来分析Serializer类，如下。我们发现，write和read函数又调用了类型里的serialize函数和deserialize函数。

头别晕，这里的serialize和deserialize函数跟上面的同名函数不是一回事。

注释中说：“Specializing the Serializer class is the only thing you need to do to get the ROS serialization system to work with a type”（要想让ROS的序列化功能适用于其它的某个类型，你唯一需要做的就是特化这个Serializer类）。

这就涉及到的另一个知识点——模板特化（template specialization）。

template struct Serializer
{
  // Write an object to the stream.  Normally the stream passed in here will be a ros::OStream
  template
  inline static void write(Stream& stream, typename boost::call_trts::pa_type t)
{
    t.serialize(stream.getData(), 0);
  }
   // Read an object from the stream.  Normally the stream passed in here will be a ros::IStream
  template
  inline static void read(Stream& stream, typename boost::call_traits::reference t)
{
    t.deserialize(stream.getData());
  }
  // Determine the serialized length of an object.
  inline static uint32_t serializedLength(typename boost::call_traits::param_type t)
{
    return t.serializationLength();
  }
};

接着又定义了一个带参数的宏函数ROS_CREATE_SIMPLE_SERIALIZER(Type)，然后把这个宏作用到了ROS中的10种基本数据类型，分别是：uint8_t, int8_t, uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t, float, double。

说明这10种数据类型的处理方式都是类似的。看到这里大家应该明白了，write和read函数都使用了memcpy函数进行数据的移动。

注意宏定义中的template<>语句，这正是模板特化的标志，关键词template后面跟一对尖括号。

关于模板特化可以看这里。

#define ROS_CREATE_SIMPLE_SERIALIZER(Type) 
  template<> struct Serializer 
  { 
    template inline static void write(Stream& stream, const Type v) 
{ 
      memcpy(stream.advance(sizeof(v)), &v, sizeof(v) ); 
    } 
    template inline static void read(Stream& stream, Type& v) 
{ 
      memcpy(&v, stream.advance(sizeof(v)), sizeof(v) ); 
    } 
    inline static uint32_t serializedLength(const Type&) 
{ 
      return sizeof(Type); 
    } 
};
ROS_CREATE_SIMPLE_SERIALIZER(uint8_t)
ROS_CREATE_SIMPLE_SERIALIZER(int8_t)
ROS_CREATE_SIMPLE_SERIALIZER(uint16_t)
ROS_CREATE_SIMPLE_SERIALIZER(int16_t)
ROS_CREATE_SIMPLE_SERIALIZER(uint32_t)
ROS_CREATE_SIMPLE_SERIALIZER(int32_t)
ROS_CREATE_SIMPLE_SERIALIZER(uint64_t)
ROS_CREATE_SIMPLE_SERIALIZER(int64_t)
ROS_CREATE_SIMPLE_SERIALIZER(float)
ROS_CREATE_SIMPLE_SERIALIZER(double)

对于其它类型的数据，例如bool、std::string、std::vector、ros::Time、ros::Duration、boost::array等等，它们各自的处理方式有细微的不同，所以不再用上面的宏函数，而是用模板特化的方式每种单独定义，这也是为什么serialization.h这个文件这么冗长。

对于int、double这种单个元素的数据，直接用上面特化的Serializer类中的memcpy函数实现序列化。

对于vector、array这种多个元素的数据类型怎么办呢？方法是分成几种情况，对于固定长度简单类型的（fixed-size simple types），还是用各自特化的Serializer类中的memcpy函数实现，没啥太大区别。

对于固定但是类型不简单的（fixed-size non-simple types）或者既不固定也不简单的（non-fixed-size, non-simple types）或者固定但是不简单的（fixed-size, non-simple types），用for循环遍历，一个元素一个元素的单独处理。

那怎么判断一个数据是不是固定是不是简单呢？这是在roscpp_traits文件夹中的message_traits.h完成的。

其中采用了萃取Type Traits，这是相对高级一点的编程技巧了，笔者也不太懂。

对序列化的介绍暂时就到这里了，有一些细节还没讲，等笔者看懂了再补。

2、消息订阅发布

2.1　ROS的本质

如果问ROS的本质是什么，或者用一句话概括ROS的核心功能。那么，笔者认为ROS就是个通信库，让不同的程序节点能够相互对话。

很多文章和书籍在介绍ROS是什么的时候，经常使用“ROS是一个通信框架”这种描述。

但是笔者认为这种描述并不是太合适。“框架”是个对初学者非常不友好的抽象词汇，用一个更抽象难懂的概念去解释一个本来就不清楚的概念，对初学者起不到任何帮助。

而且笔者严重怀疑绝大多数作者能对机器人的本质或者软件框架能有什么太深的理解，他们的见解不会比你我深刻多少。

既然提到本质，那我们就深入到最基本的问题。

在接触无穷的细节之前，我们不妨先做一个哲学层面的思考。

那就是，为什么ROS要解决通信问题？

机器人涉及的东西千千万万，、、软件、无所不包，为什么底层的设计是一套用来通信的程序而不是别的东西。

到目前为止，我还没有看到有人讨论过这个问题。这要回到机器人或者的本质。

当我们在谈论机器人的时候，最首要的问题不是设计，而是对信息的处理。一个机器人需要哪些信息，信息从何而来，如何传递，又被谁使用，这些才是最重要的问题。

人类飞不鸟，游不过鱼，跑不过马，力不如牛，为什么却自称万物之灵呢。

因为人有大脑，而且人类大脑处理的信息更多更复杂。

抛开物质，从信息的角度看，人与动物、与机器人存在很多相似的地方。

机器人由许多功能模块组成，它们之间需要协作才能形成一个有用的整体，机器人与机器人之间也需要协作才能形成一个有用的系统，要协作就离不开通信。

需要什么样的信息以及信息从何而来不是ROS首先关心的，因为这取决于机器人的应用场景。

因此，ROS首先要解决的是通信的问题，即如何建立通信、用什么方式通信、通信的格式是什么等等一系列具体问题。

带着这些问题，我们看看ROS是如何设计的。

2.2　客户端库

实现通信的代码在ros_comm包中，如下。

其中clients文件夹一共有127个文件，看来是最大的包了。

现在我们来到了ROS最核心的地带。

客户端这个名词出现的有些突然，一个机器人操作系统里为什么需要客户端。

原因是，节点与主节点master之间的关系是client/server，这时每个节点都是一个客户端（client），而master自然就是服务器端（server）。

那客户端库（client libraries）是干什么的？就是为实现节点之间通信的。

虽然整个文件夹中包含的文件众多，但是我们如果按照一定的脉络来分析就不会眼花缭乱。

节点之间最主要的通信方式就是基于消息的。为了实现这个目的，需要三个步骤，如下。

弄明白这三个步骤就明白ROS的工作方式了。这三个步骤看起来是比较合乎逻辑的，并不奇怪。

消息的发布者和订阅者（即消息的接收方）建立连接；

发布者向发布消息，订阅者在话题上接收消息，将消息保存在回调函数队列中；

调用回调函数队列中的回调函数处理消息。

2.2.1　一个节点的诞生

在建立连接之前，首先要有节点。

节点就是一个独立的程序，它运行起来后就是一个普通的进程，与计算机中其它的进程并没有太大区别。

一个问题是：ROS中为什么把一个独立的程序称为“节点”

这是因为ROS沿用了计算机网络中“节点”的概念。

在一个网络中，例如互联网，每一个上网的计算机就是一个节点。前面我们看到的客户端、服务器这样的称呼，也是从计算机网络中借用的。

下面来看一下节点是如何诞生的。我们在第一次使用ROS时，一般都会照着官方编写一个talker和一个listener节点，以熟悉ROS的使用方法。

我们以talker为例，它的部分代码如下。

#include "ros/ros.h"
int main(int argc, char **argv)
{
  /* You must call one of the versions of ros::init() before using any other part of the ROS system. */
  ros::init(argc, argv, "talker");
  ros::NodeHandle n;

main函数里首先调用了init()函数初始化一个节点，该函数的定义在init.cpp文件中。

当我们的程序运行到init()函数时，一个节点就呱呱坠地了。

而且在出生的同时我们还顺道给他起好了名字，也就是"talker"。

名字是随便起的，但是起名是必须的。

我们进入init()函数里看看它做了什么，代码如下，看上去还是挺复杂的。它初始化了一个叫g_global_queue的数据，它的类型是CallbackQueuePtr。