Rosserial Arduino Library中从一行代码开始探究系统原理

Rosserial Arduino Library中从一行代码开始探究系统原理

俗话说，管中窥豹，可见一斑。
从一行代码开始，分析rosserial arduino库的脉络。走码观花，到哪儿是哪儿。

编程写个ros节点，这是不可缺少的一句。

ros::NodeHandle  nh;

其定义在文件ros.h内

  typedef NodeHandle_<ArduinoHardware, 25, 25, 280, 280> NodeHandle;

从这个类型的定义开始提线，看25, 25, 280, 280都分别配置了什么？

找到文件ros/node_handle.h

/* Node Handle */
template<class Hardware,
         int MAX_SUBSCRIBERS = 25,/* 最大订阅数 */
         int MAX_PUBLISHERS = 25,/* 最大发布数 */
         int INPUT_SIZE = 512,/* 输入缓冲大小 */
         int OUTPUT_SIZE = 512>/* 输出缓冲大小 */

实现：

namespace ros
{

class NodeHandle
{
public:
  virtual int publish(int id, const Msg* msg) = 0;
  virtual int spinOnce() = 0;
  virtual bool connected() = 0;
protected:
  Hardware hardware_;
  uint8_t message_in[INPUT_SIZE];//接收数据缓冲区大小，这个size握手协商阶段会上报，上位机数据包字节数不能超过此限制。
  uint8_t message_out[OUTPUT_SIZE];//发送数据缓冲区大小，应估计最长数据来定义size。否则超出buf装不下，消息将会dropped


//MAX_PUBLISHERS; MAX_SUBSCRIBERS这里定义的数量为aduino内创建的puber和suber的最大数量
  Publisher * publishers[MAX_PUBLISHERS];
  Subscriber_ * subscribers[MAX_SUBSCRIBERS];
/*
定义好er们，一般在setup中会执行这样的语句。

  nh.advertise(puber);
  nh.subscribe(suber);
  nh.advertiseService(servicer_server);
  nh.serviceClient(servicer_client);
*/
//当调用advertise(puber)、subscribe(suber)时，就会被限定不能超出配置的数量。如果记不清有几个puber和suber，最好看看返回值是否成功。
/* Register a new publisher */
  bool advertise(Publisher & p)
  {
    for (int i = 0; i < MAX_PUBLISHERS; i++)
    {
      if (publishers[i] == 0) // empty slot
      {
        publishers[i] = &p;//------------------add
        p.id_ = i + 100 + MAX_SUBSCRIBERS;//起始100，之前的id保留， + MAX_SUBSCRIBERS 意图是让pubid比subid大并且不会重复。
        p.nh_ = this;
        return true;
      }
    }
    return false;
  }

  /* Register a new subscriber */
  template<typename SubscriberT>
  bool subscribe(SubscriberT& s)
  {
    for (int i = 0; i < MAX_SUBSCRIBERS; i++)
    {
      if (subscribers[i] == 0) // empty slot
      {
        subscribers[i] = static_cast<Subscriber_*>(&s);//------------------add
        s.id_ = i + 100;//起始100，之前的id保留，
        return true;
      }
    }
    return false;
  }

可以看到定义的puber和suber的指针已经存储到对应的“池”内，备用。
puber\suber是用来发布\获取数据的，值得注意的是serviceserver和service client,他们工作要发布\获取数据。

puber和suber、serviceserver，serviceclient示例汇总

ros::NodeHandle  nh;
void messageCb( const std_msgs::Empty& toggle_msg){/*suber获取利用msg的数据*/}
ros::Subscriber<std_msgs::Empty> sub("toggle_led", messageCb );
std_msgs::String str_msg;
ros::Publisher chatter("chatter", &str_msg);
char hello[13] = "hello world!";
void setup(){  nh.initNode();  nh.advertise(chatter);  nh.subscribe(sub);}//---------------------add suber和puber
void loop(){
  str_msg.data = hello;
  chatter.publish( &str_msg );//---------puber发布数据
  nh.spinOnce();
  delay(500);
}

ros::NodeHandle  nh;
void callback(const Test::Request & req, Test::Response & res){/*获取 req数据，填充res数据*/}
ros::ServiceServer<Test::Request, Test::Response> server("test_srv",&callback);
void setup(){  nh.initNode();  nh.advertiseService(server);}//-----------add servicer 
void loop(){
  nh.spinOnce();
}

ros::NodeHandle  nh;
ros::ServiceClient<Test::Request, Test::Response> client("test_srv");
void setup(){  nh.initNode();  nh.serviceClient(client);//-----------------add servicer 
  while(!nh.connected()) nh.spinOnce();
  nh.loginfo("Startup complete");
}
void loop(){
  Test::Request req;  Test::Response res;
  req.input = hello;//---------------填充请求数据
  client.call(req, res);
  //zzzzzz = res.output;//-----------利用获取的响应数据
  nh.spinOnce();
}

Publisher、Subscriber源码比较简单，略过。分析ServiceServer、ServiceClient。
他们继承了suber，包含了一个puber协同工作，
看ServiceServer 代码

template<typename MReq , typename MRes, typename ObjT = void>
class ServiceServer : public Subscriber_
{
public:
  typedef void(ObjT::*CallbackT)(const MReq&,  MRes&);

