FollowPathPointsAction.py

"""
FollowPathPointsAction — 跟随路径点导航
功能：沿预定义路径点序列进行导航移动，依次经过所有路径点
注意：机器人必须位于第一个路径点附近才能开始执行
固件：最低固件版本 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

# 路径点列表（第一个点必须靠近机器人当前位置）
PATH_POINTS = [
    {"x": 9.5,  "y": -4.2, "z": 0},
    {"x": 12.8, "y": -5.6, "z": 0},
    {"x": 14.8, "y": -4.2, "z": 0},
]

SPEED_RATIO     = 0.8
PRECISION_MM    = 200   # 到达精度（毫米），需配合 "precise" flag
POLL_INTERVAL_S = 1.0
# ──────────────────────────────────────────────────────


def build_payload():
    return {
        "action_name": "agent.actions.FollowPathPointsAction",
        "options": {
            "path_points": PATH_POINTS,
            "move_options": {
                "mode": 0,
                "flags": ["precise"],
                "speed_ratio": SPEED_RATIO,
                "acceptable_precision": PRECISION_MM,
            },
        },
    }


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} 已经过全部 {len(PATH_POINTS)} 个路径点")
            else:
                print(f"[失败] action_id={action_id} result={result} reason={reason}")
                if "CANNOT_REACH_TARGET" 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():
    print(f"[FollowPathPointsAction] 共 {len(PATH_POINTS)} 个路径点")
    for i, p in enumerate(PATH_POINTS):
        prefix = "★ 起点" if i == 0 else f"  {i+1}"
        print(f"  {prefix}. ({p['x']}, {p['y']})")
    print(f"  注意：机器人需在起点 ({PATH_POINTS[0]['x']}, {PATH_POINTS[0]['y']}) 附近")

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