Merge branch 'master' of github.com:undera/pylgbst

pylgbst/__init__.py

@@ -108,6 +108,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)
 

pylgbst/comms.py

@@ -1,13 +1,16 @@
 """
 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
 
@@ -15,29 +18,15 @@ from pylgbst.constants import LEGO_MOVE_HUB, MSG_DEVICE_SHUTDOWN
 
 log = logging.getLogger('transport')
 
-if sys.version_info[0] == 2:
-    def str2hex(data):
-        return data.encode("hex")
 
-
+def str2hex(data):  # TODO: eliminate it
-    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):
+if sys.version_info[0] == 2:
-        return binascii.unhexlify(data)
+    def get_byte(seq, index):
-
+        return ord(seq[index])
-
+else:
     def get_byte(seq, index):
         return seq[index]
 
@@ -53,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._notify_queue.put((handle, data))
-            self.notification_sink(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
@@ -81,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)
@@ -99,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]
@@ -125,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):
@@ -152,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:
@@ -160,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, hex2str(data))
+        log.debug("Notification from handle %s: %s", handle, hexlify(data.strip()))
         self._check_shutdown(data)
 
     def _notify(self, conn, handle, data):
@@ -215,7 +218,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)}
@@ -239,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()
 
@@ -264,7 +268,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):
@@ -276,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
 
@@ -286,7 +290,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:
@@ -294,3 +298,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)

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('<H', msec)
 
+        self._working = -1
         self._wrap_and_write(command, speed_primary, speed_secondary)
         self.__wait_sync(async)
 
@@ -158,6 +158,7 @@ class EncodedMotor(Peripheral):
         # angle
         command += pack('<I', angle)
 
+        self._working = -1
         self._wrap_and_write(command, speed_primary, speed_secondary)
         self.__wait_sync(async)
 
@@ -165,7 +166,9 @@ class EncodedMotor(Peripheral):
         if not async:
             log.debug("Waiting for sync command work to finish...")
             while self.is_working():
-                time.sleep(0.05)
+                log.debug("Waiting")
+                time.sleep(0.1)
+            log.debug("Command has finished.")
 
     # TODO: how to tell when motor has stopped?
 

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
@@ -69,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()
@@ -103,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)
+        time.sleep(0.1)
-        motor.timed(1.5)
+
+        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):
@@ -133,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)
 

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)
@@ -43,6 +44,10 @@ class Vernie(MoveHub):
 
     def program(self):
         time.sleep(1)
+        self.head_to(LEFT, angle=90)
+        self.head_to(RIGHT, angle=50)
+        time.sleep(1)
+
         while True:
             self.head_to(LEFT)
             time.sleep(1)
@@ -59,6 +64,7 @@ class Vernie(MoveHub):
 
 if __name__ == '__main__':
     logging.basicConfig(level=logging.DEBUG)
+    comms.log.setLevel(logging.INFO)
 
     try:
         connection = DebugServerConnection()