  ServiceServer(const char* topic_name, CallbackT cb, ObjT* obj) :
    pub(topic_name, &resp, rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_PUBLISHER),//注意类型值叠加
    obj_(obj)
  {
    this->topic_ = topic_name;
    this->cb_ = cb;
  }

  // these refer to the subscriber
  virtual void callback(unsigned char *data)
  {
    req.deserialize(data);
    (obj_->*cb_)(req, resp);//--------------处理获取到的数据，填充响应数据
    pub.publish(&resp);//------------------发布响应数据
  }
//...
  virtual int getEndpointType()
  {
    return rosserial_msgs::TopicInfo::ID_SERVICE_SERVER + rosserial_msgs::TopicInfo::ID_SUBSCRIBER;//注意类型值叠加
  }
//...
  Publisher pub;
//...

看ServiceClient 代码

template<typename MReq , typename MRes>
class ServiceClient : public Subscriber_
{
public:
  ServiceClient(const char* topic_name) :
    pub(topic_name, &req, rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_PUBLISHER)//注意类型值叠加
  {
    this->topic_ = topic_name;
    this->waiting = true;
  }

  virtual void call(const MReq & request, MRes & response)
  {
    if (!pub.nh_->connected()) return;
    ret = &response;
    waiting = true;
    pub.publish(&request);//----------------------------发布请求数据
    while (waiting && pub.nh_->connected())//----------------->>>等待响应，单片机spinOnce()卡住
      if (pub.nh_->spinOnce() < 0) break;//直到spinOnce接收数据成功，回调callback，完成整个流程；或者响应异常，请求失败，注意失败无返回数据。
  }
  // these refer to the subscriber
  virtual void callback(unsigned char *data)
  {
    ret->deserialize(data);
    waiting = false;//----------------------------------------------<<<正常响应完成
  }
//....
  virtual int getEndpointType()
  {
    return rosserial_msgs::TopicInfo::ID_SERVICE_CLIENT + rosserial_msgs::TopicInfo::ID_SUBSCRIBER;//注意类型值叠加
  }
//....
  Publisher pub;
//....

servicer中实例化的puber、suber。
它们也是存储在“池”内的，定义所以定义MAX_PUBLISHERS  MAX_SUBSCRIBERS要考虑servicer 的量

  /* Register a new Service Server */
  template<typename MReq, typename MRes, typename ObjT>
  bool advertiseService(ServiceServer<MReq, MRes, ObjT>& srv)
  {
    bool v = advertise(srv.pub);//-------------------------------------------ServiceServer  添加srv的puber
    for (int i = 0; i < MAX_SUBSCRIBERS; i++)
    {
      if (subscribers[i] == 0) // empty slot
      {
        subscribers[i] = static_cast<Subscriber_*>(&srv);//-----------------ServiceServer  添加srv的suber
        srv.id_ = i + 100;
        return v;
      }
    }
    return false;
  }

