BackOffFromTagAction.py

"""
BackOffFromTagAction — 从标签前后退
功能：从二维码/标签位置前方后退离开，常用于从充电桩或货架脱离
"""

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=矢道后退（持续对齐标签，需机器人处于对接状态）
BACKUP_MODE = 0

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

# 后退距离（米），不填则后退到机器人可以转身为止（设为 None）
BACKUP_DISTANCE = 0.5

# 是否为向后对接模式（True 时实际向前移动）
BACKWARD_DOCKING = False

POLL_INTERVAL_S = 0.5
# ──────────────────────────────────────────────────────


def build_payload():
    options = {
        "backup_mode": BACKUP_MODE,
        "tag_type": TAG_TYPE,
        "backward_docking": BACKWARD_DOCKING,
    }
    if BACKUP_DISTANCE is not None:
        options["backup_distance"] = BACKUP_DISTANCE
    return {
        "action_name": "agent.actions.BackOffFromTagAction",
        "options": 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:
                dist_desc = f"{BACKUP_DISTANCE}m" if BACKUP_DISTANCE else "直到可转身"
                print(f"[完成] action_id={action_id} 后退 {dist_desc} 完成")
            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():
    mode_desc = "矢道后退" if BACKUP_MODE == 1 else "自由后退"
    dist_desc = f"{BACKUP_DISTANCE}m" if BACKUP_DISTANCE else "直到可转身"
    print(f"[BackOffFromTagAction] 模式={mode_desc}  距离={dist_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"]
    print(f"[已创建] action_id={action_id}")

    poll_until_done(action_id)


if __name__ == "__main__":
    main()