init pro_sdk

2026-06-20 20:02:36 +08:00
commit 5ecd422a59
49 changed files with 4226 additions and 0 deletions
--- a/examples/monitor_state.py
+++ b/examples/monitor_state.py
@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+"""实时监听 LowState (只读, 不控制电机).
+
+用法:
+    python examples/monitor_state.py --duration 30 --verbose
+"""
+import argparse
+import math
+import signal
+import sys
+import time
+
+from go1_pro_sdk import MCUClient, JOINT_NAMES, decode_sn
+
+running = True
+
+
+def sigint(s, f):
+    global running
+    running = False
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument('--state', default=None, help='Blowfish state 文件 (默认用包内置)')
+    p.add_argument('--duration', type=float, default=30)
+    p.add_argument('--rate', type=float, default=5, help='打印频率 Hz')
+    p.add_argument('--verbose', '-v', action='store_true')
+    args = p.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    print('📡 监听 LowState')
+    print(f'  时长 {args.duration}s, 打印 {args.rate}Hz')
+
+    with MCUClient(state_path=args.state) as client:
+        recv_count = client.wake_mcu(50)
+        print(f'唤醒: 收到 {recv_count}/50 帧')
+        if recv_count == 0:
+            print('❌ 无回包. 检查 Pi 抢占源或 state 是否正确')
+            return 1
+
+        start = time.time()
+        last_print = 0
+        n_decoded = 0
+        damping = __import__('go1_pro_sdk').LowCmd().all_damping()
+
+        print(f"\n{'时间':>6} {'电量':>5} {'FR_0':>8} {'FR_1':>8} {'FR_2':>8} {'rpy[°]':>20}")
+        print('-' * 70)
+
+        while running and time.time() - start < args.duration:
+            client.send(damping)
+            time.sleep(0.005)
+            state = client.recv_latest()
+            if state is None:
+                continue
+            n_decoded += 1
+
+            now = time.time()
+            if now - last_print >= 1.0 / args.rate:
+                t = now - start
+                rpy_str = (f"{math.degrees(state.imu.rpy[0]):+5.1f},"
+                           f"{math.degrees(state.imu.rpy[1]):+5.1f},"
+                           f"{math.degrees(state.imu.rpy[2]):+5.1f}")
+                print(f'{t:6.1f} {state.bms.SOC:4d}% '
+                      f'{state.motorState[0].q:+8.3f} '
+                      f'{state.motorState[1].q:+8.3f} '
+                      f'{state.motorState[2].q:+8.3f} '
+                      f'{rpy_str:>20}')
+                if args.verbose:
+                    print(f'    SN={decode_sn(state.SN)} '
+                          f'acc_z={state.imu.accelerometer[2]:.2f} m/s²  '
+                          f'BMS current={state.bms.current_a:.2f}A')
+                last_print = now
+
+        print(f'\n解码 {n_decoded} 帧, 退出.')
+
+
+if __name__ == '__main__':
+    sys.exit(main() or 0)