102 lines
3.6 KiB
Python
102 lines
3.6 KiB
Python
"""
|
|
controller.py — G1 wireless-remote DDS reader for Saqr.
|
|
|
|
Trimmed copy of the helper used by Project/Manual Photographer. The G1's
|
|
built-in remote publishes its button state on the `rt/lowstate` DDS topic;
|
|
LowStateHub subscribes to that topic and exposes the parsed button bits and
|
|
a couple of combo helpers for the bridge to poll.
|
|
|
|
Buttons follow the same byte layout as Manual Photographer:
|
|
data1 bits: R1, L1, Start, Select, R2, L2, F1, F3
|
|
data2 bits: A, B, X, Y, Up, Right, Down, Left
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import struct
|
|
import time
|
|
from pathlib import Path
|
|
|
|
from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
|
|
|
|
|
|
def auto_pick_iface() -> str:
|
|
"""Best-effort guess of the DDS network interface."""
|
|
try:
|
|
net_dir = Path("/sys/class/net")
|
|
if not net_dir.exists():
|
|
return "lo"
|
|
names = [p.name for p in net_dir.iterdir() if p.is_dir()]
|
|
for pref in ("en", "eth", "wl"):
|
|
for n in names:
|
|
if n != "lo" and n.startswith(pref):
|
|
return n
|
|
for n in names:
|
|
if n != "lo":
|
|
return n
|
|
return "lo"
|
|
except Exception:
|
|
return "lo"
|
|
|
|
|
|
class UnitreeRemote:
|
|
def __init__(self):
|
|
self.Lx = 0; self.Rx = 0; self.Ry = 0; self.Ly = 0
|
|
self.L1 = 0; self.L2 = 0; self.R1 = 0; self.R2 = 0
|
|
self.A = 0; self.B = 0; self.X = 0; self.Y = 0
|
|
self.Up = 0; self.Down = 0; self.Left = 0; self.Right = 0
|
|
self.Select = 0; self.F1 = 0; self.F3 = 0; self.Start = 0
|
|
|
|
def _parse_buttons(self, data1: int, data2: int) -> None:
|
|
self.R1 = (data1 >> 0) & 1; self.L1 = (data1 >> 1) & 1
|
|
self.Start = (data1 >> 2) & 1; self.Select = (data1 >> 3) & 1
|
|
self.R2 = (data1 >> 4) & 1; self.L2 = (data1 >> 5) & 1
|
|
self.F1 = (data1 >> 6) & 1; self.F3 = (data1 >> 7) & 1
|
|
self.A = (data2 >> 0) & 1; self.B = (data2 >> 1) & 1
|
|
self.X = (data2 >> 2) & 1; self.Y = (data2 >> 3) & 1
|
|
self.Up = (data2 >> 4) & 1; self.Right = (data2 >> 5) & 1
|
|
self.Down = (data2 >> 6) & 1; self.Left = (data2 >> 7) & 1
|
|
|
|
def _parse_axes(self, data: bytes) -> None:
|
|
offsets = [4, 8, 12, 20] # Lx, Rx, Ry, Ly
|
|
self.Lx, self.Rx, self.Ry, self.Ly = [
|
|
struct.unpack('<f', data[o:o + 4])[0] for o in offsets
|
|
]
|
|
|
|
def parse(self, remote_data: bytes) -> None:
|
|
self._parse_axes(remote_data)
|
|
self._parse_buttons(remote_data[2], remote_data[3])
|
|
|
|
def state(self) -> dict:
|
|
return self.__dict__.copy()
|
|
|
|
|
|
class LowStateHub:
|
|
"""DDS subscriber callback target. Stores the latest parsed remote state."""
|
|
|
|
def __init__(self, watchdog_timeout: float = 0.25):
|
|
self.low_state: LowState_ | None = None
|
|
self.first_state = False
|
|
self.last_state_time = 0.0
|
|
self.watchdog_timeout = float(watchdog_timeout)
|
|
self.remote = UnitreeRemote()
|
|
|
|
def handler(self, msg: LowState_) -> None:
|
|
self.low_state = msg
|
|
self.first_state = True
|
|
self.last_state_time = time.time()
|
|
try:
|
|
self.remote.parse(msg.wireless_remote)
|
|
except Exception:
|
|
pass
|
|
|
|
def fresh(self) -> bool:
|
|
return (time.time() - self.last_state_time) < self.watchdog_timeout
|
|
|
|
# ── Combo helpers ───────────────────────────────────────────────────────
|
|
def combo_r2x(self) -> bool:
|
|
return bool(self.remote.R2 and self.remote.X)
|
|
|
|
def combo_r2y(self) -> bool:
|
|
return bool(self.remote.R2 and self.remote.Y)
|