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的空間。

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