  /* Register a new Service Client */
  template<typename MReq, typename MRes>
  bool serviceClient(ServiceClient<MReq, MRes>& srv)
  {
    bool v = advertise(srv.pub);//-------------------------------------------ServiceClient 添加srv的puber
    for (int i = 0; i < MAX_SUBSCRIBERS; i++)
    {
      if (subscribers[i] == 0) // empty slot
      {
        subscribers[i] = static_cast<Subscriber_*>(&srv);//-----------------ServiceClient 添加srv的suber
        srv.id_ = i + 100;
        return v;
      }
    }
    return false;
  }

//当配置好了puber、suber、servicer,如何运作？消息怎么传出去，又怎么传进来？
如果没有spinOnce()驱动，他们中获取数据的er们将会一直游手好闲，无所事事。
spinOnce()是放在loop循环中不断重复执行的。
spinOnce()内主要是接收数据解包的工作，处理好接收到的数据往message_in  里填充，然后驱动对应er上工的逻辑。

首先spinOnce会等上位机发数据，来检测上位机的ros程序已经正常启动。
就像种子等待发芽的信号，春天来了，接收到暖阳和雨露的信号，就开始萌发。

触发设备第一个数据包是这样的

0xff 0xfe 0x00 0x00 0xff 0x00 0x00 0xff

spinOnce()内，接收数据逐个字节进行，解包并验证。
其中的mode_变量控制解析过程的不同阶段。
默认值为MODE_FIRST_FF，
先检查第一字节是否 0xff
然后 mode_++
看版本是否是0xfe
再 mode_++
...

根据下列数据看到解析过程经过8个阶段

const uint8_t SYNC_SECONDS  = 5;
const uint8_t MODE_FIRST_FF = 0;
const uint8_t MODE_PROTOCOL_VER   = 1;
const uint8_t PROTOCOL_VER1       = 0xff; // through groovy
const uint8_t PROTOCOL_VER2       = 0xfe; // in hydro
const uint8_t PROTOCOL_VER        = PROTOCOL_VER2;
const uint8_t MODE_SIZE_L         = 2;
const uint8_t MODE_SIZE_H         = 3;
const uint8_t MODE_SIZE_CHECKSUM  = 4;    // checksum for msg size received from size L and H
const uint8_t MODE_TOPIC_L        = 5;    // waiting for topic id
const uint8_t MODE_TOPIC_H        = 6;
const uint8_t MODE_MESSAGE        = 7;
const uint8_t MODE_MSG_CHECKSUM   = 8;    // checksum for msg and topic id

这里不一一展开，有兴趣可以对照源码参考前文进行测试。
前文
ROS中rosserial通讯协议初探
https://blog.csdn.net/qq_38288618/article/details/102931684

只说最后一步 

      else if (mode_ == MODE_MSG_CHECKSUM)    /* do checksum */
      {
        mode_ = MODE_FIRST_FF;
        if ((checksum_ % 256) == 255)
        {
          if (topic_ == TopicInfo::ID_PUBLISHER)
          {
//收到上位机第一个topicid==0的包，执行发送同步请求requestSyncTime，和协商话题。negotiateTopics，
            requestSyncTime();
            negotiateTopics();
            last_sync_time = c_time;
            last_sync_receive_time = c_time;
            return SPIN_ERR;
          }
          else if (topic_ == TopicInfo::ID_TIME)
          {
            syncTime(message_in);//--------------------------------------这里用到数据buf
          }
          else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST)
          {
            req_param_resp.deserialize(message_in);//--------------------------------------这里用到数据buf
            param_recieved = true;
          }
          else if (topic_ == TopicInfo::ID_TX_STOP)
          {
            configured_ = false;
          }
          else
          {
//这里订阅的suber执行回调
            if (subscribers[topic_ - 100])//因为之前add的时候subid=下标+100，这里用topic_ 来找是哪个suber的数据
              subscribers[topic_ - 100]->callback(message_in);//--------------------------------------这里用到数据buf
//如果是serviceserver回调处理完数据，再publish()发送数据
          }
        }
      }

这部分是接收完整包之后的逻辑
根据topic_ 不同类别分了5个分支

握手阶段

分支1、同步时间与协商话题，

