"""
Used to accumulate key presses and create movement commands from them.
"""
from queue import Queue
import logging
import time
from threading import Thread
from typing import NamedTuple, List, Tuple, Any
from datetime import datetime
MOVE_FREQUENCY = 10
[docs]
class MoveKey(NamedTuple):
# key to listen to
key: str
# number of the joint to be moved
joint_index: int
# whether moved forward
positive: bool
move_keys = [
MoveKey("ArrowLeft", 0, True),
MoveKey("ArrowRight", 0, False),
MoveKey("ArrowUp", 1, True),
MoveKey("ArrowDown", 1, False),
MoveKey('y', 2, True),
MoveKey('a', 2, False),
MoveKey('x', 3, True),
MoveKey('s', 3, False),
MoveKey('c', 4, True),
MoveKey('d', 4, False),
MoveKey('v', 5, True),
MoveKey('f', 5, False),
MoveKey('b', 6, True),
MoveKey('g', 6, False),
MoveKey('n', 7, True),
MoveKey('h', 7, False),
MoveKey('m', 8, True),
MoveKey('j', 8, False),
]
[docs]
class ManualMovementManager:
input_queue: Queue[str]
def __init__(self, robot: Any, joint_names: List[str]):
self.input_queue = Queue[str]()
self._stop = False
self.joint_names = joint_names
# start the listener
Thread(daemon=True, target=self.listen).start()
self.robot = robot
self.sending_stats = []
[docs]
def listen(self):
"""
will run forever and accumulate received key presses.
:return:
"""
while not self._stop:
# wait to match the update frequency
time.sleep(1/MOVE_FREQUENCY)
movements = self.empty_queue()
if any(movements):
try:
self.robot.move_relative(movements)
except Exception:
logging.exception(f"{datetime.now()}: Manual movement failed; ignoring and continuing")
# drop any queued key presses so a held-down key does not
# immediately re-trigger the same failing movement
self.empty_queue()
[docs]
def empty_queue(self) -> List[Tuple[str, int]]:
# collect all inputs
movements = [0 for move_key in move_keys if move_key.positive]
while not self.input_queue.empty():
key = self.input_queue.get()
# check whether the key has a meaning and add a movement to the respective joint
for move_key in move_keys:
# check whether there are enough joints in the current robot
if move_key.joint_index <= len(self.joint_names):
if key == move_key.key:
if move_key.positive:
movements[move_key.joint_index] += 1
else:
movements[move_key.joint_index] -= 1
to_move = []
for idx, diff in enumerate(movements):
# only add relevant key-presses
if diff and idx < len(self.joint_names):
to_move.append((self.joint_names[idx], diff))
return to_move
[docs]
def receive_key(self, key: str):
self.input_queue.put(key)
[docs]
def stop(self):
self._stop = True
