Python_unitree_demos/MoveToTagAction.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
"""
MoveToTagAction — 精准标签对接
功能：通过视觉/激光标签进行精准对接，机器人自主导航并对准标签停靠
需要：视觉标签或激光标签硬件支持
"""

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

# 标签类型：0=二维码视觉标签  1=激光标签  2=激光反光板
TAG_ID_TYPE = 0

# 标签编号
TAG_ID = 1

# 最终对接距离（米），停在标签前方该距离处
BLOCK_DISTANCE = 0.05

# 对接前是否先重定位
RELOCALIZATION = False

# 失败重试次数
FAIL_RETRY_COUNT = 3

# 移动前是否先旋转朝向标签
ROTATE_BEFORE_MOVE = True

# 到达后的 Yaw 偏移量（弧度）
FINAL_YAW_OFFSET = 0.0

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

TAG_TYPE_DESC = {0: "二维码视觉标签", 1: "激光标签", 2: "激光反光板"}


def build_payload():
    return {
        "action_name": "agent.actions.MoveToTagAction",
        "options": {
            "target_id": {
                "id_type": TAG_ID_TYPE,
                "tag_id": TAG_ID,
            },
            "block_distance": BLOCK_DISTANCE,
            "relocalization": RELOCALIZATION,
            "fail_retry_count": FAIL_RETRY_COUNT,
            "rotate_before_move": ROTATE_BEFORE_MOVE,
            "final_yaw_offset": FINAL_YAW_OFFSET,
        },
    }


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():
    tag_desc = TAG_TYPE_DESC.get(TAG_ID_TYPE, str(TAG_ID_TYPE))
    print(f"[MoveToTagAction] 标签类型={tag_desc}  标签ID={TAG_ID}  距离={BLOCK_DISTANCE}m")

    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()