230 lines
7.6 KiB
Python
230 lines
7.6 KiB
Python
"""Keyboard sender that emits binary control packets over OmniSocket."""
|
|
|
|
from __future__ import annotations
|
|
|
|
import os
|
|
from pathlib import Path
|
|
import select
|
|
import signal
|
|
import sys
|
|
import termios
|
|
import threading
|
|
import tty
|
|
from typing import Dict, Optional
|
|
|
|
import yaml
|
|
|
|
try:
|
|
from .omnisocket_control import make_control_packet
|
|
except ImportError: # pragma: no cover - direct script execution fallback
|
|
from omnisocket_control import make_control_packet
|
|
|
|
|
|
def _load_omnisocket_api():
|
|
try:
|
|
from omnisocket import CONTROL_DEFAULTS, Session
|
|
except ImportError as exc: # pragma: no cover - environment dependent
|
|
raise RuntimeError(
|
|
"omnisocket is not installed. Install it before using "
|
|
"omnisocket_keyboard_sender.py."
|
|
) from exc
|
|
return CONTROL_DEFAULTS, Session
|
|
|
|
|
|
class OmniSocketKeyboardSender:
|
|
"""Standalone keyboard sender for OmniSocket control-plane testing."""
|
|
|
|
def __init__(self) -> None:
|
|
self.config: Dict[str, object] = {}
|
|
self.seq_id = 0
|
|
self.running = False
|
|
self.session = None
|
|
self.input_thread: Optional[threading.Thread] = None
|
|
self.original_terminal_settings = None
|
|
|
|
self._load_config()
|
|
self._init_session()
|
|
self._print_help()
|
|
|
|
def _load_config(self) -> None:
|
|
config_path = Path(__file__).resolve().parent / "config" / "omnisocket_demo.yaml"
|
|
if config_path.exists():
|
|
with config_path.open("r", encoding="utf-8") as file:
|
|
self.config = yaml.safe_load(file) or {}
|
|
else:
|
|
self.config = {}
|
|
|
|
transport_cfg = self.config.get("transport", {})
|
|
sender_cfg = self.config.get("control_sender", {})
|
|
|
|
self.server_addr = str(transport_cfg.get("server_addr", "127.0.0.1:10909"))
|
|
self.relay_via = str(transport_cfg.get("relay_via", ""))
|
|
self.bind_ip = str(transport_cfg.get("bind_ip", ""))
|
|
self.bind_device = str(transport_cfg.get("bind_device", ""))
|
|
self.peer_id = str(sender_cfg.get("peer_id", "peer-a-ctrl"))
|
|
self.target_peer = str(sender_cfg.get("target_peer", "peer-b-ctrl"))
|
|
|
|
def _init_session(self) -> None:
|
|
control_defaults, session_cls = _load_omnisocket_api()
|
|
self.session = session_cls()
|
|
self.session.connect(
|
|
server_addr=self.server_addr,
|
|
peer_id=self.peer_id,
|
|
relay_via=self.relay_via,
|
|
bind_ip=self.bind_ip,
|
|
bind_device=self.bind_device,
|
|
**control_defaults,
|
|
)
|
|
|
|
def _print_help(self) -> None:
|
|
print("OmniSocket keyboard sender ready")
|
|
print(f"Peer: {self.peer_id} -> {self.target_peer} via {self.server_addr}")
|
|
print("Keys:")
|
|
print(" z -> pose_home")
|
|
print(" c -> pose_hold")
|
|
print(" m -> mode_stride")
|
|
print(" w/s -> surge +/-")
|
|
print(" a/d -> sway +/-")
|
|
print(" q/e -> spin +/-")
|
|
print(" Left/Right -> lift +/-")
|
|
print(" Up/Down -> surge +/-")
|
|
print(" r -> trim_reset")
|
|
print(" 4 -> clear x speed")
|
|
print(" 5 -> clear y speed")
|
|
print(" 6 -> clear yaw speed")
|
|
print(" x -> session_quit")
|
|
|
|
def start(self) -> None:
|
|
self.running = True
|
|
self.input_thread = threading.Thread(target=self._input_loop, daemon=True)
|
|
self.input_thread.start()
|
|
print("OmniSocket keyboard sender thread started")
|
|
|
|
def stop(self) -> None:
|
|
self.running = False
|
|
if self.input_thread and self.input_thread.is_alive():
|
|
self.input_thread.join(timeout=1.0)
|
|
if self.original_terminal_settings is not None:
|
|
try:
|
|
termios.tcsetattr(
|
|
sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
|
|
)
|
|
except termios.error:
|
|
pass
|
|
self.original_terminal_settings = None
|
|
if self.session is not None:
|
|
self.session.close()
|
|
self.session = None
|
|
print("OmniSocket keyboard sender stopped")
|
|
|
|
def _input_loop(self) -> None:
|
|
self.original_terminal_settings = termios.tcgetattr(sys.stdin)
|
|
try:
|
|
tty.setraw(sys.stdin.fileno())
|
|
while self.running:
|
|
if select.select([sys.stdin], [], [], 0.1)[0]:
|
|
key = sys.stdin.read(1)
|
|
self._process_key(key)
|
|
except KeyboardInterrupt:
|
|
self._handle_ctrl_c()
|
|
finally:
|
|
if self.original_terminal_settings is not None:
|
|
termios.tcsetattr(
|
|
sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
|
|
)
|
|
self.original_terminal_settings = None
|
|
|
|
def _process_key(self, key: str) -> None:
|
|
event_map = {
|
|
"w": ("surge_up", "w", 1.0),
|
|
"s": ("surge_down", "s", 1.0),
|
|
"a": ("sway_left", "a", 1.0),
|
|
"d": ("sway_right", "d", 1.0),
|
|
"q": ("spin_left", "q", 1.0),
|
|
"e": ("spin_right", "e", 1.0),
|
|
"z": ("pose_home", "z", 1.0),
|
|
"c": ("pose_hold", "c", 1.0),
|
|
"m": ("mode_stride", "m", 1.0),
|
|
"r": ("trim_reset", "r", 1.0),
|
|
"4": ("set_surge", "4", 0.0),
|
|
"5": ("set_sway", "5", 0.0),
|
|
"6": ("set_spin", "6", 0.0),
|
|
"x": ("session_quit", "x", 1.0),
|
|
}
|
|
|
|
if key == "\x03":
|
|
self._handle_ctrl_c()
|
|
return
|
|
|
|
if key == "\x1b":
|
|
self._handle_arrow_key()
|
|
return
|
|
|
|
if key in event_map:
|
|
event_code, key_name, drive_value = event_map[key]
|
|
self._send_event(event_code, key_name, drive_value)
|
|
|
|
def _handle_arrow_key(self) -> None:
|
|
if not select.select([sys.stdin], [], [], 0.1)[0]:
|
|
return
|
|
key2 = sys.stdin.read(1)
|
|
if key2 != "[":
|
|
return
|
|
if not select.select([sys.stdin], [], [], 0.1)[0]:
|
|
return
|
|
key3 = sys.stdin.read(1)
|
|
arrow_map = {
|
|
"A": ("surge_up", "arrow_up"),
|
|
"B": ("surge_down", "arrow_down"),
|
|
"C": ("lift_down", "arrow_right"),
|
|
"D": ("lift_up", "arrow_left"),
|
|
}
|
|
if key3 in arrow_map:
|
|
event_code, key_name = arrow_map[key3]
|
|
self._send_event(event_code, key_name)
|
|
|
|
def _send_event(
|
|
self, event_code: str, key_name: str, drive_value: float = 1.0
|
|
) -> None:
|
|
if self.session is None:
|
|
return
|
|
packet = make_control_packet(self.seq_id, event_code, drive_value)
|
|
payload = packet.encode()
|
|
self.seq_id += 1
|
|
self.session.send(to=self.target_peer, data=payload)
|
|
print(
|
|
f"sent seq={packet.seq_id} event={event_code} key={key_name} "
|
|
f"bytes={len(payload)}"
|
|
)
|
|
if event_code == "session_quit":
|
|
self.running = False
|
|
|
|
def _handle_ctrl_c(self) -> None:
|
|
self.running = False
|
|
if self.original_terminal_settings is not None:
|
|
try:
|
|
termios.tcsetattr(
|
|
sys.stdin, termios.TCSADRAIN, self.original_terminal_settings
|
|
)
|
|
except termios.error:
|
|
pass
|
|
self.original_terminal_settings = None
|
|
os.kill(os.getpid(), signal.SIGINT)
|
|
|
|
|
|
def main() -> None:
|
|
sender = OmniSocketKeyboardSender()
|
|
sender.start()
|
|
try:
|
|
while sender.running:
|
|
if sender.input_thread is not None:
|
|
sender.input_thread.join(timeout=0.2)
|
|
except KeyboardInterrupt:
|
|
pass
|
|
finally:
|
|
sender.stop()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|