Serializing SickScanPointCloudMsg for gRPC protocol buffers.

[gsainsbury86]

Hi,

I'm working with the Python API and a SICK LMS4000. In reading the examples, there is a comment about the conversion to XYZ and visulisation being too compute-intensive to be done for every message. I would like to still keep the messages so that some post-processing can be completed after scanning.

I am working with gRPC and Protocol Buffers. In the callback function, I think have two options - I can either encode/pickle/serialize the entire SickScanPointCloudMsg as bytes and decode it in my client, or I can try to redefine the message structure in the Protocol Buffers language. Is there an already-implemeted way to serialize the message for transfer?

Thanks

[rostest]

Thanks for your feedback. Currently there is no predefined way implemented to serialize the messages for further transmission.

The message classes are derived from ctypes.Structure, which supports serialization. The solution suggested in https://stackoverflow.com/questions/34533409/serializing-a-c-struct-in-python-and-sending-over-a-socket looks promising (I have not used it). Serialization to resp. from a byte array using struct.pack() and struct.unpack() should be a possible and effective way.

[gsainsbury86]

Thanks. I was able to implement a method to serialize it into the protocol buffer message. I had to make some small changes to the datatypes but it seems to work correctly to later do the conversion to xyz points. Here is the code I used.

syntax = "proto3";

message SickScanPointCloudMsg {
  message SickScanHeader {
    uint32 seq = 1;                         // sequence ID: consecutively increasing ID
    uint32 timestamp_sec = 2;               // seconds part of message timestamps: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
    uint32 timestamp_nsec = 3;              // seconds part of message timestamps: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
    bytes frame_id = 4;                    // Frame this data is associated with
  }

  message SickScanPointFieldMsg {
    bytes name = 1;                   // Name of field (max. length 256 characters)
    uint32 offset = 2;                 // Offset from start of point struct
    uint32 datatype = 3;               // Datatype enumeration (see SickScanNativeDataType), equivalent to type enum in ros::sensor_msgs::PointField
    uint32 count = 4;                  // How many elements in the field
  }

  message SickScanUint8Array {
    uint64 capacity = 1;                           // Number of allocated elements, i.e. max. number of elements in buffer
    uint64 size = 2;                               // Number of currently used elements in the buffer
    bytes buffer = 3;                              // Memory, data in plain order and system endianess
  }

  message SickScanPointFieldArray {
    uint64 capacity = 1;                                 // Number of allocated elements, i.e. max. number of elements in buffer
    uint64 size = 2;                                     // Number of currently used elements in the buffer
    repeated SickScanPointFieldMsg buffer = 3;         // Array of SickScanPointFieldMsg
  }

  SickScanHeader header = 1;
  uint32 height = 2;
  uint32 width = 3;
  SickScanPointFieldArray fields = 4;
  uint32 is_bigendian = 5;
  uint32 point_step = 6;
  uint32 row_step = 7;
  SickScanUint8Array data = 8;
  uint32 is_dense = 9;
  int32 num_echos = 10;
  int32 segment_idx = 11;
}

def to_proto(message_contents):
    protocolbuf = lidar_pb2.SickScanPointCloudMsg()

    protocolbuf.height = message_contents.height
    protocolbuf.width = message_contents.width
    protocolbuf.is_bigendian = message_contents.is_bigendian
    protocolbuf.point_step = message_contents.point_step
    protocolbuf.row_step = message_contents.row_step
    protocolbuf.is_dense = message_contents.is_dense
    protocolbuf.num_echos = message_contents.num_echos
    protocolbuf.segment_idx = message_contents.segment_idx

    protocol_buf_header = lidar_pb2.SickScanPointCloudMsg.SickScanHeader()
    protocol_buf_header.seq = message_contents.header.seq
    protocol_buf_header.timestamp_sec = message_contents.header.timestamp_sec
    protocol_buf_header.timestamp_nsec = message_contents.header.timestamp_nsec
    protocol_buf_header.frame_id = bytes(message_contents.header.frame_id)

    protocol_buf_data = lidar_pb2.SickScanPointCloudMsg.SickScanUint8Array()
    protocol_buf_data.capacity = message_contents.data.capacity
    protocol_buf_data.size = message_contents.data.size

    buffer = ctypes.cast(
        message_contents.data.buffer,
        ctypes.POINTER(ctypes.c_uint8 * message_contents.data.size),
    ).contents
    protocol_buf_data.buffer = bytes(buffer)

    protocol_buf_fields = lidar_pb2.SickScanPointCloudMsg.SickScanPointFieldArray()

    protocol_buf_fields.capacity = message_contents.fields.capacity
    protocol_buf_fields.size = message_contents.fields.size

    num_fields = message_contents.fields.size

    for n in range(num_fields):

        field_message_for_pb = lidar_pb2.SickScanPointCloudMsg.SickScanPointFieldMsg()

        field_message_for_pb.name = message_contents.fields.buffer[n].name
        field_message_for_pb.offset = message_contents.fields.buffer[n].offset
        field_message_for_pb.datatype = message_contents.fields.buffer[n].datatype
        field_message_for_pb.count = message_contents.fields.buffer[n].count

        protocol_buf_fields.buffer.append(field_message_for_pb)

    protocolbuf.header.CopyFrom(protocol_buf_header)
    protocolbuf.data.CopyFrom(protocol_buf_data)
    protocolbuf.fields.CopyFrom(protocol_buf_fields)

    return protocolbuf

def from_proto(protocol_message):

    header = SickScanHeader(
        seq=protocol_message.header.seq,
        timestamp_sec=protocol_message.header.timestamp_sec,
        timestamp_nsec=protocol_message.header.timestamp_nsec,
        frame_id=protocol_message.header.frame_id,
    )

    num_fields = protocol_message.fields.size
    array = SickScanPointFieldMsg * num_fields
    elements = array()

    for n in range(num_fields):
        field = SickScanPointFieldMsg(
            name=protocol_message.fields.buffer[n].name,
            offset=protocol_message.fields.buffer[n].offset,
            datatype=protocol_message.fields.buffer[n].datatype,
            count=protocol_message.fields.buffer[n].count,
        )
        elements[n] = field

    fields = SickScanPointFieldArray(
        capacity=protocol_message.fields.capacity,
        size=protocol_message.fields.size,
        buffer=elements,
    )

    byte_data = bytes(protocol_message.data.buffer)
    size = len(byte_data)
    buffer_copy = bytearray(byte_data)
    buffer_type = ctypes.c_uint8 * size
    buffer_instance = buffer_type.from_buffer(buffer_copy)

    data = SickScanUint8Array(
        capacity=protocol_message.data.capacity,
        size=protocol_message.data.size,
        buffer=buffer_instance,
    )

    message_contents = SickScanPointCloudMsg(
        header=header,
        height=protocol_message.height,
        width=protocol_message.width,
        fields=fields,
        is_bigendian=protocol_message.is_bigendian,
        point_step=protocol_message.point_step,
        row_step=protocol_message.row_step,
        data=data,
        is_dense=protocol_message.is_dense,
        num_echos=protocol_message.num_echos,
        segment_idx=protocol_message.segment_idx,
    )

    return message_contents