def read_messages(f):
"""
Yields a series of (TopicHeader, Message) tuples, contained in the given
log file.
"""
len_size = len(messages.MessageSize(bytes=0).SerializeToString())
data = True
try:
while data:
hdr_len = messages.MessageSize()
hdr_len.ParseFromString(f.read(len_size))
hdr = messages.TopicHeader()
hdr.ParseFromString(f.read(hdr_len.bytes))
msg_len = messages.MessageSize()
msg_len.ParseFromString(f.read(len_size))
for m in messages.messages:
if messages.msgid(m) == hdr.msgid:
Message = m
break
data = f.read(msg_len.bytes)
msg = Message()
msg.ParseFromString(data)
if data:
yield hdr, msg
except DecodeError:
pass
def test_generate(self):
data = messages.Timestamp()
data.us = 42
topic = "/imu"
msg = rpc.generate_topic_injection_packet(topic, data)
to_decode, msg = msg[:MESSAGESIZE_SIZE], msg[MESSAGESIZE_SIZE:]
header_size = messages.MessageSize()
header_size.ParseFromString(to_decode)
to_decode, msg = msg[:header_size.bytes], msg[header_size.bytes:]
header = messages.TopicHeader()
header.ParseFromString(to_decode)
self.assertEqual(header.name, "/imu")
self.assertEqual(header.msgid, 1)
to_decode, msg = msg[:MESSAGESIZE_SIZE], msg[MESSAGESIZE_SIZE:]
body_size = messages.MessageSize()
body_size.ParseFromString(to_decode)
to_decode, msg = msg[:header_size.bytes], msg[header_size.bytes:]
body = messages.Timestamp()
body.ParseFromString(to_decode)
self.assertEqual(42, body.us)
def parse_packet(data):
messagesize_size = len(messages.MessageSize(bytes=0).SerializeToString())
header_size = messages.MessageSize()
header_size.ParseFromString(data[0:messagesize_size])
cut_point = messagesize_size + header_size.bytes
header_data, msg_data = data[:cut_point], data[cut_point:]
header = messages.TopicHeader()
header.ParseFromString(header_data[messagesize_size:])
for m in messages.messages:
if messages.msgid(m) == header.msgid:
msg = m()
msg.ParseFromString(msg_data)
break
else:
msg = None
return header, msg
def test_read_messages(self):
msg = messages.Timestamp(us=42)
hdr = messages.TopicHeader(msgid=messages.msgid(msg), name="ts")
def encoded_length(m):
encoded = m.SerializeToString()
res = messages.MessageSize(bytes=len(encoded))
return res.SerializeToString()
data = encoded_length(hdr) + hdr.SerializeToString()
data = data + encoded_length(msg) + msg.SerializeToString()
# Duplicate data so that it is twice here
data = io.BytesIO(data + data)
msg_list = list(read_messages(data))
self.assertEqual(len(msg_list), 2,
"There should be only two messages.")
self.assertEqual(hdr, msg_list[0][0])
self.assertEqual(msg, msg_list[0][1])
self.assertEqual(hdr, msg_list[1][0])
self.assertEqual(msg, msg_list[1][1])