def publish(self, channel, data):
stream_id = self.stream_id[channel] # local maping of indexes
if stream_id in self.no_output:
to_write = serialize(
None) # i.e. at least there will be timestamp record
else:
to_write = serialize(data)
if stream_id in self.compressed_output:
to_write = compress(to_write)
with self.logger.lock:
timestamp = self.logger.write(stream_id, to_write)
if self._time is not None:
delay = timestamp - self._time
if delay > self.max_delay:
self.max_delay = delay
self.max_delay_timestamp = timestamp
if delay > ASSERT_QUEUE_DELAY:
caller = inspect.currentframe().f_back.f_code.co_name
self.report_error(delay=delay.total_seconds(),
channel=channel,
caller=caller)
for queue, input_channel in self.out[channel]:
queue.put((timestamp, input_channel, data))
for slot in self.slots.get(channel, []):
slot(timestamp, data)
return timestamp
def test_serialization(self):
self.assertEqual(serialize(b'\x01\x02'), b'\xc4\x02\x01\x02')
position = (51749517, 180462688)
self.assertEqual(serialize(position),
b'\x92\xce\x03\x15\xa2\x8d\xce\n\xc1\xa4`')
self.assertEqual(serialize((123.4, 'Hi')),
b'\x92\xcb@^\xd9\x99\x99\x99\x99\x9a\xa2Hi')
def test_packed_data(self):
data = [0] * 1000
packet = serialize(data)
self.assertEqual(len(packet), 1003)
self.assertEqual(deserialize(packet), data)
compressed_packet = serialize(data, compress=True)
self.assertEqual(len(compressed_packet), 22)
self.assertEqual(deserialize(compressed_packet), data)
def test_msg_pack(self):
bus = MagicMock()
dev = Radio(bus=bus, config={})
bus.reset_mock()
dev.send_data('dummy_artf', [1, 2, 3])
bus.publish.assert_called()
self.assertEqual(bus.method_calls[-1],
call.publish('radio', serialize([0, 'dummy_artf', [1, 2, 3]])))
dev.send_data('dummy_artf', [1, 2, 3])
self.assertEqual(bus.method_calls[-1],
call.publish('radio', serialize([1, 'dummy_artf', [1, 2, 3]])))
def test_log_bus_handler_inputs_onlye(self):
log_data = [
(timedelta(microseconds=10), 1, serialize([1, 2])),
(timedelta(microseconds=11), 1, serialize([3, 4, 5])),
(timedelta(microseconds=30), 2, serialize([8, 9])),
]
log = iter(log_data)
inputs = {1:'raw'}
bus = LogBusHandlerInputsOnly(log, inputs)
self.assertEqual(bus.listen(), (timedelta(microseconds=10), 'raw', [1, 2]))
bus.publish('new_channel', b'some data')
self.assertEqual(bus.listen(), (timedelta(microseconds=11), 'raw', [3, 4, 5]))
def test_on_radio(self):
# virtual world
data = [b'A0F150L', serialize([13, 'artf', ['TYPE_RESCUE_RANDY', 15982, 104845, 3080]])]
bus = MagicMock()
dev = Radio(bus=bus, config={})
bus.reset_mock()
dev.on_radio(data)
bus.publish.assert_called()
data = [b'A0F150L', serialize([0, 'pose2d', [11896, 7018, -11886]])]
bus.reset_mock()
dev.on_radio(data)
bus.publish.assert_not_called()
def test_numpy(self):
data = numpy.asarray(range(1, 100, 2), dtype=numpy.uint16)
packet = serialize(data)
self.assertEqual(len(packet), 163)
dedata = deserialize(packet)
self.assertTrue(numpy.array_equal(dedata, data))
self.assertEqual(dedata.dtype, data.dtype)
with unittest.mock.patch('osgar.lib.serialize.numpy', new=False):
with self.assertRaises(TypeError):
serialize(data)
with self.assertRaises(TypeError):
deserialize(packet)
def publish(self, channel, data):
with self.logger.lock:
stream_id = self.stream_id[channel] # local maping of indexes
if self.compressed_output:
to_write = zlib.compress(serialize(data))
else:
to_write = serialize(data)
timestamp = self.logger.write(stream_id, to_write)
for queue, input_channel in self.out[channel]:
queue.put((timestamp, input_channel, data))
for slot in self.slots.get(channel, []):
slot(timestamp, data)
return timestamp
def test_on_breadcrumb(self):
data = [29.929257253892082, -1.5821685677914703, 1.575092509709292] # xyz location of newly deployed breadcrumb
bus = MagicMock()
dev = Radio(bus=bus, config={})
bus.reset_mock()
dev.on_breadcrumb(data)
bus.publish.assert_called()
self.assertEqual(bus.method_calls[0],
call.publish('radio', serialize([0, 'breadcrumb', [29.929257253892082, -1.5821685677914703, 1.575092509709292]])))
new_msg = serialize([0, 'breadcrumb', [29.929257253892082, -1.5821685677914703, 1.575092509709292]])
bus.reset_mock()
dev.on_radio([b'A1300L', new_msg])
bus.publish.assert_called()
def test_log_bus_handler_inputs_onlye(self):
log_data = [
(timedelta(microseconds=10), 1, serialize([1, 2])),
(timedelta(microseconds=11), 1, serialize([3, 4, 5])),
(timedelta(microseconds=30), 2, serialize([8, 9])),
]
log = iter(log_data)
inputs = {1: 'raw'}
bus = LogBusHandlerInputsOnly(log, inputs)
self.assertEqual(bus.listen(),
(timedelta(microseconds=10), 'raw', [1, 2]))
bus.publish('new_channel', b'some data')
self.assertEqual(bus.listen(),
(timedelta(microseconds=11), 'raw', [3, 4, 5]))
def test_log_bus_handler(self):
log_data = [
(timedelta(microseconds=10), 1, serialize(b'(1,2)')),
(timedelta(microseconds=11), 1, serialize(b'(3,4,5)')),
(timedelta(microseconds=30), 2, serialize([8, 9])),
]
log = iter(log_data)
inputs = {1:'raw'}
outputs = {2:'can'}
bus = LogBusHandler(log, inputs, outputs)
bus.listen()
with self.assertRaises(AssertionError) as e:
bus.publish('can', b'parsed data')
self.assertEqual(str(e.exception), "(b'parsed data', [8, 9], datetime.timedelta(0, 0, 30))")
def test_log_bus_handler(self):
log_data = [
(timedelta(microseconds=10), 1, serialize(b'(1,2)')),
(timedelta(microseconds=11), 1, serialize(b'(3,4,5)')),
(timedelta(microseconds=30), 2, serialize([8, 9])),
]
log = iter(log_data)
inputs = {1: 'raw'}
outputs = {2: 'can'}
bus = LogBusHandler(log, inputs, outputs)
bus.listen()
with self.assertRaises(AssertionError) as e:
bus.publish('can', b'parsed data')
self.assertEqual(e.exception.args[0],
(b'parsed data', [8, 9], timedelta(0, 0, 30)))
def run(self):
context = zmq.Context.instance()
socket = context.socket(zmq.PUSH)
if self.is_bind_set:
socket.setsockopt(zmq.LINGER, 100) # milliseconds
socket.bind(self.endpoint)
else:
# https://stackoverflow.com/questions/7538988/zeromq-how-to-prevent-infinite-wait
socket.SNDTIMEO = int(self.timeout * 1000) # convert to milliseconds
socket.LINGER = 100 #milliseconds
socket.connect(self.endpoint)
try:
with contextlib.closing(socket):
while True:
dt, channel, data = self.bus.listen()
raw = serialize(data)
socket.send_multipart([bytes(channel, 'ascii'), raw])
except zmq.ZMQError as e:
if e.errno == zmq.EAGAIN:
pass #TODO log timeout
else:
pass #TODO log other error
except BusShutdownException:
pass
def test_wrong_publish_channel(self):
log_data = [
(timedelta(microseconds=10), 1, serialize(b'(1,2)')),
(timedelta(microseconds=30), 2, serialize(b'[8,9]')),
(timedelta(microseconds=35), 3, serialize(b'[8,9]')),
]
log = iter(log_data)
inputs = {1:'raw'}
outputs = {2:'can', 3:'can2'}
bus = LogBusHandler(log, inputs, outputs)
bus.listen()
with self.assertRaises(AssertionError) as e:
bus.publish('can2', [8, 9])
self.assertEqual(str(e.exception), "('can2', 'can', datetime.timedelta(0, 0, 30))")
with self.assertRaises(AssertionError) as e:
bus.publish('can3', [1, 2])
self.assertEqual(str(e.exception), "('can3', dict_values(['can', 'can2']))")
def publish(self, channel, data):
with self.logger.lock:
stream_id = self.stream_id[channel] # local maping of indexes
timestamp = self.logger.write(stream_id, serialize(data))
for queue, input_channel in self.out[channel]:
queue.put((timestamp, input_channel, data))
for slot in self.slots.get(channel, []):
slot(data)
return timestamp
def publish(self, channel, data):
with self.logger.lock:
stream_id = self.stream_id[channel] # local maping of indexes
timestamp = self.logger.write(stream_id, serialize(data))
for queue, input_channel in self.out[channel]:
queue.put((timestamp, input_channel, data))
for slot in self.slots.get(channel, []):
slot(data)
return timestamp
def test_wrong_publish_channel(self):
log_data = [
(timedelta(microseconds=10), 1, serialize(b'(1,2)')),
(timedelta(microseconds=30), 2, serialize(b'[8,9]')),
(timedelta(microseconds=35), 3, serialize(b'[8,9]')),
]
log = iter(log_data)
inputs = {1: 'raw'}
outputs = {2: 'can', 3: 'can2'}
bus = LogBusHandler(log, inputs, outputs)
bus.listen()
with self.assertRaises(AssertionError) as e:
bus.publish('can2', [8, 9])
self.assertEqual(e.exception.args[0],
('can2', 'can', timedelta(0, 0, 30)))
with self.assertRaises(AssertionError) as e:
bus.publish('can3', [1, 2])
self.assertEqual(e.exception.args[0], ('can3', ('can', 'can2')))
def publish_pointcloud(points, push, push_lock, channel):
assert points.height == 1, points.height
assert points.point_step == 12, points.point_step
assert points.row_step == points.width * points.point_step, (
points.row_step, points.width, points.point_step)
arr = np.frombuffer(points.data, dtype=np.float32)
points3d = arr.reshape((points.height, points.width, 3))
raw = serialize(points3d)
with push_lock:
send_msg(push, [channel, raw])
def test_pose3d(self):
bus = MagicMock()
dev = Radio(bus=bus, config={})
bus.reset_mock()
data = [[-5.999917943872727, 4.999913526023388, 0.12287766016353471], [6.229389211972107e-12, -7.943606405014296e-12, 1.5034993858140517e-12, 1.0]]
dev.on_pose3d(data)
bus.publish.assert_not_called() # not defined sim_time
dev.on_sim_time_sec(13)
dev.on_pose3d(data)
bus.publish.assert_called()
self.assertEqual(bus.method_calls[-1],
call.publish('radio', serialize([1, 'xyz', [13, [-5.999917943872727, 4.999913526023388, 0.12287766016353471]]])))
def test_array(self):
# USB read returns array('B', [...]), which fails to serialize
arr = array('B', [1, 2, 3])
b_arr = serialize(bytes(arr))
self.assertEqual(deserialize(b_arr), bytes([1, 2, 3]))
def publish_scan(scan, push, push_lock, channel):
scan = [(0 if math.isinf(x) else int(1000 * x)) for x in scan.ranges]
raw = serialize(scan)
with push_lock:
send_msg(push, [channel, raw])
def test_array(self):
# USB read returns array('B', [...]), which fails to serialize
arr = array('B', [1, 2, 3])
b_arr = serialize(bytes(arr))
self.assertEqual(deserialize(b_arr), bytes([1, 2, 3]))
def send_data(self, channel, data):
raw = serialize([self.message_counter, channel, data])
self.publish('radio', raw)
self.message_counter += 1
def test_serialization(self):
self.assertEqual(serialize(b'\x01\x02'), b'\xc4\x02\x01\x02')
position = (51749517, 180462688)
self.assertEqual(serialize(position), b'\x92\xce\x03\x15\xa2\x8d\xce\n\xc1\xa4`')
self.assertEqual(serialize((123.4, 'Hi')), b'\x92\xcb@^\xd9\x99\x99\x99\x99\x9a\xa2Hi')
