Initial commit: S10 remote control module, demos, and docs
- s10_robot.py: protocol framing, mTLS client, heartbeat, status and control APIs - demo_stand_rotate_lie.py: stand, rotate in place, lie down with safe recovery - status_probe.py: read-only status subscription - tests: protocol unit tests and local fake robot - docs: remote control command reference
This commit is contained in:
@@ -0,0 +1,228 @@
|
||||
#!/usr/bin/env python3
|
||||
"""s10_robot 协议层单元测试（不连机器人）。
|
||||
|
||||
运行::
|
||||
|
||||
uv run python -m unittest discover -s tests -v
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
import struct
|
||||
import sys
|
||||
import threading
|
||||
import unittest
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parent.parent))
|
||||
|
||||
import s10_robot as s10
|
||||
from s10_robot import (
|
||||
C_AXIS,
|
||||
C_HEARTBEAT,
|
||||
C_STATUS,
|
||||
FMT_JSON,
|
||||
HEADER_LEN,
|
||||
SYNC,
|
||||
T_HEARTBEAT,
|
||||
T_FAULT,
|
||||
T_MOTION,
|
||||
Frame,
|
||||
S10Client,
|
||||
S10CommandError,
|
||||
build_asdu,
|
||||
build_frame,
|
||||
parse_frames,
|
||||
)
|
||||
|
||||
|
||||
def make_frame(type_: int, command: int, items: dict, report_id: int = 0) -> bytes:
|
||||
return build_frame(build_asdu(type_, command, items), report_id)
|
||||
|
||||
|
||||
class FrameTest(unittest.TestCase):
|
||||
def test_header_layout(self):
|
||||
asdu = build_asdu(0x00100064, 5, {})
|
||||
packet = build_frame(asdu, report_id=0x1234)
|
||||
self.assertEqual(packet[:4], SYNC)
|
||||
self.assertEqual(len(packet), HEADER_LEN + len(asdu))
|
||||
length, rid = struct.unpack_from("<HH", packet, 4)
|
||||
self.assertEqual(length, len(asdu))
|
||||
self.assertEqual(rid, 0x1234)
|
||||
self.assertEqual(packet[8], FMT_JSON)
|
||||
self.assertEqual(packet[9], 0)
|
||||
self.assertEqual(packet[10], 0x01)
|
||||
self.assertEqual(packet[11:16], b"\x00" * 5)
|
||||
|
||||
def test_asdu_is_valid_json_with_numeric_ids(self):
|
||||
asdu = build_asdu(0x00100001, 0x00100002, {"X": 0.5})
|
||||
doc = json.loads(asdu)
|
||||
self.assertEqual(doc["PatrolDevice"]["Type"], 0x00100001)
|
||||
self.assertEqual(doc["PatrolDevice"]["Command"], 0x00100002)
|
||||
self.assertEqual(doc["PatrolDevice"]["Items"], {"X": 0.5})
|
||||
self.assertRegex(doc["PatrolDevice"]["Time"], r"^\d{4}-\d{2}-\d{2} \d{2}:\d{2}:\d{2}$")
|
||||
|
||||
def test_parse_single(self):
|
||||
packet = make_frame(T_MOTION, C_AXIS, {"X": 0.1}, report_id=7)
|
||||
buf = bytearray(packet)
|
||||
frames = parse_frames(buf)
|
||||
self.assertEqual(len(frames), 1)
|
||||
frame = frames[0]
|
||||
self.assertEqual((frame.type, frame.command, frame.report_id), (T_MOTION, C_AXIS, 7))
|
||||
self.assertEqual(frame.items, {"X": 0.1})
|
||||
self.assertEqual(buf, bytearray())
|
||||
|
||||
def test_parse_split_across_reads(self):
|
||||
packet = make_frame(T_HEARTBEAT, C_STATUS, {"BasicStatus": {"MotionState": 1}})
|
||||
buf = bytearray()
|
||||
frames = []
|
||||
for i in range(0, len(packet), 5):
|
||||
buf.extend(packet[i:i + 5])
|
||||
frames.extend(parse_frames(buf))
|
||||
self.assertEqual(len(frames), 1)
|
||||
self.assertEqual(frames[0].items["BasicStatus"]["MotionState"], 1)
|
||||
|
||||
def test_parse_multiple_in_one_read(self):
|
||||
data = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1}) + make_frame(
|
||||
T_MOTION, C_STATUS, {"i": 2}
|
||||
)
|
||||
frames = parse_frames(bytearray(data))
|
||||
self.assertEqual([f.items["i"] for f in frames], [1, 2])
|
||||
|
||||
def test_resync_after_garbage(self):
|
||||
packet = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1})
|
||||
frames = parse_frames(bytearray(b"\x00\xff\xeb\x91" + packet))
|
||||
self.assertEqual(len(frames), 1)
|
||||
self.assertEqual(frames[0].items, {"i": 1})
|
||||
|
||||
def test_bad_json_is_skipped(self):
|
||||
asdu = b'{"PatrolDevice": not-json'
|
||||
bad = build_frame(asdu)
|
||||
good = make_frame(T_HEARTBEAT, C_STATUS, {"i": 1})
|
||||
frames = parse_frames(bytearray(bad + good))
|
||||
self.assertEqual(len(frames), 1)
|
||||
self.assertEqual(frames[0].items, {"i": 1})
|
||||
|
||||
def test_real_device_response(self):
|
||||
# README 中实测返回的原始报文（无 ErrorMessage）
|
||||
payload = (
|
||||
'{"PatrolDevice":{"Command":5,"Items":{"ErrorCode":0},'
|
||||
'"Time":"2026-10-29 19:50:18.403","Type":1048676}}'
|
||||
).encode()
|
||||
buf = bytearray(build_frame(payload, report_id=3))
|
||||
frames = parse_frames(buf)
|
||||
self.assertEqual(len(frames), 1)
|
||||
self.assertEqual(frames[0].type, 0x00100064)
|
||||
self.assertEqual(frames[0].command, 5)
|
||||
self.assertEqual(frames[0].items, {"ErrorCode": 0})
|
||||
self.assertEqual(frames[0].report_id, 3)
|
||||
|
||||
def test_axis_asdu_too_long_raises(self):
|
||||
with self.assertRaises(ValueError):
|
||||
build_frame(b"x" * (s10.MAX_ASDU + 1))
|
||||
|
||||
|
||||
class ClientTest(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.client = S10Client()
|
||||
|
||||
def tearDown(self):
|
||||
self.client.close()
|
||||
|
||||
def test_command_error(self):
|
||||
err = S10CommandError(0xE006, "Mismatched request client")
|
||||
self.assertIn("0xE006", str(err))
|
||||
self.assertEqual(err.code, 0xE006)
|
||||
|
||||
def test_move_range_validation(self):
|
||||
class FakeSock:
|
||||
def sendall(self, _packet):
|
||||
pass
|
||||
|
||||
self.client._sock = FakeSock() # type: ignore[assignment]
|
||||
with self.assertRaises(ValueError):
|
||||
self.client.move(x=1.5)
|
||||
with self.assertRaises(ValueError):
|
||||
self.client.drive_velocity(x=2.0)
|
||||
self.client.move(x=0.5, yaw=-0.5) # 合法，不应抛异常
|
||||
|
||||
def test_successful_request_resolves_pending(self):
|
||||
client = self.client
|
||||
sent = []
|
||||
|
||||
class FakeSock:
|
||||
def sendall(self, packet):
|
||||
sent.append(packet)
|
||||
rid = struct.unpack_from("<H", packet, 6)[0]
|
||||
# 模拟机器人在同一连接上异步回包
|
||||
threading.Thread(
|
||||
target=lambda: client._dispatch(
|
||||
Frame(T_MOTION, C_AXIS, "", {"ErrorCode": 0}, rid, FMT_JSON, 0, 1)
|
||||
),
|
||||
daemon=True,
|
||||
).start()
|
||||
|
||||
client._sock = FakeSock() # type: ignore[assignment]
|
||||
result = client._request(T_MOTION, C_AXIS, {"X": 0.0}, timeout=2)
|
||||
self.assertEqual(result, {"ErrorCode": 0})
|
||||
self.assertEqual(len(sent), 1)
|
||||
|
||||
def test_error_response_raises(self):
|
||||
client = self.client
|
||||
|
||||
class FakeSock:
|
||||
def sendall(self, packet):
|
||||
rid = struct.unpack_from("<H", packet, 6)[0]
|
||||
client._dispatch(
|
||||
Frame(T_MOTION, C_AXIS, "",
|
||||
{"ErrorCode": 0xE006, "ErrorMessage": "Mismatched request client"},
|
||||
rid, FMT_JSON, 0, 1)
|
||||
)
|
||||
|
||||
client._sock = FakeSock() # type: ignore[assignment]
|
||||
with self.assertRaises(S10CommandError) as ctx:
|
||||
client._request(T_MOTION, C_AXIS, {}, timeout=2)
|
||||
self.assertEqual(ctx.exception.code, 0xE006)
|
||||
|
||||
def test_status_frames_update_snapshot(self):
|
||||
client = self.client
|
||||
client._dispatch(
|
||||
Frame(T_HEARTBEAT, C_STATUS, "",
|
||||
{"BasicStatus": {"MotionState": 17, "Gait": 0x1001, "HES": 0}},
|
||||
0, FMT_JSON, 0, 1)
|
||||
)
|
||||
self.assertEqual(client.basic_status["MotionState"], 17)
|
||||
self.assertEqual(client.wait_motion_state({17}, timeout=0.5), 17)
|
||||
|
||||
def test_fault_start_and_stop(self):
|
||||
client = self.client
|
||||
client._dispatch(
|
||||
Frame(T_FAULT, C_STATUS, "",
|
||||
{"ErrorList": [{"Code": 0x8014, "Type": 1, "Name": "joint_position_over_limit",
|
||||
"Severities": [4]}]},
|
||||
0, FMT_JSON, 0, 1)
|
||||
)
|
||||
self.assertEqual(len(client.active_errors), 1)
|
||||
client._dispatch(
|
||||
Frame(T_FAULT, C_STATUS, "",
|
||||
{"ErrorList": [{"Code": 0x8014, "Type": 2}]}, 0, FMT_JSON, 0, 1)
|
||||
)
|
||||
self.assertEqual(client.active_errors, [])
|
||||
|
||||
def test_estop_aborts_wait(self):
|
||||
client = self.client
|
||||
client._dispatch(
|
||||
Frame(T_HEARTBEAT, C_STATUS, "",
|
||||
{"BasicStatus": {"MotionState": 1, "HES": 1}}, 0, FMT_JSON, 0, 1)
|
||||
)
|
||||
with self.assertRaises(s10.S10Error):
|
||||
client.wait_motion_state({17}, timeout=0.5)
|
||||
|
||||
def test_wait_timeout(self):
|
||||
with self.assertRaises(s10.S10Timeout):
|
||||
self.client.wait_motion_state({17}, timeout=0.2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main(verbosity=2)
|
||||
Reference in New Issue
Block a user