            requestSyncTime();
            negotiateTopics();

把池里的suber和puber情况发送给上位机，上位机serial_node.py 的node根据情况进行配置，一边与ros沟通，一边与下位机沟通。

用到message_in数据的有3个分支

分支2、suber、servicer（server+client）这些suber的callback、
分支3、时间同步syncTime
分支4、参数获取req_param_resp

还有一个分支是配置开关

分支5、configured_ = false;

message_in是装载发过来的数据的buf，没见有检查溢出的代码句子，因为在握手阶段就把自己的buf大小告诉了上位机。

上位机serial_node.py 应检查要发送数据的大小，发来的数据不会超限。

分析握手阶段，协商话题发送的数据

  void negotiateTopics()
  {
    rosserial_msgs::TopicInfo ti;
    int i;
    for (i = 0; i < MAX_PUBLISHERS; i++)
    {
      if (publishers[i] != 0) // non-empty slot
      {
        ti.topic_id = publishers[i]->id_;
        ti.topic_name = (char *) publishers[i]->topic_;
        ti.message_type = (char *) publishers[i]->msg_->getType();
        ti.md5sum = (char *) publishers[i]->msg_->getMD5();
        ti.buffer_size = OUTPUT_SIZE;
        publish(publishers[i]->getEndpointType(), &ti);//组织好每个puber的信息发送
      }
    }
    for (i = 0; i < MAX_SUBSCRIBERS; i++)
    {
      if (subscribers[i] != 0) // non-empty slot
      {
        ti.topic_id = subscribers[i]->id_;
        ti.topic_name = (char *) subscribers[i]->topic_;
        ti.message_type = (char *) subscribers[i]->getMsgType();
        ti.md5sum = (char *) subscribers[i]->getMsgMD5();
        ti.buffer_size = INPUT_SIZE;
        publish(subscribers[i]->getEndpointType(), &ti);//组织好每个suber的信息发送
      }
    }
    configured_ = true;
  }

 看看通用的打包方法

  virtual int publish(int id, const Msg * msg)
  {
    if (id >= 100 && !configured_)
      return 0;

    /* serialize message */
    int l = msg->serialize(message_out + 7);

    /* setup the header */
    message_out[0] = 0xff;
    message_out[1] = PROTOCOL_VER;
    message_out[2] = (uint8_t)((uint16_t)l & 255);
    message_out[3] = (uint8_t)((uint16_t)l >> 8);
    message_out[4] = 255 - ((message_out[2] + message_out[3]) % 256);
    message_out[5] = (uint8_t)((int16_t)id & 255);
    message_out[6] = (uint8_t)((int16_t)id >> 8);

    /* calculate checksum */
    int chk = 0;
    for (int i = 5; i < l + 7; i++)
      chk += message_out[i];
    l += 7;
    message_out[l++] = 255 - (chk % 256);
//判断是否消息是不是超出message_out范围了。我觉得应该靠前判断，算了半天，结果一看超限了做了无用功。
    if (l <= OUTPUT_SIZE)
    {
      hardware_.write(message_out, l);
//串口发送
      return l;
    }
    else
    {
      //logerror("Message from device dropped: message larger than buffer.");
      logerror("MsgEr:OutBuf");
//log的字符串太长，多占内存？
      return -1;
    }
  }

//上面的topic类型是 rosserial_msgs::TopicInfo，通讯协议依赖的消息类型。
可以找到rosserial_msgs/TopicInfo .h看一眼

namespace rosserial_msgs
{

