From 96db3f40f4f787b59ccc6c18737085b8164b3887 Mon Sep 17 00:00:00 2001 From: cyy_mac Date: Sat, 20 Jun 2026 20:31:12 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0high=20lever=EF=BC=8C?= =?UTF-8?q?=E5=BE=85=E6=B5=8B=E8=AF=95=EF=BC=8C=E5=BE=85=E5=85=A8=E9=83=A8?= =?UTF-8?q?=E8=BD=AC=E6=8D=A2=E4=B8=BAC++=E7=89=88=E6=9C=AC=E5=8A=A0?= =?UTF-8?q?=E9=80=9F=EF=BC=8C=E5=BE=85=E6=8E=A5=E5=8F=A3=E4=B8=8E=E5=AE=98?= =?UTF-8?q?=E6=96=B9=E5=90=8C=E6=AD=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 18 ++++ examples/README.md | 30 ++++++ examples/example_highlevel_dance.py | 38 +++++++ examples/example_highlevel_pose.py | 46 +++++++++ examples/example_highlevel_walk.py | 47 +++++++++ go1_pro_sdk/__init__.py | 5 +- go1_pro_sdk/highlevel/__init__.py | 15 +++ go1_pro_sdk/highlevel/go1.py | 121 ++++++++++++++++++++++ go1_pro_sdk/highlevel/mqtt_client.py | 142 ++++++++++++++++++++++++++ go1_pro_sdk/highlevel/types.py | 68 +++++++++++++ pyproject.toml | 8 +- tests/test_highlevel.py | 144 +++++++++++++++++++++++++++ 12 files changed, 677 insertions(+), 5 deletions(-) create mode 100644 examples/example_highlevel_dance.py create mode 100644 examples/example_highlevel_pose.py create mode 100644 examples/example_highlevel_walk.py create mode 100644 go1_pro_sdk/highlevel/__init__.py create mode 100644 go1_pro_sdk/highlevel/go1.py create mode 100644 go1_pro_sdk/highlevel/mqtt_client.py create mode 100644 go1_pro_sdk/highlevel/types.py create mode 100644 tests/test_highlevel.py diff --git a/README.md b/README.md index fb5cb5f..edca214 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,24 @@ Unitree Go1 **PRO** 机型的低层电机控制 Python SDK。完整逆向 PRO ## 快速上手 +### 高层控制 (走/跳/姿态, 通过 sportMode 系统) + +```python +from go1_pro_sdk import Go1, Velocity, Pose, LED + +with Go1() as dog: + dog.stand_up() + dog.set_walk_mode() + dog.walk(Velocity(vx=0.3)) # 前进 + dog.dance_1() + dog.set_led(LED(0, 255, 0)) + dog.stand_down() +``` + +通过 MQTT 控制树莓派上的 Legged_sport. 跟低层互不冲突, 但只能二选一同时用. + +### 低层控制 (直接控 12 个电机) + ```python from go1_pro_sdk import MCUClient, LowCmd, MotorCmd, MotorMode diff --git a/examples/README.md b/examples/README.md index 58d91e2..3a699b0 100644 --- a/examples/README.md +++ b/examples/README.md @@ -1,5 +1,35 @@ # Examples +## 高层 (HighLevel) — 通过 sportMode + MQTT, 不需停 Pi 进程 + +适合: 走/跳/姿态/表演动作. 跟 lowlevel 控制方式互不兼容, 同时只用一种. + +### example_highlevel_walk.py — 走一圈 + +```bash +python examples/example_highlevel_walk.py +``` + +起立 → walk 模式 → 前进 2 秒 → 转向 2 秒 → 趴下. + +### example_highlevel_pose.py — 姿态变化 + +```bash +python examples/example_highlevel_pose.py +``` + +stand 模式下做各种姿态: 倾斜/扭转/抬头/低头/蹲下/撑直. + +### example_highlevel_dance.py — 跳舞 + 头灯 + +```bash +python examples/example_highlevel_dance.py +``` + +dance_1 + 头灯炫彩切换. + +## 低层 (LowLevel) — 直接控制 12 电机, 必须停 sportMode + 每个示例都假定: - 你已经从狗上提取了 `blowfish_state.bin` (或用包内置的) - 已经停掉 Pi 上的 keep_sport_alive + Legged_sport + appTransit diff --git a/examples/example_highlevel_dance.py b/examples/example_highlevel_dance.py new file mode 100644 index 0000000..4dc5c92 --- /dev/null +++ b/examples/example_highlevel_dance.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +"""高层表演: 跳舞 1 + 头灯炫彩.""" +import sys +import time + +from go1_pro_sdk import Go1, LED + + +def main(): + print('🐕 高层示例: 跳舞 + 头灯') + + with Go1() as dog: + # 头灯橙黄 + dog.set_led(LED(255, 128, 0)) + + print('stand_up...') + dog.stand_up() + time.sleep(3) + + print('dance_1!') + dog.dance_1() + + # 跳舞期间循环换色 + colors = [(255, 0, 0), (0, 255, 0), (0, 0, 255), (255, 255, 0), (255, 0, 255)] + for r, g, b in colors: + dog.set_led(LED(r, g, b)) + time.sleep(2) + + print('stand_down...') + dog.stand_down() + time.sleep(3) + dog.set_led(LED(0, 0, 0)) + + print('✅ 结束') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/examples/example_highlevel_pose.py b/examples/example_highlevel_pose.py new file mode 100644 index 0000000..75d1768 --- /dev/null +++ b/examples/example_highlevel_pose.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +"""高层姿态控制: 站立 + 各方向倾斜/扭转.""" +import sys +import time + +from go1_pro_sdk import Go1, Pose + + +def main(): + print('🐕 高层示例: 站立姿态变化') + + with Go1() as dog: + print('stand_up...') + dog.stand_up() + time.sleep(3) + + print('进入 stand 模式...') + dog.set_stand_mode() + time.sleep(1) + + poses = [ + ('左倾', Pose(lean_left_right=-0.5)), + ('右倾', Pose(lean_left_right=+0.5)), + ('扭头左', Pose(twist_left_right=-0.5)), + ('扭头右', Pose(twist_left_right=+0.5)), + ('低头', Pose(look_up_down=-0.5)), + ('抬头', Pose(look_up_down=+0.5)), + ('蹲下', Pose(extend_squat=-0.5)), + ('撑直', Pose(extend_squat=+0.5)), + ('归位', Pose()), + ] + for name, pose in poses: + print(f' → {name}') + for _ in range(15): + dog.pose(pose) + time.sleep(0.1) + + print('stand_down...') + dog.stand_down() + time.sleep(3) + + print('✅ 结束') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/examples/example_highlevel_walk.py b/examples/example_highlevel_walk.py new file mode 100644 index 0000000..a5b783c --- /dev/null +++ b/examples/example_highlevel_walk.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +"""高层走动示例 — 站起来 → 走一圈 → 趴下. + +跟 lowlevel 控制不同, 高层走的是 sportMode 系统, 不需要 stop_sportmode. +""" +import sys +import time + +from go1_pro_sdk import Go1, Velocity, Pose + + +def main(): + print('🐕 高层控制示例: stand_up → walk → 转向 → 趴下') + print(' 通过 MQTT 控制 Pi 上的 sportMode') + + with Go1() as dog: + print('\n[1/5] stand_up (起立)...') + dog.stand_up() + time.sleep(3) + + print('[2/5] 进入 walk 模式...') + dog.set_walk_mode() + time.sleep(1) + + print('[3/5] 前进 2 秒 (vx=0.2)...') + for _ in range(20): + dog.walk(Velocity(vx=0.2)) + time.sleep(0.1) + + print('[4/5] 原地转向 2 秒 (vz=0.3)...') + for _ in range(20): + dog.walk(Velocity(vz=0.3)) + time.sleep(0.1) + + print(' 停止 1 秒...') + dog.stop() + time.sleep(1) + + print('[5/5] stand_down (趴下)...') + dog.stand_down() + time.sleep(3) + + print('✅ 结束') + + +if __name__ == '__main__': + sys.exit(main() or 0) diff --git a/go1_pro_sdk/__init__.py b/go1_pro_sdk/__init__.py index e8ad83f..8ea9caa 100644 --- a/go1_pro_sdk/__init__.py +++ b/go1_pro_sdk/__init__.py @@ -24,6 +24,7 @@ client.safe_stop() # 退出前发 damping """ from .connection.mcu_client import MCUClient +from .highlevel import Go1, Go1MQTT, Velocity, Pose, LED, Mode from .codec.blowfish import Blowfish, verify_state from .codec.lowcmd_builder import build_low_cmd_plain, build_low_cmd_encrypted from .codec.lowstate_parser import parse_low_state @@ -47,7 +48,9 @@ from .utils import ( __version__ = '0.1.0' __all__ = [ - # 高层 API + # 高层 (MQTT, sportMode) + 'Go1', 'Go1MQTT', 'Velocity', 'Pose', 'LED', 'Mode', + # 低层 (UDP, MCU 直连) 'MCUClient', # 数据结构 'MotorCmd', 'MotorState', 'MotorMode', diff --git a/go1_pro_sdk/highlevel/__init__.py b/go1_pro_sdk/highlevel/__init__.py new file mode 100644 index 0000000..f242a28 --- /dev/null +++ b/go1_pro_sdk/highlevel/__init__.py @@ -0,0 +1,15 @@ +"""高层控制 (MQTT 模式切换 + 速度/姿态命令). + +跟 lowlevel (MCUClient / 12 关节) 是两条独立通道: + - lowlevel: 直接控电机, 必须停 sportMode, 接管完全控制 + - highlevel: 通过 sportMode/Legged_sport, 用 MQTT 切换 walk/stand/dance 等模式 + +正常使用应当只选一种, 不能同时使用 (会冲突). + +参考 https://github.com/Daniel89710/unitree-go1-py (MIT License) +""" +from .go1 import Go1 +from .mqtt_client import Go1MQTT +from .types import Velocity, Pose, LED, Mode + +__all__ = ['Go1', 'Go1MQTT', 'Velocity', 'Pose', 'LED', 'Mode'] diff --git a/go1_pro_sdk/highlevel/go1.py b/go1_pro_sdk/highlevel/go1.py new file mode 100644 index 0000000..95be50c --- /dev/null +++ b/go1_pro_sdk/highlevel/go1.py @@ -0,0 +1,121 @@ +"""Go1 高层控制 — 友好 API 封装. + +跟 unitree-go1-py 的 Go1 类 API 一致, 但去掉了相机/UDP highState 部分 +(我们 lowlevel 通过 MCUClient + Blowfish 拿 12 关节, 不需要 :8082 highState). + +用法: + from go1_pro_sdk import Go1, Velocity, Pose + + with Go1() as dog: + dog.stand_up() + time.sleep(2) + dog.set_walk_mode() + dog.walk(Velocity(vx=0.3)) # 前进 + time.sleep(2) + dog.stop() + dog.stand_down() +""" +import time +from typing import Optional + +from .mqtt_client import Go1MQTT, DEFAULT_HOST, DEFAULT_PORT +from .types import LED, Mode, Pose, Velocity + + +class Go1: + """Go1 高层控制对象.""" + + def __init__(self, host: str = DEFAULT_HOST, port: int = DEFAULT_PORT): + self._mqtt = Go1MQTT(host, port) + + def __enter__(self): + return self + + def __exit__(self, *args): + self.close() + + def close(self): + self._mqtt.disconnect() + + # ===== 起立 / 趴下 ===== + + def stand_up(self) -> None: + """从趴地起立 (forceStand).""" + self._mqtt.switch_mode(Mode.STAND_UP) + + def stand_down(self) -> None: + """趴下.""" + self._mqtt.switch_mode(Mode.STAND_DOWN) + + def recover_stand(self) -> None: + """从任意姿态恢复站立.""" + self._mqtt.switch_mode(Mode.RECOVER_STAND) + + def damping(self) -> None: + """阻尼模式 (电机失能, 关节自由可手动转动).""" + self._mqtt.switch_mode(Mode.DAMPING) + + # ===== 运动模式切换 ===== + + def set_stand_mode(self) -> None: + """进入 stand 模式 (能用 pose() 调姿态).""" + self.pose(Pose()) # 先零姿态 + self._mqtt.switch_mode(Mode.STAND) + + def set_walk_mode(self) -> None: + """进入 walk 模式 (能用 walk() 给速度).""" + self.walk(Velocity()) + self._mqtt.switch_mode(Mode.WALK) + + def set_run_mode(self) -> None: + """进入 run 模式 (高速 trot).""" + self.walk(Velocity()) + self._mqtt.switch_mode(Mode.RUN) + + def set_climb_mode(self) -> None: + """进入 climb 模式 (上下楼梯).""" + self.walk(Velocity()) + self._mqtt.switch_mode(Mode.CLIMB) + + # ===== 速度 / 姿态命令 ===== + + def walk(self, cmd: Velocity) -> None: + """发速度命令 (需先 set_walk_mode/run/climb).""" + self._mqtt.send_cmd_vel(cmd) + + def pose(self, cmd: Pose) -> None: + """发姿态命令 (需先 set_stand_mode).""" + self._mqtt.send_cmd_pose(cmd) + + def stop(self) -> None: + """立即零速度 (停止行走).""" + self.walk(Velocity()) + + # ===== 表演动作 ===== + + def dance_1(self) -> None: + self._mqtt.switch_mode(Mode.DANCE_1) + + def dance_2(self) -> None: + self._mqtt.switch_mode(Mode.DANCE_2) + + def straight_hand(self) -> None: + """握手动作.""" + self._mqtt.switch_mode(Mode.STRAIGHT_HAND) + + def jump_yaw(self) -> None: + """原地转 90°.""" + self._mqtt.switch_mode(Mode.JUMP_YAW) + + # ===== LED 头灯 ===== + + def set_led(self, led: LED) -> None: + """设置头灯 RGB.""" + self._mqtt.set_led(led) + + # ===== 通用 ===== + + @property + def mqtt(self) -> Go1MQTT: + """直接访问 MQTT 客户端 (高级用途, 比如订阅 BMS).""" + return self._mqtt diff --git a/go1_pro_sdk/highlevel/mqtt_client.py b/go1_pro_sdk/highlevel/mqtt_client.py new file mode 100644 index 0000000..4cba1ff --- /dev/null +++ b/go1_pro_sdk/highlevel/mqtt_client.py @@ -0,0 +1,142 @@ +"""MQTT 客户端 — 高层控制底层封装.""" +import random +import socket +import struct +import sys +from typing import Callable, Optional + +from .types import LED, Mode, Pose, Velocity, PubTopic, SubTopic + +DEFAULT_HOST = "192.168.123.161" # 树莓派 (MQTT broker 跑在上面) +DEFAULT_PORT = 1883 +DEFAULT_KEEPALIVE = 60 + + +def _clip(v: float, lo: float = -1.0, hi: float = 1.0) -> float: + return max(lo, min(hi, v)) + + +class Go1MQTT: + """高层 MQTT 客户端. + + 用法: + with Go1MQTT() as mqtt: + mqtt.switch_mode(Mode.STAND) + mqtt.send_cmd_pose(Pose(lean_left_right=0.3)) + """ + + def __init__(self, host: str = DEFAULT_HOST, port: int = DEFAULT_PORT, + keepalive: int = DEFAULT_KEEPALIVE, + on_message: Optional[Callable] = None): + try: + import paho.mqtt.client as mqtt_client + except ImportError: + print("❌ paho-mqtt 未安装. 装一下: pip install paho-mqtt") + sys.exit(1) + + self._host = host + self._port = port + self._keepalive = keepalive + self._client_id = f"go1_pro_sdk-{random.randint(0, 100000)}" + + self._mqttc = mqtt_client.Client( + client_id=self._client_id, + callback_api_version=mqtt_client.CallbackAPIVersion.VERSION2, + ) + + def _on_connect(client, userdata, flags, rc, properties): + if rc == 0: + pass # quiet + else: + print(f"[MQTT] connect failed rc={rc}") + + def _on_message(client, userdata, msg): + if on_message: + on_message(msg.topic, msg.payload) + + self._mqttc.on_connect = _on_connect + self._mqttc.on_message = _on_message + try: + self._mqttc.connect(host, port, keepalive) + except (socket.timeout, OSError) as err: + print(f"❌ [MQTT] 连接 {host}:{port} 失败: {err}") + print(" 检查 192.168.123.161 (Pi) 是否在线, MQTT broker 是否运行") + raise + + self._mqttc.loop_start() + + def __enter__(self): + return self + + def __exit__(self, *args): + self.disconnect() + + def disconnect(self): + try: + self._mqttc.loop_stop() + self._mqttc.disconnect() + except Exception: + pass + + # ===== 订阅 ===== + + def subscribe(self, topic: str, qos: int = 0): + """订阅一个 topic. 消息到达时调用构造函数传入的 on_message.""" + self._mqttc.subscribe(topic, qos) + + # ===== 模式切换 ===== + + def switch_mode(self, mode): + """切换运动模式. + + Args: + mode: Mode 枚举或字符串. 例: Mode.STAND_UP / "standUp" + """ + payload = mode.value if isinstance(mode, Mode) else str(mode) + self._mqttc.publish(PubTopic.ACTION.value, payload, qos=1) + + # ===== 速度/姿态命令 ===== + + def send_cmd_vel(self, cmd: Velocity): + """发送速度命令 (须先 walk/run/climb 模式). + + Unitree 协议: stick 上发 4 个 float (vy, vz, 0, vx), 比较反直觉. + """ + # 先发零给摇杆缓冲清零 (跟原版一致) + self._mqttc.publish(PubTopic.STICK.value, struct.pack("ffff", 0, 0, 0, 0), qos=0) + + vx = _clip(cmd.vx) + vy = _clip(cmd.vy) + vz = _clip(cmd.vz) + self._mqttc.publish( + PubTopic.STICK.value, + struct.pack("ffff", vy, vz, 0.0, vx), + qos=0, + ) + + def send_cmd_pose(self, cmd: Pose): + """发送姿态命令 (须先进入 stand 模式).""" + ll = _clip(cmd.lean_left_right) + tw = _clip(cmd.twist_left_right) + lu = _clip(cmd.look_up_down) + es = _clip(cmd.extend_squat) + self._mqttc.publish( + PubTopic.STICK.value, + struct.pack("ffff", ll, tw, lu, es), + qos=0, + ) + + # ===== LED ===== + + def set_led(self, led: LED): + """设置头灯 RGB 颜色 (0-255).""" + r = max(0, min(255, int(led.r))) + g = max(0, min(255, int(led.g))) + b = max(0, min(255, int(led.b))) + self._mqttc.publish(PubTopic.LED.value, bytes([r, g, b]), qos=1) + + # ===== 原始访问 ===== + + def publish_raw(self, topic: str, payload, qos: int = 0): + """直接发布 (高级用途).""" + self._mqttc.publish(topic, payload, qos) diff --git a/go1_pro_sdk/highlevel/types.py b/go1_pro_sdk/highlevel/types.py new file mode 100644 index 0000000..f00eb40 --- /dev/null +++ b/go1_pro_sdk/highlevel/types.py @@ -0,0 +1,68 @@ +"""高层控制数据类型 (移植自 unitree-go1-py).""" +from enum import Enum +from typing import NamedTuple + + +class Mode(str, Enum): + """可切换的运动模式 (MQTT controller/action payload).""" + DAMPING = "damping" + STAND_UP = "standUp" + STAND_DOWN = "standDown" + RECOVER_STAND = "recoverStand" + STAND = "stand" + WALK = "walk" + RUN = "run" + CLIMB = "climb" + # 表演模式 + DANCE_1 = "dance1" + DANCE_2 = "dance2" + STRAIGHT_HAND = "straightHand1" + JUMP_YAW = "jumpYaw" + + +class Velocity(NamedTuple): + """走动速度命令. + + 需要先进入 walk/run/climb 模式才生效. 各值范围 -1..+1. + """ + vx: float = 0.0 # 前后 (前正) + vy: float = 0.0 # 左右 (左正) + vz: float = 0.0 # 转向角速度 (左转正) + + +class Pose(NamedTuple): + """姿态命令 (需先进入 stand 模式). + + 各值范围 -1..+1. + """ + lean_left_right: float = 0.0 # 身体左右倾 + twist_left_right: float = 0.0 # 身体扭转 + look_up_down: float = 0.0 # 抬头/低头 + extend_squat: float = 0.0 # 撑直/下蹲 + + +class LED(NamedTuple): + """LED RGB (0-255).""" + r: int = 0 + g: int = 0 + b: int = 0 + + +# ===== MQTT topics ===== + +class PubTopic(str, Enum): + """发布主题.""" + ACTION = "controller/action" # 切换运动模式 + STICK = "controller/stick" # 摇杆命令 (4 floats) + CODE = "programming/code" # 编程接口 (高级) + CURRENT_ACTION = "programming/current_action" + RUN = "programming/run" + LED = "face_light/color" # 头灯颜色 + + +class SubTopic(str, Enum): + """订阅主题.""" + BMS = "bms/state" + FIRMWARE = "firmware/version" + CODE = "programming/code" + ACTION = "programming/action" # 当前正在执行的动作 diff --git a/pyproject.toml b/pyproject.toml index b6403fe..5e79c5e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -19,10 +19,9 @@ classifiers = [ "Programming Language :: Python :: 3.8", "Topic :: Scientific/Engineering :: Robotics", ] -dependencies = [] # 纯标准库 - -[project.optional-dependencies] -mqtt = ["paho-mqtt>=2.0.0"] # 用 MQTT 切换 sportMode 模式时需要 +dependencies = [ + "paho-mqtt>=2.0.0", # 高层 MQTT 控制 (Go1 / Go1MQTT) +] # lowlevel (MCUClient) 不需要任何依赖, 纯标准库 [project.urls] Homepage = "https://github.com/your-username/go1_pro_sdk" @@ -32,6 +31,7 @@ Documentation = "https://github.com/your-username/go1_pro_sdk/blob/main/docs/" packages = ["go1_pro_sdk", "go1_pro_sdk.codec", "go1_pro_sdk.connection", + "go1_pro_sdk.highlevel", "go1_pro_sdk.safety", "go1_pro_sdk.types", "go1_pro_sdk.utils"] diff --git a/tests/test_highlevel.py b/tests/test_highlevel.py new file mode 100644 index 0000000..c4dd2d1 --- /dev/null +++ b/tests/test_highlevel.py @@ -0,0 +1,144 @@ +"""高层 MQTT 控制测试 (用 unittest.mock 替换 paho.mqtt, 不需要真实 broker).""" +import struct +from unittest.mock import MagicMock, patch +import pytest + + +@pytest.fixture +def mock_mqtt(): + """打补丁让 Go1MQTT 用 mock client, 不实际连接.""" + with patch('paho.mqtt.client.Client') as MockClient: + instance = MagicMock() + MockClient.return_value = instance + yield instance + + +def test_velocity_default(): + from go1_pro_sdk import Velocity + v = Velocity() + assert v.vx == 0.0 + assert v.vy == 0.0 + assert v.vz == 0.0 + + +def test_pose_named_arg(): + from go1_pro_sdk import Pose + p = Pose(lean_left_right=0.5) + assert p.lean_left_right == 0.5 + assert p.twist_left_right == 0.0 + + +def test_mode_enum_string(): + from go1_pro_sdk import Mode + assert Mode.STAND_UP.value == 'standUp' + assert Mode.DANCE_1.value == 'dance1' + assert Mode.DAMPING.value == 'damping' + + +def test_go1mqtt_connect_and_disconnect(mock_mqtt): + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT(host='localhost', port=1883) + mock_mqtt.connect.assert_called_once_with('localhost', 1883, 60) + mock_mqtt.loop_start.assert_called_once() + + client.disconnect() + mock_mqtt.loop_stop.assert_called() + mock_mqtt.disconnect.assert_called() + + +def test_switch_mode_publishes_to_action(mock_mqtt): + from go1_pro_sdk import Mode + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + + client.switch_mode(Mode.STAND_UP) + + # 应该发送 controller/action = "standUp" + mock_mqtt.publish.assert_called_with('controller/action', 'standUp', qos=1) + + +def test_switch_mode_string_also_works(mock_mqtt): + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + client.switch_mode('walk') + mock_mqtt.publish.assert_called_with('controller/action', 'walk', qos=1) + + +def test_send_cmd_vel_publishes_4_floats(mock_mqtt): + from go1_pro_sdk import Velocity + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + + client.send_cmd_vel(Velocity(vx=0.3, vy=0.1, vz=-0.2)) + + # 应该有 2 次 publish: 先归零再发实际 + assert mock_mqtt.publish.call_count >= 2 + last_call = mock_mqtt.publish.call_args_list[-1] + topic, payload = last_call.args[0], last_call.args[1] + assert topic == 'controller/stick' + # 协议: (vy, vz, 0, vx) — unitree 反直觉的顺序 + vy, vz, _, vx = struct.unpack('ffff', payload) + assert abs(vy - 0.1) < 1e-5 + assert abs(vz - (-0.2)) < 1e-5 + assert abs(vx - 0.3) < 1e-5 + + +def test_send_cmd_vel_clips_to_1(mock_mqtt): + from go1_pro_sdk import Velocity + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + + client.send_cmd_vel(Velocity(vx=5.0, vy=-3.0, vz=0.0)) # 超界 + last_payload = mock_mqtt.publish.call_args_list[-1].args[1] + vy, vz, _, vx = struct.unpack('ffff', last_payload) + assert vx == 1.0 + assert vy == -1.0 + + +def test_send_cmd_pose(mock_mqtt): + from go1_pro_sdk import Pose + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + + client.send_cmd_pose(Pose(lean_left_right=0.3, twist_left_right=0.0, + look_up_down=0.2, extend_squat=-0.5)) + payload = mock_mqtt.publish.call_args.args[1] + ll, tw, lu, es = struct.unpack('ffff', payload) + assert abs(ll - 0.3) < 1e-5 + assert abs(lu - 0.2) < 1e-5 + assert abs(es - (-0.5)) < 1e-5 + + +def test_set_led_publishes_3_bytes(mock_mqtt): + from go1_pro_sdk import LED + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + + client.set_led(LED(255, 128, 0)) + last_call = mock_mqtt.publish.call_args + assert last_call.args[0] == 'face_light/color' + assert last_call.args[1] == bytes([255, 128, 0]) + + +def test_set_led_clips_0_255(mock_mqtt): + from go1_pro_sdk import LED + from go1_pro_sdk.highlevel import Go1MQTT + client = Go1MQTT() + + client.set_led(LED(300, -10, 100)) + assert mock_mqtt.publish.call_args.args[1] == bytes([255, 0, 100]) + + +def test_go1_high_level_api_calls_mqtt(mock_mqtt): + """Go1 类的方法应该都委托给 Go1MQTT.""" + from go1_pro_sdk import Go1, Velocity, LED + + with Go1() as dog: + dog.stand_up() + dog.dance_1() + dog.walk(Velocity(vx=0.5)) + dog.set_led(LED(0, 255, 0)) + dog.stand_down() + + # 应该有多次 publish + assert mock_mqtt.publish.call_count >= 5