caowangrenbo
5e2017de69
- 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
229 lines
7.9 KiB
Python
229 lines
7.9 KiB
Python
#!/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)
|