80 lines
2.0 KiB
Python
80 lines
2.0 KiB
Python
import sched
|
|
import time
|
|
from threading import Thread
|
|
import json
|
|
|
|
from redis import Redis
|
|
|
|
from control.walle import WallE
|
|
from control.camera import Camera
|
|
|
|
INTERVAL_TICK = 0.0125 # 20/s
|
|
INTERVAL_IMAGE = 0.05 # 20/s
|
|
|
|
|
|
class EventListener(Thread):
|
|
def __init__(self):
|
|
super(EventListener, self).__init__()
|
|
self.keep_running = True
|
|
self.dbcon = Redis()
|
|
self.pubsub = self.dbcon.pubsub()
|
|
self.pubsub.subscribe(['move', 'look'])
|
|
|
|
def run(self) -> None:
|
|
while self.keep_running:
|
|
item = self.pubsub.get_message()
|
|
if not item:
|
|
time.sleep(0.01)
|
|
channel = item['channel'].decode()
|
|
data = item['data']
|
|
if not isinstance(data, bytes):
|
|
print(f"Channel {channel} got message that is not bytes {data}")
|
|
continue
|
|
try:
|
|
self.handle_message(channel, data)
|
|
except ValueError:
|
|
pass # todo
|
|
self.pubsub.close()
|
|
|
|
def stop(self):
|
|
self.keep_running = False
|
|
|
|
def handle_message(self, channel: str, data: bytes):
|
|
data_dict = json.loads(data.decode())
|
|
if channel == 'move':
|
|
walle.set_movement(data_dict['angle'], data_dict['force'])
|
|
elif channel == 'look':
|
|
walle.set_eye_velocity(data_dict['angle'], data_dict['force'])
|
|
else:
|
|
print(f'Unknown channel {channel}')
|
|
|
|
|
|
scheduler = sched.scheduler()
|
|
camera = Camera()
|
|
walle = WallE()
|
|
walle.setup()
|
|
|
|
def walle_tick():
|
|
scheduler.enter(INTERVAL_TICK, 1, walle_tick)
|
|
walle.tick()
|
|
|
|
|
|
def camera_image():
|
|
scheduler.enter(INTERVAL_IMAGE, 1, camera_image)
|
|
image = camera.get_image()
|
|
redisdb.publish('image', image)
|
|
|
|
|
|
scheduler.enter(INTERVAL_TICK, 1, walle_tick)
|
|
scheduler.enter(INTERVAL_IMAGE, 1, camera_image)
|
|
|
|
if __name__ == '__main__':
|
|
redisdb = Redis()
|
|
event_listener = EventListener()
|
|
event_listener.start()
|
|
try:
|
|
scheduler.run()
|
|
except KeyboardInterrupt:
|
|
event_listener.stop()
|
|
raise
|