58 lines
2.6 KiB
Python
58 lines
2.6 KiB
Python
"""Query FFG identity/angles only. No force commands, ROS publication or CAN access."""
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
import json
|
||
import numpy as np
|
||
import serial
|
||
|
||
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'src/linkerhand_retarget/linkerhand_retarget'))
|
||
from linkerhand.linkerforce import FrameParser, FrameHandler
|
||
from linkerhand.constants import HandType
|
||
|
||
handler = FrameHandler(HandType.left)
|
||
parser = FrameParser()
|
||
samples = []
|
||
identity = None
|
||
print('准备开始：请戴好手套，接下来按提示缓慢张手、握拳，不要强掰关节。', file=sys.stderr, flush=True)
|
||
for remaining in (3, 2, 1):
|
||
print(f'倒计时 {remaining}', file=sys.stderr, flush=True)
|
||
time.sleep(1)
|
||
with serial.Serial('/dev/ttyUSB0', 2000000, timeout=0.1, exclusive=True) as port:
|
||
port.write(handler.pack_version_query())
|
||
start = time.monotonic()
|
||
end = start + 20
|
||
next_query = 0
|
||
next_identity_query = start + 1
|
||
last_phase = -1
|
||
while time.monotonic() < end:
|
||
if identity is None and time.monotonic() >= next_identity_query:
|
||
port.write(handler.pack_version_query())
|
||
next_identity_query = time.monotonic() + 1
|
||
phase = min(int((time.monotonic() - start) // 5), 3)
|
||
if phase != last_phase:
|
||
gesture = ('张开手并保持', '缓慢握拳并保持', '再次张开手并保持', '再次缓慢握拳并保持')[phase]
|
||
print(f'{gesture}（5秒）；已收到 {len(samples)} 帧', file=sys.stderr, flush=True)
|
||
last_phase = phase
|
||
if time.monotonic() >= next_query:
|
||
port.write(handler.pack_position_query())
|
||
next_query = time.monotonic() + 0.1
|
||
for byte in port.read(port.in_waiting or 1):
|
||
if parser.process_byte(byte):
|
||
result = handler.handle_frame(parser.frame_buf)
|
||
parser.reset()
|
||
if result and 'version' in result:
|
||
identity = result
|
||
if result and 'poslist' in result:
|
||
samples.append(result['poslist'])
|
||
report = {'identity': identity, 'frames': len(samples), 'baudrate': 2000000,
|
||
'writes': ['version query 0x01', 'position query 0x03']}
|
||
if samples:
|
||
a = np.asarray(samples)
|
||
report.update(dimension=int(a.shape[1]), finite=bool(np.isfinite(a).all()),
|
||
last_degrees=np.rad2deg(a[-1]).round(2).tolist(),
|
||
range_degrees=np.rad2deg(np.ptp(a, axis=0)).round(2).tolist())
|
||
print(json.dumps(report, ensure_ascii=False, indent=2))
|
||
if not identity or identity.get('raw_handtype') != 'Left' or not samples:
|
||
sys.exit(1)
|