rosserial_arduino开发，string、array消息类的解读和优化。

rosserial_arduino开发，string、array消息类的解读和优化。

ros是个通讯框架系统，机器人硬件开发时，数据通讯是必须的。
使用rosserial_arduino功能包能把ros的msg消息转换成标准的.h头文件供硬件编程使用。

传输大量的数据时，比如一长串的字符、元素很多的数组。

常用的数据类型是 string ，array。

首先解读string类中解析数据的实现

源码

#ifndef _ROS_std_msgs_String_h
#define _ROS_std_msgs_String_h

#include <stdint.h>
#include <string.h>
#include <stdlib.h>
#include "ros/msg.h"

namespace std_msgs
{

  class String : public ros::Msg
  {
    public:
      typedef const char* _data_type;
      _data_type data;

    String():
      data("")
    {
    }

    virtual int serialize(unsigned char *outbuffer) const
    {
      int offset = 0;
      uint32_t length_data = strlen(this->data);
      varToArr(outbuffer + offset, length_data);
      offset += 4;
      memcpy(outbuffer + offset, this->data, length_data);
      offset += length_data;
      return offset;
    }

    virtual int deserialize(unsigned char *inbuffer)
    {
      int offset = 0;
      uint32_t length_data;
      arrToVar(length_data, (inbuffer + offset));
      offset += 4;
      for(unsigned int k= offset; k< offset+length_data; ++k){
          inbuffer[k-1]=inbuffer[k];
      }
      inbuffer[offset+length_data-1]=0;
      this->data = (char *)(inbuffer + offset-1);
      offset += length_data;
     return offset;
    }

    const char * getType(){ return "std_msgs/String"; };
    const char * getMD5(){ return "992ce8a1687cec8c8bd883ec73ca41d1"; };

  };

}
#endif

分析：

由此可见实际传送的数据内容是
4字节的length_data +length_data 个字节的形式

解析方法

virtual int deserialize(unsigned char *inbuffer)

参数inbuffer是接收数据的缓冲区buff，其大小在ros.h文件中定义

typedef NodeHandle_<ArduinoHardware, 8, 8, 300, 150> NodeHandle;

其中300就是buff空间的size

第一步解析4个字节的数据长度值uint32_t length_data

  int offset = 0;
  uint32_t length_data;
  arrToVar(length_data, (inbuffer + offset));
  offset += 4;

arrToVar实现了4 byte --> uint32_t 的转换

接下来的inbuffer[k-1]=inbuffer[k]操作，有点意思：
把整个字串数据往前挪了一个字节

  for(unsigned int k= offset; k< offset+length_data; ++k){
      inbuffer[k-1]=inbuffer[k];
  }
  inbuffer[offset+length_data-1]=0;

数据最后一位补个0，就是’\0’呗。
于是就在缓冲区，构造了一个 string 。
将这个string的data指针，指向了挪了一个的位置上。

 this->data = (char *)(inbuffer + offset-1);

解析工作就此完成。

简述运行调用

参考
Rosserial Arduino Library中从一行代码开始探究系统原理
https://blog.csdn.net/qq_38288618/article/details/104464561

如果程序中sub了一个string话题，
nh.spinOnce()接收数据到buff,
完成一个包，根据包的id（topic_ ）就能找到对应suber是
subscribers[topic_ - 100]
执行
subscribers[topic_ - 100]->callback(message_in);
这里的message_in 就是buff

suber实例在内部告诉自己的msg成员解析数据message_in ，
如果msg 是string的话，调用

deserialize(unsigned char *inbuffer)这个方法

其中的inbuffer就是message_in
deserialize的工作是在缓冲区message_in 内构造了一个’\0’结尾的string data。

最后执行到自定义的 callback 时调用这个data就能使用了。
这种方式对于内存吃紧的单片机来说是极好的。

继续优化的畅想：

