Python_unitree_demos/MultiFloorMoveAction.py at main · unitreerobotics/Python_unitree_demos · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
"""
MultiFloorMoveAction — 跨楼层自主导航移动
功能：自动规划跨楼层路径（含乘梯），导航至目标楼层目标点
依赖：需要配置多楼层地图（slamware.agent.multi_floor 能力）
"""

import math
import time
import requests

# ── 配置 ──────────────────────────────────────────────
ROBOT_IP = "10.160.129.252"
PORT     = 1448
BASE_URL = f"http://{ROBOT_IP}:{PORT}/api/core/motion/v1/actions"

# 绕过系统代理（避免 127.0.0.1:7890 超时）
SESSION = __import__("requests").Session()
SESSION.trust_env = False

# 目标坐标（目标楼层地图坐标，米）
TARGET_X = 5.0
TARGET_Y = 3.0
TARGET_Z = 0

# 是否启用精确导航（精确到点 + 指定到达朝向）
PRECISE_MODE = False
YAW          = math.pi / 2   # 到达后朝向（弧度），仅 PRECISE_MODE=True 时生效
PRECISION_MM = 100           # 到达精度（毫米），仅 PRECISE_MODE=True 时生效

# 导航模式：0=自由导航  2=轨道优先
MODE = 0

SPEED_RATIO     = 0.8
FAIL_RETRY      = 3
POLL_INTERVAL_S = 2.0
# ──────────────────────────────────────────────────────


def build_payload():
    if PRECISE_MODE:
        flags = ["precise", "with_yaw", "with_directed_virtual_track"]
        move_options = {
            "mode": 2,
            "flags": flags,
            "yaw": YAW,
            "acceptable_precision": PRECISION_MM,
            "fail_retry_count": FAIL_RETRY,
            "speed_ratio": SPEED_RATIO,
        }
    else:
        move_options = {
            "mode": MODE,
            "flags": [],
            "fail_retry_count": FAIL_RETRY,
            "speed_ratio": SPEED_RATIO,
        }

    return {
        "action_name": "agent.actions.MultiFloorMoveAction",
        "options": {
            "target": {"x": TARGET_X, "y": TARGET_Y, "z": TARGET_Z},
            "move_options": move_options,
        },
    }


def poll_until_done(action_id):
    url = f"{BASE_URL}/{action_id}"
    while True:
        resp = SESSION.get(url, timeout=5)
        data = resp.json()
        status = data["state"]["status"]
        result = data["state"]["result"]
        reason = data["state"].get("reason", "")

        if status == 4:
            if result == 0:
                print(f"[完成] action_id={action_id} 跨楼层导航成功")
            else:
                print(f"[失败] action_id={action_id} result={result} reason={reason}")
                if "start_action_failed" in reason:
                    print("  提示：请确认已配置多楼层地图")
            return result == 0

        status_desc = {0: "初始化中", 1: "跨楼层导航中"}.get(status, str(status))
        print(f"[{status_desc}] action_id={action_id} ...")
        time.sleep(POLL_INTERVAL_S)


def main():
    mode_desc = "精确模式" if PRECISE_MODE else "基础模式"
    print(f"[MultiFloorMoveAction] 目标=({TARGET_X}, {TARGET_Y})  {mode_desc}")

    payload = build_payload()
    resp = SESSION.post(BASE_URL, json=payload, timeout=5)
    if resp.status_code != 200:
        print(f"[错误] HTTP {resp.status_code}: {resp.text}")
        return

    data = resp.json()
    action_id = data["action_id"]
    stage = data.get("stage", "")
    print(f"[已创建] action_id={action_id}  stage={stage}")

    poll_until_done(action_id)


if __name__ == "__main__":
    main()