Supported 32bit array lengths in python make_libraries script

[sevenbitbyte]

This patch enables de/serialization of ROS messages with arrays containing greater than 256 elements. This is important for many types such as sensor_msgs/Image. There now exist many controllers supported under ROS such as the teensy 3.1 with large amounts of RAM. Correct header generation is the only thing currently preventing developers interested in utilizing these powerful devices from quickly and easily integrating with ROS.

For example this patch created the following header for sensor_msgs/Image:

#ifndef _ROS_sensor_msgs_Image_h
#define _ROS_sensor_msgs_Image_h

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

namespace sensor_msgs
{

  class Image : public ros::Msg
  {
    public:
      std_msgs::Header header;
      uint32_t height;
      uint32_t width;
      const char* encoding;
      uint8_t is_bigendian;
      uint32_t step;
      uint32_t data_length;
      uint8_t st_data;
      uint8_t * data;

    Image():
      header(),
      height(0),
      width(0),
      encoding(""),
      is_bigendian(0),
      step(0),
      data_length(0), data(NULL)
    {
    }

    virtual int serialize(unsigned char *outbuffer) const
    {
      int offset = 0;
      offset += this->header.serialize(outbuffer + offset);
      *(outbuffer + offset + 0) = (this->height >> (8 * 0)) & 0xFF;
      *(outbuffer + offset + 1) = (this->height >> (8 * 1)) & 0xFF;
      *(outbuffer + offset + 2) = (this->height >> (8 * 2)) & 0xFF;
      *(outbuffer + offset + 3) = (this->height >> (8 * 3)) & 0xFF;
      offset += sizeof(this->height);
      *(outbuffer + offset + 0) = (this->width >> (8 * 0)) & 0xFF;
      *(outbuffer + offset + 1) = (this->width >> (8 * 1)) & 0xFF;
      *(outbuffer + offset + 2) = (this->width >> (8 * 2)) & 0xFF;
      *(outbuffer + offset + 3) = (this->width >> (8 * 3)) & 0xFF;
      offset += sizeof(this->width);
      uint32_t length_encoding = strlen(this->encoding);
      memcpy(outbuffer + offset, &length_encoding, sizeof(uint32_t));
      offset += 4;
      memcpy(outbuffer + offset, this->encoding, length_encoding);
      offset += length_encoding;
      *(outbuffer + offset + 0) = (this->is_bigendian >> (8 * 0)) & 0xFF;
      offset += sizeof(this->is_bigendian);
      *(outbuffer + offset + 0) = (this->step >> (8 * 0)) & 0xFF;
      *(outbuffer + offset + 1) = (this->step >> (8 * 1)) & 0xFF;
      *(outbuffer + offset + 2) = (this->step >> (8 * 2)) & 0xFF;
      *(outbuffer + offset + 3) = (this->step >> (8 * 3)) & 0xFF;
      offset += sizeof(this->step);
      *(outbuffer + offset + 0) = (this->data_length >> (8 * 0)) & 0xFF;
      *(outbuffer + offset + 1) = (this->data_length >> (8 * 1)) & 0xFF;
      *(outbuffer + offset + 2) = (this->data_length >> (8 * 2)) & 0xFF;
      *(outbuffer + offset + 3) = (this->data_length >> (8 * 3)) & 0xFF;
      offset += sizeof(this->data_length);
      for( uint32_t i = 0; i < data_length; i++){
      *(outbuffer + offset + 0) = (this->data[i] >> (8 * 0)) & 0xFF;
      offset += sizeof(this->data[i]);
      }
      return offset;
    }

    virtual int deserialize(unsigned char *inbuffer)
    {
      int offset = 0;
      offset += this->header.deserialize(inbuffer + offset);
      this->height =  ((uint32_t) (*(inbuffer + offset)));
      this->height |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
      this->height |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
      this->height |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
      offset += sizeof(this->height);
      this->width =  ((uint32_t) (*(inbuffer + offset)));
      this->width |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
      this->width |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
      this->width |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
      offset += sizeof(this->width);
      uint32_t length_encoding;
      memcpy(&length_encoding, (inbuffer + offset), sizeof(uint32_t));
      offset += 4;
      for(unsigned int k= offset; k< offset+length_encoding; ++k){
          inbuffer[k-1]=inbuffer[k];
      }
      inbuffer[offset+length_encoding-1]=0;
      this->encoding = (char *)(inbuffer + offset-1);
      offset += length_encoding;
      this->is_bigendian =  ((uint8_t) (*(inbuffer + offset)));
      offset += sizeof(this->is_bigendian);
      this->step =  ((uint32_t) (*(inbuffer + offset)));
      this->step |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1);
      this->step |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2);
      this->step |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3);
      offset += sizeof(this->step);
      uint32_t data_lengthT = ((uint32_t) (*(inbuffer + offset))); 
      data_lengthT |= ((uint32_t) (*(inbuffer + offset + 1))) << (8 * 1); 
      data_lengthT |= ((uint32_t) (*(inbuffer + offset + 2))) << (8 * 2); 
      data_lengthT |= ((uint32_t) (*(inbuffer + offset + 3))) << (8 * 3); 
      offset += sizeof(this->data_length);
      if(data_lengthT > data_length)
        this->data = (uint8_t*)realloc(this->data, data_lengthT * sizeof(uint8_t));
      data_length = data_lengthT;
      for( uint32_t i = 0; i < data_length; i++){
      this->st_data =  ((uint8_t) (*(inbuffer + offset)));
      offset += sizeof(this->st_data);
        memcpy( &(this->data[i]), &(this->st_data), sizeof(uint8_t));
      }
     return offset;
    }

    const char * getType(){ return "sensor_msgs/Image"; };
    const char * getMD5(){ return "060021388200f6f0f447d0fcd9c64743"; };

  };

}
#endif

[mikepurvis]

This looks right to me. Have you checked both the publish and subscribe side?

[mikepurvis]

Also, you're still going to be limited by an overall message length limit of 256 bytes, as discussed in #130.

Edit: Wait, I'm an idiot— the length is two bytes, so you're good up to 64k.

[sevenbitbyte]

I've tested both subscribing and publishing a std_msgs/Image which was 32x16xRGB (1536 bytes long). The program simply received and echo'd any images sent to it and both sides displayed the image on a screen. I did have to increase the RX and TX buffer size, in my test case I set both to 4KB. But the test worked out and showed no data corruption on either side.

I'm a little confused what length limit you're referring to. Is it that the total message length(counting all fields) is expected to by 256 bytes long? Is there a protocol format design document I can compare against?

[mikepurvis]

Protocol is described here: http://wiki.ros.org/rosserial/Overview/Protocol

See my edit above, re: overall message length.

[mikepurvis]

Thanks for the contribution! I'll merge as is, and then test and release for Kinetic, eventually probably Jade too.

[sevenbitbyte]

Oh awesome, glad I could help :)

[spmaniato]

@sevenbitbyte, @mikepurvis, We just run into the uint8_t array length issue. Would there be anything preventing us from applying this patch to our Indigo installation? For example, is there a reason it was not targeted to indigo-devel as well? (We'll give it a shot in the meantime.) Thanks for the patch btw 😃

[sevenbitbyte]

@spmaniato I don't see any reason this won't work on indigo. However, it does not look like this exact PR would merge cleanly on to the indigo-devel branch so I should probably double check that it works there and open a new PR. It will take me a day or two to find time to test and re-submit for indigo.

[spmaniato]

Thanks for getting back to me @sevenbitbyte Sounds good. No rush though. We ended up building jade-devel from source in our Indigo workspace. So far so good 😜