如果当初string的协议规定 在data之后默认多传输一个’\0’，那么单片机内一系列的挪动工作都省掉了。
指针一指就ok了。
或者解析完data之后的数据（一般是4字节的length）在data之后的一个字节改为’\0’，
但如果data是最后一个字节正好是buff的最后一个字节，会溢出破坏后边的数据。

第二：数组类型数据的实现，以一个复合类型的数据为例

由于消息的头文件.h文件是由 .msg文件通过模板转换得来。
关于数组部分的处理，跟string不同，它是额外分配内存的。
如一个消息

string cmd
uint8[] input

数据包部分组织结构
4个字节length_cmd
cmddata
4个字节input_lengthT
inputdata

源码

virtual int deserialize(unsigned char *inbuffer)
{
  int offset = 0;
  uint32_t length_cmd;
  arrToVar(length_cmd, (inbuffer + offset));
  offset += 4;
  for(unsigned int k= offset; k< offset+length_cmd; ++k){
      inbuffer[k-1]=inbuffer[k];
  }
  inbuffer[offset+length_cmd-1]=0;
  this->cmd = (char *)(inbuffer + offset-1);
  offset += length_cmd;
  uint32_t input_lengthT = ((uint32_t) (*(inbuffer + offset))); 
  input_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
  input_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
  input_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
  offset += sizeof(this->input_length);
  if(input_lengthT > input_length)
    this->input = (uint8_t*)realloc(this->input, input_lengthT * sizeof(uint8_t));
  input_length = input_lengthT;
  for( uint32_t i = 0; i < input_length; i++){
  this->st_input =  ((uint8_t) (*(inbuffer + offset)));
  offset += sizeof(this->st_input);
    memcpy( &(this->input[i]), &(this->st_input), sizeof(uint8_t));
  }
 return offset;
}

分析：

解析方法
可以看出string部分还是极好的，数组部分则重新分配了空间

this->input = (uint8_t*)realloc(this->input, input_lengthT * sizeof(uint8_t));

for循环中 内存拷贝

memcpy( &(this->input[i]), &(this->st_input), sizeof(uint8_t));

这与string解析的方法不一样，为什么这里不把指针直接指过去？难道是为了考虑代码的通用性和扩展性？
按上文说法如果分配buff空间是300字节，假如一个数组消息填即将满缓冲区，280字节
那么，瞬时会多占280字节的内存，还要复制内存，耗费时间。
对于ram仅仅2048字节的单片机atmega328p来说，这样的大消息有可能会让它吐血。

对于我用的数据很简单，不必考虑通用和扩展性，所以改成这样：

virtual int deserialize(unsigned char *inbuffer)
{
  int offset = 0;
  uint32_t length_cmd;
  arrToVar(length_cmd, (inbuffer + offset));
  offset += 4;
  for(unsigned int k= offset; k< offset+length_cmd; ++k){
      inbuffer[k-1]=inbuffer[k];
  }
  inbuffer[offset+length_cmd-1]=0;
  this->cmd = (char *)(inbuffer + offset-1);
  offset += length_cmd;
  uint32_t input_lengthT = ((uint32_t) (*(inbuffer + offset))); 
  input_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
  input_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
  input_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
  offset += sizeof(this->input_length);
  //if(input_lengthT > input_length)
    //this->input = (uint8_t*)realloc(this->input, input_lengthT * sizeof(uint8_t));
  input_length = input_lengthT;
  this->input = (uint8_t *)(inbuffer + offset);
  offset += sizeof(this->st_input)*input_length;
  //for( uint32_t i = 0; i < input_length; i++){
  //this->st_input =  ((uint8_t) (*(inbuffer + offset)));
  //offset += sizeof(this->st_input);
    //memcpy( &(this->input[i]), &(this->st_input), sizeof(uint8_t));
  //}
 return offset;
}

这样就跟string操作一样，共用同一个buff的空间。

嵌入式，嵌入式，字节掰成八瓣使。