From b40ee3ac1a78f38d1d38da0b2e1ef776c938c876 Mon Sep 17 00:00:00 2001 From: Andrey Pokhilko Date: Fri, 15 Sep 2017 16:42:10 +0300 Subject: [PATCH 1/3] Refactoring --- pylgbst/comms.py | 35 ++++++++++++----------------------- tests.py | 3 ++- vernie.py | 1 + 3 files changed, 15 insertions(+), 24 deletions(-) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index 70a6a52..be30221 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -8,36 +8,25 @@ import sys import time import traceback from abc import abstractmethod +from binascii import unhexlify from gattlib import DiscoveryService, GATTRequester from threading import Thread +import binascii + from pylgbst.constants import LEGO_MOVE_HUB, MSG_DEVICE_SHUTDOWN log = logging.getLogger('transport') + +def str2hex(data): # TODO: eliminate it + return binascii.hexlify(data).decode("utf8") + + if sys.version_info[0] == 2: - def str2hex(data): - return data.encode("hex") - - - def hex2str(data): - return data.decode("hex") - - def get_byte(seq, index): return ord(seq[index]) else: - import binascii - - - def str2hex(data): - return binascii.hexlify(data).decode("utf8") - - - def hex2str(data): - return binascii.unhexlify(data) - - def get_byte(seq, index): return seq[index] @@ -167,7 +156,7 @@ class DebugServer(object): self.sock.close() def _notify_dummy(self, handle, data): - log.debug("Notification from handle %s: %s", handle, hex2str(data)) + log.debug("Notification from handle %s: %s", handle, unhexlify(data)) self._check_shutdown(data) def _notify(self, conn, handle, data): @@ -215,7 +204,7 @@ class DebugServer(object): def _handle_cmd(self, cmd): if cmd['type'] == 'write': - self.ble.write(cmd['handle'], hex2str(cmd['data'])) + self.ble.write(cmd['handle'], unhexlify(cmd['data'])) elif cmd['type'] == 'read': data = self.ble.read(cmd['handle']) payload = {"type": "response", "data": str2hex(data)} @@ -264,7 +253,7 @@ class DebugServerConnection(Connection): for item in self.incoming: if item['type'] == 'response': self.incoming.remove(item) - return hex2str(item['data']) + return unhexlify(item['data']) time.sleep(0.1) def _send(self, payload): @@ -286,7 +275,7 @@ class DebugServerConnection(Connection): if line: item = json.loads(line) if item['type'] == 'notification' and self.notify_handler: - self.notify_handler(item['handle'], hex2str(item['data'])) + self.notify_handler(item['handle'], unhexlify(item['data'])) elif item['type'] == 'response': self.incoming.append(item) else: diff --git a/tests.py b/tests.py index f5bfaab..43cded6 100644 --- a/tests.py +++ b/tests.py @@ -1,4 +1,5 @@ import unittest +from binascii import unhexlify from pylgbst import * @@ -33,7 +34,7 @@ class ConnectionMock(Connection): if self.notification_handler: while self.notifications: handle, data = self.notifications.pop(0) - self.notification_handler(handle, hex2str(data.replace(' ', ''))) + self.notification_handler(handle, unhexlify(data.replace(' ', ''))) time.sleep(0.1) self.finished = True diff --git a/vernie.py b/vernie.py index acde3ea..a68654c 100644 --- a/vernie.py +++ b/vernie.py @@ -22,6 +22,7 @@ class Vernie(MoveHub): self._color_detected = COLOR_NONE self._sensor_distance = 10 self.color_distance_sensor.subscribe(self._color_distance_data) + log.info("Vernie is ready.") def _external_motor_data(self, data): log.debug("External motor position: %s", data) From 4e6c7bf125debb2f505522a6f4f96411a04c9814 Mon Sep 17 00:00:00 2001 From: Andrey Pokhilko Date: Fri, 15 Sep 2017 16:57:12 +0300 Subject: [PATCH 2/3] Fix tests --- pylgbst/comms.py | 7 +++++++ tests.py | 14 +++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index be30221..28e7a56 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -283,3 +283,10 @@ class DebugServerConnection(Connection): def set_notify_handler(self, handler): self.notify_handler = handler + + +def start_debug_server(iface="hci0", port=9090): + ble = BLEConnection() + ble.connect(iface) + server = DebugServer(ble) + server.start(port) diff --git a/tests.py b/tests.py index 43cded6..dc9a735 100644 --- a/tests.py +++ b/tests.py @@ -70,7 +70,7 @@ class GeneralTest(unittest.TestCase): hub = HubMock() led = LED(hub, PORT_LED) led.set_color(COLOR_RED) - self.assertEqual("0801813211510009", hub.connection.writes[0][1]) + self.assertEqual("0801813201510009", hub.connection.writes[0][1]) def test_tilt_sensor(self): hub = HubMock() @@ -104,10 +104,14 @@ class GeneralTest(unittest.TestCase): conn = ConnectionMock() conn.notifications.append((14, '1b0e00 0900 04 39 0227003738')) hub = HubMock(conn) - motor = EncodedMotor(hub, PORT_AB) - motor.timed(1.5) + time.sleep(0.1) + + conn.notifications.append((14, '1b0e00050082390a')) + hub.motor_AB.timed(1.5) self.assertEqual("0d018139110adc056464647f03", conn.writes[0][1]) - motor.angled(90) + + conn.notifications.append((14, '1b0e00050082390a')) + hub.motor_AB.angled(90) self.assertEqual("0f018139110c5a0000006464647f03", conn.writes[1][1]) def test_capabilities(self): @@ -134,7 +138,7 @@ class GeneralTest(unittest.TestCase): hub.connection.notifications.append((HANDLE, '1b0e000f0004010125000000001000000010')) time.sleep(1) - def callback(color, unk1, unk2): + def callback(color, unk1, unk2=None): name = COLORS[color] if color is not None else 'NONE' log.info("Color: %s %s %s", name, unk1, unk2) From f5b9b66e600f466aeb0ee11ab89d00976a2052bb Mon Sep 17 00:00:00 2001 From: Andrey Pokhilko Date: Fri, 15 Sep 2017 17:52:50 +0300 Subject: [PATCH 3/3] Mark progress --- pylgbst/__init__.py | 1 + pylgbst/comms.py | 53 +++++++++++++++++++++++++++--------------- pylgbst/peripherals.py | 7 ++++-- vernie.py | 5 ++++ 4 files changed, 45 insertions(+), 21 deletions(-) diff --git a/pylgbst/__init__.py b/pylgbst/__init__.py index b9ee15f..e3d09d2 100644 --- a/pylgbst/__init__.py +++ b/pylgbst/__init__.py @@ -111,6 +111,7 @@ class MoveHub(object): self.devices[port].finished() elif status == STATUS_CONFLICT: log.warning("Command conflict on port %s", PORTS[port]) + self.devices[port].finished() else: log.warning("Unhandled status value: 0x%x", status) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index 28e7a56..a5a5c8b 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -1,19 +1,19 @@ """ This package holds communication aspects """ +import binascii import json import logging import socket import sys import time import traceback +from Queue import Queue from abc import abstractmethod from binascii import unhexlify from gattlib import DiscoveryService, GATTRequester from threading import Thread -import binascii - from pylgbst.constants import LEGO_MOVE_HUB, MSG_DEVICE_SHUTDOWN log = logging.getLogger('transport') @@ -42,14 +42,31 @@ class Requester(GATTRequester): super(Requester, self).__init__(p_object, *args, **kwargs) self.notification_sink = None + self._notify_queue = Queue() + self._notifier_thread = Thread(target=self._dispatch_notifications) + self._notifier_thread.setDaemon(True) + self._notifier_thread.setName("Notify queue dispatcher") + self._notifier_thread.start() + def on_notification(self, handle, data): # log.debug("requester notified, sink: %s", self.notification_sink) - if self.notification_sink: - self.notification_sink(handle, data) + self._notify_queue.put((handle, data)) def on_indication(self, handle, data): log.debug("Indication on handle %s: %s", handle, str2hex(data)) + def _dispatch_notifications(self): + while True: + handle, data = self._notify_queue.get() + if self.notification_sink: + try: + self.notification_sink(handle, data) + except BaseException: + log.warning("Failed to dispatch notification: %s", str2hex(data)) + log.warning("Failed to dispatch notification: %s", traceback.format_exc()) + else: + log.warning("Dropped notification %s: %s", handle, str2hex(data)) + class Connection(object): @abstractmethod @@ -70,17 +87,17 @@ class BLEConnection(Connection): Main transport class, uses real Bluetooth LE connection. Loops with timeout of 1 seconds to find device named "Lego MOVE Hub" - :type requester: Requester + :type _requester: Requester """ def __init__(self): super(BLEConnection, self).__init__() - self.requester = None + self._requester = None def connect(self, bt_iface_name='hci0'): service = DiscoveryService(bt_iface_name) - while not self.requester: + while not self._requester: log.info("Discovering devices using %s...", bt_iface_name) devices = service.discover(1) log.debug("Devices: %s", devices) @@ -88,25 +105,22 @@ class BLEConnection(Connection): for address, name in devices.items(): if name == LEGO_MOVE_HUB: logging.info("Found %s at %s", name, address) - self._get_requester(address, bt_iface_name) + self._requester = Requester(address, True, bt_iface_name) break return self - def _get_requester(self, address, bt_iface_name): - self.requester = Requester(address, True, bt_iface_name) - def set_notify_handler(self, handler): - if self.requester: + if self._requester: log.debug("Setting notification handler: %s", handler) - self.requester.notification_sink = handler + self._requester.notification_sink = handler else: raise RuntimeError("No requester available") def read(self, handle): # FIXME: repeating reads hangs it... log.debug("Reading from: %s", handle) - data = self.requester.read_by_handle(handle) + data = self._requester.read_by_handle(handle) log.debug("Result: %s", data) if isinstance(data, list): data = data[0] @@ -114,7 +128,7 @@ class BLEConnection(Connection): def write(self, handle, data): log.debug("Writing to %s: %s", handle, str2hex(data)) - return self.requester.write_by_handle(handle, data) + return self._requester.write_by_handle(handle, data) class DebugServer(object): @@ -141,7 +155,7 @@ class DebugServer(object): conn, addr = self.sock.accept() if not self._running: raise KeyboardInterrupt("Shutdown") - self.ble.requester.notification_sink = lambda x, y: self._notify(conn, x, y) + self.ble._requester.notification_sink = lambda x, y: self._notify(conn, x, y) try: self._handle_conn(conn) except KeyboardInterrupt: @@ -149,14 +163,14 @@ class DebugServer(object): except BaseException: log.error("Problem handling incoming connection: %s", traceback.format_exc()) finally: - self.ble.requester.notification_sink = self._notify_dummy + self.ble._requester.notification_sink = self._notify_dummy conn.close() def __del__(self): self.sock.close() def _notify_dummy(self, handle, data): - log.debug("Notification from handle %s: %s", handle, unhexlify(data)) + log.debug("Notification from handle %s: %s", handle, hexlify(data.strip())) self._check_shutdown(data) def _notify(self, conn, handle, data): @@ -228,6 +242,7 @@ class DebugServerConnection(Connection): self.incoming = [] self.reader = Thread(target=self._recv) + self.reader.setName("Debug connection reader") self.reader.setDaemon(True) self.reader.start() @@ -265,7 +280,7 @@ class DebugServerConnection(Connection): data = self.sock.recv(1024) log.debug("Recv from debug server: %s", data.strip()) if not data: - break + raise KeyboardInterrupt("Server has closed connection") self.buf += data diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py index 3029362..a682975 100644 --- a/pylgbst/peripherals.py +++ b/pylgbst/peripherals.py @@ -126,7 +126,6 @@ class EncodedMotor(Peripheral): command += self.TRAILER - self._working = -1 self._write_to_hub(MSG_SET_PORT_VAL, command) def timed(self, seconds, speed_primary=1, speed_secondary=None, async=False): @@ -141,6 +140,7 @@ class EncodedMotor(Peripheral): raise ValueError("Too large value for seconds: %s", seconds) command += pack('