Locomotion 控制完整示例

本示例演示 GR-W 轮式底盘控制流程：进入 grw_policy_task（状态 3），将速度源切到 Navigation 并回读确认，再发送低速底盘命令。

涉及状态： GRW 策略任务状态

涉及接口： set_velocity、set_velocity_source

运行前必读

1. 在创建任何 DDS participant 前设置 export FASTDDS_BUILTIN_TRANSPORTS=UDPv4（见连接说明）。
2. 现场确认急停可用、周围净空后再运行。前进段约移动 0.4 m，旋转段约原地左转 20°（名义角度 0.4 rad，策略平滑后实际约 0.3 rad）。
3. 机器人本体基座数据中的 vel_B/pos_W 在静止时也存在明显偏置与漂移，不能作为"机器人已停止"的判据，以现场观察为准。

Python

import os
import time

os.environ.setdefault("FASTDDS_BUILTIN_TRANSPORTS", "UDPv4")

from fourier_aurora_client import AuroraClient


def wait_fsm(client, expected, timeout=4.0):
    deadline = time.monotonic() + timeout
    while client.get_fsm_state() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"状态未在 {timeout}s 内切换到 {expected}")
        time.sleep(0.05)


def wait_velocity_source(client, expected, timeout=2.0):
    deadline = time.monotonic() + timeout
    while client.get_velocity_source() != expected:
        if time.monotonic() >= deadline:
            raise RuntimeError(f"速度源未在 {timeout}s 内切换到 {expected}")
        time.sleep(0.05)


client = AuroraClient.get_instance(domain_id=123, robot_name="grw")
try:
    time.sleep(1.0)

    # Step 1: 进入 grw_policy_task（状态 3），并回读确认
    input("Press Enter to set FSM to grw_policy_task (state 3)...")
    if not client.set_fsm_state(3):
        raise RuntimeError("状态切换请求未受理")
    wait_fsm(client, 3)

    # Step 2: 速度源切到 Navigation(2)，并回读确认
    input("Press Enter to set velocity source to Navigation...")
    if not client.set_velocity_source(2):
        raise RuntimeError("速度源切换请求未受理")
    wait_velocity_source(client, 2)

    # Step 3: 低速前进 0.2 m/s，持续 2 s（duration 到期后命令自动失效）
    input("Press Enter to move forward at 0.2 m/s for 2 s...")
    print("Moving forward at 0.2 m/s")
    client.set_velocity(0.2, 0.0, 0.0, 2.0)
    time.sleep(2.0)
    client.set_velocity(0.0, 0.0, 0.0, 1.0)   # 显式停止

    # Step 4: 原地左转 0.2 rad/s，持续 2 s（正值 yaw 为逆时针）
    input("Press Enter to rotate anti-clockwise at 0.2 rad/s for 2 s...")
    print("Rotating anti-clockwise at 0.2 rad/s")
    client.set_velocity(0.0, 0.0, 0.2, 2.0)
    time.sleep(2.0)
    client.set_velocity(0.0, 0.0, 0.0, 1.0)   # 显式停止

    print("Motion command demonstration complete")
finally:
    client.set_velocity(0.0, 0.0, 0.0, 1.0)   # 兜底零速度
    client.set_velocity_source(0)             # 交还手柄控制
    client.close()

行为说明：

• 状态 3 与速度源 2 均需"请求 + 回读确认"两步，服务返回 True 只代表受理。
• 速度源不是 2 时，速度命令会被 AuroraCore 忽略，并在机器人日志记录 Ignore velocity command because current velocity source is not 'Navigation'。
• duration 到期后命令自动失效；例如 0.05 m/s、duration=0.5 的命令在到期后约 0.3 s 内速度回落，随后仍应显式补发零速度。