RecoverLocalizationAction.py

"""
RecoverLocalizationAction — 自动重定位
功能：不指定区域时为全局重定位，指定区域时为局部重定位
"""

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

# 是否为局部重定位（True=局部，需配置下方 AREA；False=全局重定位）
LOCAL_MODE = False

# 局部重定位区域（仅 LOCAL_MODE=True 时生效）
AREA = {
    "x": 1.0,       # 区域左下角 X（米）
    "y": 1.0,       # 区域左下角 Y（米）
    "width": 3.0,   # 区域宽度（米）
    "height": 3.0,  # 区域高度（米）
}

# 重定位超时时间（毫秒）
MAX_RECOVER_TIME = 30000

# 重定位运动方式："RotateOnly"=旋转重定位  "NoMove"=静止重定位
RECOVER_MOVEMENT_TYPE = "RotateOnly"

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


def build_payload():
    options = {
        "relocalization_options": {
            "max_recover_time": MAX_RECOVER_TIME,
            "recover_movement_type": RECOVER_MOVEMENT_TYPE,
        }
    }
    if LOCAL_MODE:
        options["area"] = AREA
    return {
        "action_name": "agent.actions.RecoverLocalizationAction",
        "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:
                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():
    mode_desc = f"局部重定位 area={AREA}" if LOCAL_MODE else "全局重定位"
    print(f"[RecoverLocalizationAction] {mode_desc}  方式={RECOVER_MOVEMENT_TYPE}  超时={MAX_RECOVER_TIME}ms")

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