141 lines
3.9 KiB
Python
141 lines
3.9 KiB
Python
"""Contract tests for the official ``robot_interface`` compatibility API."""
|
|
|
|
from copy import deepcopy
|
|
|
|
import pytest
|
|
|
|
import robot_interface as sdk
|
|
from go1_pro_sdk import build_low_cmd_plain
|
|
|
|
|
|
class _FakeMCUClient:
|
|
instances = []
|
|
|
|
def __init__(self, mcu_ip, mcu_port, local_port):
|
|
self.mcu_ip = mcu_ip
|
|
self.mcu_port = mcu_port
|
|
self.local_port = local_port or 49152
|
|
self.next_state = sdk.LowState()
|
|
self.next_state.motorState[sdk.FR_1].q = 1.23
|
|
self.sent = []
|
|
self.closed = False
|
|
self.__class__.instances.append(self)
|
|
|
|
def send(self, cmd):
|
|
self.sent.append(deepcopy(cmd))
|
|
return 616
|
|
|
|
def recv_latest(self):
|
|
return deepcopy(self.next_state)
|
|
|
|
def recv_state(self, timeout):
|
|
return deepcopy(self.next_state)
|
|
|
|
def close(self):
|
|
self.closed = True
|
|
|
|
|
|
@pytest.fixture
|
|
def fake_client(monkeypatch):
|
|
_FakeMCUClient.instances.clear()
|
|
monkeypatch.setattr(
|
|
"go1_pro_sdk.compat.robot_interface.MCUClient",
|
|
_FakeMCUClient,
|
|
)
|
|
return _FakeMCUClient
|
|
|
|
|
|
def test_official_names_and_data_types_are_available():
|
|
assert sdk.LOWLEVEL == 0xff
|
|
assert sdk.LeggedType.Go1 == 2
|
|
assert sdk.Go1 is sdk.LeggedType.Go1
|
|
assert sdk.FR_0 == 0
|
|
assert sdk.RL_2 == 11
|
|
assert sdk.BmsState().SOC == 0
|
|
assert sdk.HighCmd().velocity == [0.0, 0.0]
|
|
|
|
|
|
def test_init_cmd_data_uses_official_stop_sentinels():
|
|
udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007)
|
|
cmd = sdk.LowCmd()
|
|
|
|
assert udp.InitCmdData(cmd) is None
|
|
assert cmd.levelFlag == sdk.LOWLEVEL
|
|
assert len(cmd.motorCmd) == 20
|
|
assert all(m.mode == 0x0A for m in cmd.motorCmd)
|
|
assert all(m.q == sdk.PosStopF for m in cmd.motorCmd)
|
|
assert all(m.dq == sdk.VelStopF for m in cmd.motorCmd)
|
|
|
|
|
|
def test_official_lowlevel_send_receive_sequence(fake_client):
|
|
udp = sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007)
|
|
cmd = sdk.LowCmd()
|
|
state = sdk.LowState()
|
|
udp.InitCmdData(cmd)
|
|
|
|
assert udp.Recv() == 858
|
|
assert udp.GetRecv(state) is None
|
|
assert state.motorState[sdk.FR_1].q == pytest.approx(1.23)
|
|
|
|
cmd.motorCmd[sdk.FR_1].q = 1.2
|
|
cmd.motorCmd[sdk.FR_1].dq = 0.0
|
|
cmd.motorCmd[sdk.FR_1].Kp = 5.0
|
|
cmd.motorCmd[sdk.FR_1].Kd = 1.0
|
|
assert udp.SetSend(cmd) == 0
|
|
assert udp.Send() == 616
|
|
|
|
client = fake_client.instances[0]
|
|
assert client.sent[0].motorCmd[sdk.FR_1].q == pytest.approx(1.2)
|
|
assert udp.udpState.SendCount == 1
|
|
assert udp.udpState.RecvCount == 1
|
|
|
|
|
|
def test_position_limit_skips_stop_sentinel():
|
|
cmd = sdk.LowCmd()
|
|
sdk.UDP(sdk.LOWLEVEL, 0, "192.168.123.10", 8007).InitCmdData(cmd)
|
|
cmd.motorCmd[sdk.FR_1].q = 99.0
|
|
|
|
safe = sdk.Safety(sdk.LeggedType.Go1)
|
|
assert safe.PositionLimit(cmd) is None
|
|
|
|
assert cmd.motorCmd[sdk.FR_0].q == sdk.PosStopF
|
|
assert cmd.motorCmd[sdk.FR_1].q == pytest.approx(3.5)
|
|
|
|
|
|
def test_bms_command_is_serialized():
|
|
cmd = sdk.LowCmd()
|
|
cmd.bms.off = 0xA5
|
|
cmd.bms.reserve = [1, 2, 3]
|
|
|
|
plain = build_low_cmd_plain(cmd)
|
|
assert plain[562:566] == b"\xa5\x01\x02\x03"
|
|
|
|
|
|
def test_official_reserved_field_shapes_are_serialized():
|
|
cmd = sdk.LowCmd()
|
|
cmd.SN = [0x04030201, 0x08070605]
|
|
cmd.version = [0x01020304, 0x05060708]
|
|
cmd.reserve = 0x44332211
|
|
|
|
plain = build_low_cmd_plain(cmd)
|
|
assert plain[4:12] == bytes.fromhex("0102030405060708")
|
|
assert plain[12:20] == bytes.fromhex("0403020108070605")
|
|
assert plain[606:610] == bytes.fromhex("11223344")
|
|
|
|
|
|
def test_fixed_size_official_arrays_are_validated():
|
|
cmd = sdk.LowCmd()
|
|
cmd.wirelessRemote = [0] * 39
|
|
with pytest.raises(ValueError, match="wirelessRemote"):
|
|
build_low_cmd_plain(cmd)
|
|
|
|
|
|
def test_highlevel_udp_reports_the_unsupported_boundary():
|
|
udp = sdk.UDP(sdk.HIGHLEVEL, 8080, "192.168.123.161", 8082)
|
|
cmd = sdk.HighCmd()
|
|
udp.InitCmdData(cmd)
|
|
udp.SetSend(cmd)
|
|
|
|
with pytest.raises(NotImplementedError, match="HighCmd/HighState UDP"):
|
|
udp.Send()
|