@foxglove/rosmsg2-serialization
Version:
ROS 2 message serialization, for reading and writing bags and network messages
594 lines (557 loc) • 18.2 kB
text/typescript
import { parseRos2idl } from "@foxglove/ros2idl-parser";
import { parse as parseMessageDefinition } from "@foxglove/rosmsg";
import { MessageReader } from "./MessageReader";
const serializeString = (str: string): Uint8Array => {
const data = Buffer.from(str, "utf8");
const len = Buffer.alloc(4);
len.writeUInt32LE(data.byteLength + 1, 0);
return Uint8Array.from([...len, ...data, 0x00]);
};
const float32Buffer = (floats: number[]): Uint8Array => {
return new Uint8Array(Float32Array.from(floats).buffer);
};
describe("MessageReader", () => {
it.each([
[`int8 sample # lowest`, [0x80], { sample: -128 }],
[`int8 sample # highest`, [0x7f], { sample: 127 }],
[`uint8 sample # lowest`, [0x00], { sample: 0 }],
[`uint8 sample # highest`, [0xff], { sample: 255 }],
[`int16 sample # lowest`, [0x00, 0x80], { sample: -32768 }],
[`int16 sample # highest`, [0xff, 0x7f], { sample: 32767 }],
[`uint16 sample # lowest`, [0x00, 0x00], { sample: 0 }],
[`uint16 sample # highest`, [0xff, 0xff], { sample: 65535 }],
[`int32 sample # lowest`, [0x00, 0x00, 0x00, 0x80], { sample: -2147483648 }],
[`int32 sample # highest`, [0xff, 0xff, 0xff, 0x7f], { sample: 2147483647 }],
[`uint32 sample # lowest`, [0x00, 0x00, 0x00, 0x00], { sample: 0 }],
[`uint32 sample # highest`, [0xff, 0xff, 0xff, 0xff], { sample: 4294967295 }],
[
`int64 sample # lowest`,
[0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80],
{ sample: -9223372036854775808n },
],
[
`int64 sample # highest`,
[0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x7f],
{ sample: 9223372036854775807n },
],
[`uint64 sample # lowest`, [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], { sample: 0n }],
[
`uint64 sample # highest`,
[0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff],
{ sample: 18446744073709551615n },
],
[`float32 sample`, float32Buffer([5.5]), { sample: 5.5 }],
[
`float64 sample`,
// eslint-disable-next-line no-loss-of-precision
new Uint8Array(Float64Array.of(0.123456789121212121212).buffer),
// eslint-disable-next-line no-loss-of-precision
{ sample: 0.123456789121212121212 },
],
[
`int32[] arr`,
[
...[0x02, 0x00, 0x00, 0x00], // length
...new Uint8Array(Int32Array.of(3, 7).buffer),
],
{ arr: Int32Array.from([3, 7]) },
],
// unaligned access
[
`uint8 blank\nint32[] arr`,
[
0x00,
...[0x00, 0x00, 0x00], // alignment
...[0x02, 0x00, 0x00, 0x00], // length
...new Uint8Array(Int32Array.of(3, 7).buffer),
],
{ blank: 0, arr: Int32Array.from([3, 7]) },
],
[`float32[2] arr`, float32Buffer([5.5, 6.5]), { arr: Float32Array.from([5.5, 6.5]) }],
[
`uint8 blank\nfloat32[2] arr`,
[
0x00,
...[0x00, 0x00, 0x00], // alignment
...float32Buffer([5.5, 6.5]),
],
{ blank: 0, arr: Float32Array.from([5.5, 6.5]) },
],
[
`float32[] arr`,
[
...[0x02, 0x00, 0x00, 0x00], // length
...float32Buffer([5.5, 6.5]),
],
{ arr: Float32Array.from([5.5, 6.5]) },
],
[
`uint8 blank\nfloat32[] arr`,
[
0x00,
...[0x00, 0x00, 0x00], // alignment
...[0x02, 0x00, 0x00, 0x00],
...float32Buffer([5.5, 6.5]),
],
{ blank: 0, arr: Float32Array.from([5.5, 6.5]) },
],
[
`float32[] first\nfloat32[] second`,
[
...[0x02, 0x00, 0x00, 0x00], // length
...float32Buffer([5.5, 6.5]),
...[0x02, 0x00, 0x00, 0x00], // length
...float32Buffer([5.5, 6.5]),
],
{
first: Float32Array.from([5.5, 6.5]),
second: Float32Array.from([5.5, 6.5]),
},
],
[`string sample # empty string`, serializeString(""), { sample: "" }],
[`string sample # some string`, serializeString("some string"), { sample: "some string" }],
[`int8[4] first`, [0x00, 0xff, 0x80, 0x7f], { first: new Int8Array([0, -1, -128, 127]) }],
[
`int8[] first`,
[
...[0x04, 0x00, 0x00, 0x00], // length
0x00,
0xff,
0x80,
0x7f,
],
{ first: new Int8Array([0, -1, -128, 127]) },
],
[`uint8[4] first`, [0x00, 0xff, 0x80, 0x7f], { first: new Uint8Array([0, -1, -128, 127]) }],
[
`string[2] first`,
[...serializeString("one"), ...serializeString("longer string")],
{ first: ["one", "longer string"] },
],
[
`string[] first`,
[
...[0x02, 0x00, 0x00, 0x00], // length
...serializeString("one"),
...serializeString("longer string"),
],
{ first: ["one", "longer string"] },
],
// first size value after fixed size value
[`int8 first\nint8 second`, [0x80, 0x7f], { first: -128, second: 127 }],
[
`string first\nint8 second`,
[...serializeString("some string"), 0x80],
{ first: "some string", second: -128 },
],
[
`CustomType custom
============
MSG: custom_type/CustomType
uint8 first`,
[0x02],
{
custom: { first: 0x02 },
},
],
[
`CustomType[3] custom
============
MSG: custom_type/CustomType
uint8 first`,
[0x02, 0x03, 0x04],
{
custom: [{ first: 0x02 }, { first: 0x03 }, { first: 0x04 }],
},
],
[
`CustomType[] custom
============
MSG: custom_type/CustomType
uint8 first`,
[
...[0x03, 0x00, 0x00, 0x00], // length
0x02,
0x03,
0x04,
],
{
custom: [{ first: 0x02 }, { first: 0x03 }, { first: 0x04 }],
},
],
// ignore constants
[
`int8 STATUS_ONE = 1
int8 STATUS_TWO = 2
int8 status`,
[0x02],
{ status: 2 },
],
// An array of custom types which themselves have a custom type
// This tests an array's ability to properly size custom types
[
`CustomType[] custom
============
MSG: custom_type/CustomType
MoreCustom another
============
MSG: custom_type/MoreCustom
uint8 field`,
[
...[0x03, 0x00, 0x00, 0x00], // length
0x02,
0x03,
0x04,
],
{
custom: [
{ another: { field: 0x02 } },
{ another: { field: 0x03 } },
{ another: { field: 0x04 } },
],
},
],
])(
"should deserialize %s",
(msgDef: string, arr: Iterable<number>, expected: Record<string, unknown>) => {
const buffer = Uint8Array.from([0, 1, 0, 0, ...arr]);
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
// check that our message matches the object
expect(read).toEqual(expected);
},
);
it.each([
[
`time stamp`,
[0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00],
{ stamp: { sec: 0, nanosec: 1 } },
{ stamp: { sec: 0, nsec: 1 } },
],
[
`duration stamp`,
[0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00],
{ stamp: { sec: 0, nanosec: 1 } },
{ stamp: { sec: 0, nsec: 1 } },
],
[
`time[1] arr`,
[0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00],
{ arr: [{ sec: 1, nanosec: 2 }] },
{ arr: [{ sec: 1, nsec: 2 }] },
],
[
`duration[1] arr`,
[0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00],
{ arr: [{ sec: 1, nanosec: 2 }] },
{ arr: [{ sec: 1, nsec: 2 }] },
],
[
`time[] arr`,
[0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00],
{ arr: [{ sec: 2, nanosec: 3 }] },
{ arr: [{ sec: 2, nsec: 3 }] },
],
[
`duration[] arr`,
[0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00],
{ arr: [{ sec: 2, nanosec: 3 }] },
{ arr: [{ sec: 2, nsec: 3 }] },
],
// unaligned access
[
`uint8 blank\ntime[] arr`,
[
0x00,
...[0x00, 0x00, 0x00], // alignment
...[0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00],
],
{ blank: 0, arr: [{ sec: 2, nanosec: 3 }] },
{ blank: 0, arr: [{ sec: 2, nsec: 3 }] },
],
])(
"should deserialize %s correctly based on specified time type",
(
msgDef: string,
arr: Iterable<number>,
expected: Record<string, unknown>,
ros1Expected: Record<string, unknown>,
) => {
const buffer = Uint8Array.from([0, 1, 0, 0, ...arr]);
expect(
new MessageReader(parseMessageDefinition(msgDef, { ros2: true })).readMessage(buffer),
).toEqual(expected);
expect(
new MessageReader(parseMessageDefinition(msgDef, { ros2: true }), {
timeType: "sec,nanosec",
}).readMessage(buffer),
).toEqual(expected);
expect(
new MessageReader(parseMessageDefinition(msgDef, { ros2: true }), {
timeType: "sec,nsec",
}).readMessage(buffer),
).toEqual(ros1Expected);
},
);
it("should deserialize a ROS 2 log message", () => {
const buffer = Uint8Array.from(
Buffer.from(
"00010000fb65865e80faae0614000000120000006d696e696d616c5f7075626c69736865720000001e0000005075626c697368696e673a202748656c6c6f2c20776f726c64212030270000004c0000002f6f70742f726f73325f77732f656c6f7175656e742f7372632f726f73322f6578616d706c65732f72636c6370702f6d696e696d616c5f7075626c69736865722f6c616d6264612e637070000b0000006f70657261746f722829007326000000",
"hex",
),
);
const msgDef = `
byte DEBUG=10
byte INFO=20
byte WARN=30
byte ERROR=40
byte FATAL=50
##
## Fields
##
builtin_interfaces/Time stamp
uint8 level
string name # name of the node
string msg # message
string file # file the message came from
string function # function the message came from
uint32 line # line the message came from
`;
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
expect(read).toEqual({
stamp: { sec: 1585866235, nanosec: 112130688 },
level: 20,
name: "minimal_publisher",
msg: "Publishing: 'Hello, world! 0'",
file: "/opt/ros2_ws/eloquent/src/ros2/examples/rclcpp/minimal_publisher/lambda.cpp",
function: "operator()",
line: 38,
});
});
it("should deserialize a ROS 2 tf2_msgs/TFMessage", () => {
const buffer = Uint8Array.from(
Buffer.from(
"0001000001000000286fae6169ddd73108000000747572746c6531000e000000747572746c65315f616865616400000000000000000000000000f03f00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000f03f",
"hex",
),
);
const msgDef = `
geometry_msgs/TransformStamped[] transforms
================================================================================
MSG: geometry_msgs/TransformStamped
Header header
string child_frame_id # the frame id of the child frame
Transform transform
================================================================================
MSG: std_msgs/Header
time stamp
string frame_id
================================================================================
MSG: geometry_msgs/Transform
Vector3 translation
Quaternion rotation
================================================================================
MSG: geometry_msgs/Vector3
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Quaternion
float64 x
float64 y
float64 z
float64 w
`;
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
expect(read).toEqual({
transforms: [
{
header: {
stamp: { sec: 1638821672, nanosec: 836230505 },
frame_id: "turtle1",
},
child_frame_id: "turtle1_ahead",
transform: {
translation: { x: 1, y: 0, z: 0 },
rotation: { x: 0, y: 0, z: 0, w: 1 },
},
},
],
});
});
it("should deserialize ros2idl tf2_msg/TFMessage", () => {
// same buffer as above
const buffer = Uint8Array.from(
Buffer.from(
"0001000001000000286fae6169ddd73108000000747572746c6531000e000000747572746c65315f616865616400000000000000000000000000f03f00000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000f03f",
"hex",
),
);
const msgDef = `
================================================================================
IDL: geometry_msgs/msg/Transforms
module geometry_msgs {
module msg {
struct Transforms {
sequence<geometry_msgs::msg::TransformStamped> transforms;
};
};
};
================================================================================
IDL: geometry_msgs/msg/TransformStamped
module geometry_msgs {
module msg {
struct TransformStamped {
std_msgs::msg::Header header;
string child_frame_id; // the frame id of the child frame
geometry_msgs::msg::Transform transform;
};
};
};
================================================================================
IDL: std_msgs/msg/Header
module std_msgs {
module msg {
struct Header {
builtin_interfaces::Time stamp;
string frame_id;
};
};
};
================================================================================
IDL: geometry_msgs/msg/Transform
module geometry_msgs {
module msg {
struct Transform {
geometry_msgs::msg::Vector3 translation;
geometry_msgs::msg::Quaternion rotation;
};
};
};
================================================================================
IDL: geometry_msgs/msg/Vector3
module geometry_msgs {
module msg {
struct Vector3 {
double x;
double y;
double z;
};
};
};
================================================================================
IDL: geometry_msgs/msg/Quaternion
module geometry_msgs {
module msg {
struct Quaternion {
double x;
double y;
double z;
double w;
};
};
};
================================================================================
IDL: builtin_interfaces/Time
// Normally added when generating idl schemas
module builtin_interfaces {
struct Time {
int32 sec;
uint32 nsec;
};
};
`;
const reader = new MessageReader(parseRos2idl(msgDef));
const read = reader.readMessage(buffer);
expect(read).toEqual({
transforms: [
{
header: {
stamp: { sec: 1638821672, nsec: 836230505 },
frame_id: "turtle1",
},
child_frame_id: "turtle1_ahead",
transform: {
translation: { x: 1, y: 0, z: 0 },
rotation: { x: 0, y: 0, z: 0, w: 1 },
},
},
],
});
});
it("should deserialize an empty ROS 2 msg (e.g. std_msgs/msg/Empty)", () => {
const buffer = Uint8Array.from(Buffer.from("0001000000", "hex"));
const msgDef = ``;
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
expect(read).toEqual({});
});
it("should deserialize a custom msg with a std_msgs/msg/Empty field followed by uint8", () => {
// Note: ROS/FastDDS seems to add 2 extra padding bytes at the end
const buffer = Uint8Array.from(Buffer.from("00010000007b0000", "hex"));
const msgDef = `
std_msgs/msg/Empty empty
uint8 uint_8_field
================================================================================
MSG: std_msgs/msg/Empty
`;
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
expect(read).toEqual({ empty: {}, uint_8_field: 123 });
});
it("should deserialize a custom msg with a std_msgs/msg/Empty field followed by int32", () => {
const buffer = Uint8Array.from(Buffer.from("00010000000000007b000001", "hex"));
const msgDef = `
std_msgs/msg/Empty empty
int32 int_32_field
================================================================================
MSG: std_msgs/msg/Empty
`;
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
expect(read).toEqual({ empty: {}, int_32_field: 16777339 });
});
it("should deserialize a custom msg with an empty message (with constants) followed by int32", () => {
const buffer = Uint8Array.from(Buffer.from("00010000000000007b000001", "hex"));
const msgDef = `
custom_msgs/msg/Nothing empty
int32 int_32_field
================================================================================
MSG: custom_msgs/msg/Nothing
int32 EXAMPLE=123
`;
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
const read = reader.readMessage(buffer);
expect(read).toEqual({ empty: {}, int_32_field: 16777339 });
});
it("ros2idl should choose non-constant root definition", () => {
const data = [0x02];
const buffer = Uint8Array.from([0, 1, 0, 0, ...data]);
const msgDef = `
module a {
module b {
const int8 STATUS_ONE = 1;
const int8 STATUS_TWO = 2;
};
struct c {
int8 status;
};
};
`;
const reader = new MessageReader(parseRos2idl(msgDef));
const read = reader.readMessage(buffer);
expect(read).toEqual({ status: 2 });
});
it.each(["wstring field", "wstring[] field"])(
"should throw exepction when encountering wstring fields",
(msgDef) => {
const buffer = Uint8Array.from(Buffer.from("00010000000000007b000000", "hex"));
const reader = new MessageReader(parseMessageDefinition(msgDef, { ros2: true }));
expect(() => reader.readMessage(buffer)).toThrow(
"wstring is implementation-defined and therefore not supported",
);
},
);
});