Python_unitree_demos/EnterElevatorAction.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
"""
EnterElevatorAction — 机器人进电梯
功能：机器人自主进入电梯，支持前门/后门进入及朝向控制
固件：最低固件版本 5.1.1
"""

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

# 电梯 ID（在地图中配置的电梯标识）
ELEVATOR_ID = "elevator_01"

# 进入方式："front_door"=从前门进入  "rear_door"=从后门进入
ELEVATOR_DOOR_FLAG = "front_door"

# 进入后朝向："face_to_front_door"=面向前门  "face_to_rear_door"=面向后门
ELEVATOR_STOPPING_YAW = "face_to_front_door"

# 进电梯操作总超时时长（毫秒）
TIMEOUT_IN_MS = 30000

# 保守模式：True=前往电梯中心点  False=前往电梯内里面
USE_CONSERVATIVE_MODE = True

POLL_INTERVAL_S = 1.0
# ──────────────────────────────────────────────────────


def build_payload():
    return {
        "action_name": "agent.actions.EnterElevatorAction",
        "options": {
            "elevator_id": ELEVATOR_ID,
            "enter_elevator_options": {
                "elevator_door_flag": ELEVATOR_DOOR_FLAG,
                "elevator_stopping_yaw": ELEVATOR_STOPPING_YAW,
                "timeout_in_ms": TIMEOUT_IN_MS,
                "use_conservative_mode": USE_CONSERVATIVE_MODE,
            },
        },
    }


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}")
            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():
    print(f"[EnterElevatorAction] 电梯={ELEVATOR_ID}  进入方式={ELEVATOR_DOOR_FLAG}  朝向={ELEVATOR_STOPPING_YAW}")

    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"]
    print(f"[已创建] action_id={action_id}")

    poll_until_done(action_id)


if __name__ == "__main__":
    main()