diff --git a/pylgbst/__init__.py b/pylgbst/__init__.py index 651b468..00e1898 100644 --- a/pylgbst/__init__.py +++ b/pylgbst/__init__.py @@ -102,7 +102,7 @@ class MoveHub(object): return device = self.devices[port] - device.handle_port_data(data) + device.queue_port_data(data) def _handle_port_status(self, data): port = get_byte(data, 3) diff --git a/pylgbst/comms.py b/pylgbst/comms.py index a2d1f40..cb7f36f 100644 --- a/pylgbst/comms.py +++ b/pylgbst/comms.py @@ -43,11 +43,11 @@ class Requester(GATTRequester): self.notification_sink = None # noinspection PyUnresolvedReferences - self._notify_queue = 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() + self._notify_queue = queue.Queue() # this queue is to minimize time spent in gattlib C code + thr = Thread(target=self._dispatch_notifications) + thr.setDaemon(True) + thr.setName("Notify queue dispatcher") + thr.start() def on_notification(self, handle, data): # log.debug("requester notified, sink: %s", self.notification_sink) @@ -63,7 +63,7 @@ class Requester(GATTRequester): try: self.notification_sink(handle, data) except BaseException: - log.warning("Failed to dispatch notification: %s", str2hex(data)) + log.warning("Data was: %s", str2hex(data)) log.warning("Failed to dispatch notification: %s", traceback.format_exc()) else: log.warning("Dropped notification %s: %s", handle, str2hex(data)) diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py index 1f21348..6233e74 100644 --- a/pylgbst/peripherals.py +++ b/pylgbst/peripherals.py @@ -1,6 +1,9 @@ import logging import time from struct import pack, unpack +from threading import Thread + +from six.moves import queue from pylgbst import get_byte, str2hex from pylgbst.constants import * @@ -11,6 +14,7 @@ log = logging.getLogger('peripherals') class Peripheral(object): """ :type parent: MoveHub + :type _incoming_port_data: queue.Queue """ def __init__(self, parent, port): @@ -24,6 +28,11 @@ class Peripheral(object): self._working = False self._subscribers = set() self._port_subscription_mode = None + self._incoming_port_data = queue.Queue() + thr = Thread(target=self._queue_reader) + thr.setDaemon(True) + thr.setName("Port data queue: %s" % self) + thr.start() def __repr__(self): return "%s on port %s" % (self.__class__.__name__, PORTS[self.port] if self.port in PORTS else self.port) @@ -71,10 +80,21 @@ class Peripheral(object): for subscriber in self._subscribers: subscriber(*args, **kwargs) + def queue_port_data(self, data): + self._incoming_port_data.put(data) + def handle_port_data(self, data): log.warning("Unhandled device notification for %s: %s", self, str2hex(data[4:])) self._notify_subscribers(data[4:]) + def _queue_reader(self): + while True: + data = self._incoming_port_data.get() + try: + self.handle_port_data(data) + except BaseException: + log.warning("Failed to handle port data by %s: %s", self, str2hex(data)) + class LED(Peripheral): SOMETHING = b'\x51\x00' diff --git a/vernie/__init__.py b/vernie/__init__.py index d4fa3aa..16d6642 100644 --- a/vernie/__init__.py +++ b/vernie/__init__.py @@ -74,8 +74,9 @@ class Vernie(MoveHub): self._head_position = 0 self.motor_external.subscribe(self._external_motor_data) - self._reset_head() - self.say("ready") + # self._reset_head() FIXME: restore it + # self.say("ready") + time.sleep(1) def say(self, phrase): if phrase in self.SPEECH_LANG_MAP[self.language]: diff --git a/vernie/run_away_game.py b/vernie/run_away_game.py index 1296b55..20c245b 100644 --- a/vernie/run_away_game.py +++ b/vernie/run_away_game.py @@ -1,6 +1,6 @@ from vernie import * -logging.basicConfig(level=logging.INFO) +logging.basicConfig(level=logging.DEBUG) robot = Vernie() running = True