Efficient data transport using binary datatype - ashBabu/Utilities GitHub Wiki

In ROS, the data that are heavy like pointclouds are encoded as binary. An example is

ros2 topic echo /livox_mid360/points --once
header:
  stamp:
    sec: 16
    nanosec: 450000000
  frame_id: livox_frame
height: 1000
width: 1000
fields:
- name: x
  offset: 0
  datatype: 7
  count: 1
- name: y
  offset: 4
  datatype: 7
  count: 1
- name: z
  offset: 8
  datatype: 7
  count: 1
- name: intensity
  offset: 16
  datatype: 7
  count: 1
- name: ring
  offset: 24
  datatype: 4
  count: 1
is_bigendian: false
point_step: 32
row_step: 32000
data:
- 110
- 106
- 22

In the above ROS 2, the datatype numbers are constants.

Datatype 7: FLOAT32 (4 bytes each)

Datatype 4: UINT16 (2 bytes each)

The point_step: 32 represents that every single point in the pointcloud takes up exactly 32 bytes of memory.

Bytes	Field	Type	Size (Bytes)	Python
0	x	FLOAT32	4	f
4	y	FLOAT32	4	f
8	z	FLOAT32	4	f
12-15	Padding	-	4	4x
16	intensity	FLOAT32	4	f
20-23	Padding	-	4	4x
24	ring	UINT16	2	H
26-31	Padding	-	6	6x

Code	C Type	Python Type	Size (Bytes)
f	float	float	4
d	double	float	8
H	unsigned short	integer	2
I	unsigned int	integer	4
x	pad byte	no value	1

is_bigendign:false (Little Endian) is represented as < in python.

Since it is little endian and for x, y, z, starting at 0, 4, and 8, we need <fff. Then 4 padding makes it <fff4x, followed by intesity, 4 padding, u16 ring and 6 padding makes it <fff4xf4xH6x.

In Python, this would be

import struct

# Let's say 'raw_point' is a chunk of 32 bytes from your data
# we convert the list of numbers into actual bytes
point_bytes = bytes(raw_point) 

# Now we unpack using our map
x, y, z, intensity, ring = struct.unpack('<fff4xf4xH6x', point_bytes)

print(f"X: {x}, Y: {y}, Z: {z}, Intensity: {intensity}, Ring: {ring}")

Using Numpy, this will be

import numpy as np
timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9
point_dtype = np.dtype({
    'names':   ['x', 'y', 'z', 'intensity', ...],
    'formats': ['f4', 'f4', 'f4', 'f4', 'u2'],
    'offsets': [0, 4, 8, 16, 24],
    'itemsize': msg.point_step  # 32
})

raw_data = msg.data
full_cloud = np.frombuffer(raw_data, dtype=point_dtype)
xyz = np.stack([full_cloud['x'], full_cloud['y'], full_cloud['z']], axis=1)  # take out just x, y, z
mask = np.all(np.isfinite(xyz), axis=1)  # remove NANs and INFs
xyz = xyz[mask]  # Array of valid numbers only

#### Optional slicing if pointcloud has millions of points ##########
n = xyz.shape[0]
MAX_POINTS = 100_000
if n > MAX_POINTS:
    step_size = n // MAX_POINTS
    xyz = xyz[::step_size][:MAX_POINTS] 
    n = xyz.shape[0]
#############################################################
rotation_matrix = self.get_rotxyz('x', -90)
points = xyz @ rotation_matrix.T
###
If any operations are performed on the data, it needs to be converted
back to contiguous array
###
points = np.ascontiguousarray(points, dtype=np.float32)

header = struct.pack("<dI", timestamp, points.shape[0])
payload = header + points.tobytes()

A note on the header

• header = struct.pack("<dI", timestamp, points.shape[0])

This line is essentially creating a custom binary header for your network packet (data to be sent).

Character	Meaning	Data Type	Size
<	Little Endian	Standard byte order for Intel/AMD processors	0 bytes
d	Double	64-bit Floating Point (the timestamp)	8 bytes
I	Unsigned Int	32-bit Integer (the number of points n)	4 bytes

header becomes a bytes object exactly 12 bytes long (8 for the double + 4 for the int)

In the aiortc server, this can be unpacked as

const timestamp = dataView.getFloat64(0, true); // Read the 'd'
const numPoints = dataView.getUint32(8, true);  // Read the 'I'