  class TopicInfo : public ros::Msg
  {
    public:
      typedef uint16_t _topic_id_type;
      _topic_id_type topic_id;
      typedef const char* _topic_name_type;
      _topic_name_type topic_name;
      typedef const char* _message_type_type;
      _message_type_type message_type;
      typedef const char* _md5sum_type;
      _md5sum_type md5sum;
      typedef int32_t _buffer_size_type;
      _buffer_size_type buffer_size;
//这里数值就是协议规定的分类
      enum { ID_PUBLISHER = 0 };
      enum { ID_SUBSCRIBER = 1 };
      enum { ID_SERVICE_SERVER = 2 }; 
      enum { ID_SERVICE_CLIENT = 4 };
      enum { ID_PARAMETER_REQUEST = 6 };
      enum { ID_LOG = 7 };
      enum { ID_TIME = 10 };
      enum { ID_TX_STOP = 11 };

    TopicInfo():
      topic_id(0),
      topic_name(""),
      message_type(""),
      md5sum(""),
      buffer_size(0)
    {
    }

    virtual int serialize(unsigned char *outbuffer) const
//。。。
    virtual int deserialize(unsigned char *inbuffer)
//。。。
    const char * getType(){ return "rosserial_msgs/TopicInfo"; };
    const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; };

  };

}

跟其他消息没什么两样。类型和md5太占内存。包名和msg名每一个字符多占1byte的内存

    const char * getType(){ return "rosserial_msgs/TopicInfo"; };//24个字节
    const char * getMD5(){ return "0ad51f88fc44892f8c10684077646005"; };//32个字节

这个消息类24+32=56个字节就这样被占了。
md5要验证没办法，包名和类型名真得简化简化

整理个简图，有助于看清整个系统运作的脉络。

再回头看

  typedef NodeHandle_<ArduinoHardware, 25, 25, 280, 280> NodeHandle;

template<class Hardware,
         int MAX_SUBSCRIBERS = 25,//最大订阅数
         int MAX_PUBLISHERS = 25,//最大发布数
         int INPUT_SIZE = 512,//输入缓冲大小
         int OUTPUT_SIZE = 512>//输出缓冲大小
如何更改数值大小。

分析得出每多1个servicer不管是server还是client，puber和suber数量各加1

MAX_PUBLISHERS =pubers+servicers
MAX_SUBSCRIBERS =subers+servicers

可以多加几个量，指针数组不怎么影响内存。

INPUT_SIZE 按上位机最大数据量估算
OUTPUT_SIZE 按单片机要发送的最大数据量估算。
上个前文的包

FF FE 48 00 B7 -------------------0x0048=72 包数据长度 0xb7=0xff-((0x48+0x00)%0x100)
00 00 -----------------------------0x0000= Topic ID(ID_PUBLISHER=0)
7D 00 -----------------------------0x7D=125 topic_id (publisher ID)
07 00 00 00 ----------------------0x0007=7，数据长度 topic_name="chatter"
63 68 61 74 74 65 72 
0F 00 00 00 ----------------------0x000f=15，数据长度 message_type ="std_msgs/String"
73 74 64 5F 6D 73 67 73 2F 53 
74 72 69 6E 67 
20 00 00 00 ----------------------0x0020=32，数据长度 md5sum="992ce8a1687cec8c8bd883ec73ca41d1"
39 39 32 63 65 38 61 31 36 38 
37 63 65 63 38 63 38 62 64 38 
38 33 65 63 37 33 63 61 34 31 
64 31 
18 01 00 00 ----------------------0x0118=280,是atmage328p的buffer_size  (看2.1)
0C --------------------------------0x0C 包校验

这样观察更直接一些。
通信的这个数据72个字节，再加头7个，尾部校验1个，总共80个字节。

数据的大小与名字长短也有关系，
这些类型名字真得斟酌斟酌，毕竟单片机资源捉襟见肘。

比如有自定义包名zzz_protocol,消息名screen_1602 
zzz_protocol/screen_1602     24bytes
简化
zp/s1602      8bytes
这样省了16个字节

先告一段落。。。

纲举目张。目太多，不知怎么切入。有点碎。。。。
错误之处请多多指正！