用户输入与通信接口

1. 概述

DR02 Pro SDK 提供多种用户输入方式，包括键盘、手柄（Gamepad）和速度指令话题，统一由 UserCommandInterface 处理。

2. 开发者模式

DR02 Pro 支持三种开发者模式，SDK 运行前需通过手柄进入对应模式：

开发者模式	控制范围
高层运动控制模式	SDK 通过 /MOTION_STATE、/GAIT、/STEER 等高层指令控制运动
全身关节控制模式	SDK 通过 /JOINTS_CMD 直接控制全身 31 个关节
上半身关节控制模式	SDK 通过 /JOINTS_CMD 控制腰部和双臂，腿部由机器人内部策略控制

重要： /JOINTS_CMD 同一时间只允许一个发布者。状态机运行期间不得同时运行其他关节命令发布者。

3. 用户命令接口

3.1 功能说明

用户命令接口 UserCommandInterface 统一处理多种输入源：

• 订阅 /GAMEPAD_KEY 接收手柄按键
• 订阅 /STEER 接收归一化速度指令
• 订阅 /REAL_STEER 接收实际速度指令
• 独立线程运行 KeyboardLoop 处理键盘输入

3.2 UserCommand 结构

struct UserCommand {
    double time_stamp;           // 时间戳
    int safe_control_mode;       // 安全控制模式（0=正常）
    uint8_t target_mode;         // 目标运动状态
    float forward_vel_scale;     // 前进速度比例
    float side_vel_scale;        // 侧移速度比例
    float turning_vel_scale;     // 旋转速度比例
};

4. 控制方式

4.1 键盘控制

按键	功能	适用状态
z	进入 ZeroPos	Idle
c	进入 RLControl	ZeroPos
r	进入 JointDamping	任意
w/s	前进 / 后退	RLControl
a/d	左移 / 右移	RLControl
q/e	左转 / 右转	RLControl

键盘速度指令为固定归一化值 0.70，松开按键后约 320ms 自动归零。

4.2 手柄控制

按键	功能
L1	进入 ZeroPos（仅 Idle 状态有效）
L2	进入 RLControl（仅 ZeroPos 状态有效）
R2	进入 JointDamping
左摇杆	前进 / 左移 / 后退 / 右移
右摇杆	旋转

手柄按键通过 /GAMEPAD_KEY 话题传递（字符串消息，如 G20_KEY_L1）。

4.3 速度指令接口

状态机同时订阅两个速度指令话题：

话题	说明	输入范围
/STEER	归一化速度指令	[-1.0, 1.0]
/REAL_STEER	实际速度指令	内部自动归一化（x/1.0, y/0.75, yaw/2.25）

5. 使用示例

5.1 手柄按键订阅

// 订阅手柄按键事件
auto gamepad_sub = node->create_subscription<std_msgs::msg::String>(
    "/GAMEPAD_KEY", 10,
    [](const std_msgs::msg::String::SharedPtr msg) {
        RCLCPP_INFO(node->get_logger(), "Gamepad key: %s", msg->data.c_str());

        // 根据按键类型执行相应操作
        if (msg->data == "G20_KEY_L1") {
            // L1键按下，可触发ZeroPos状态
        } else if (msg->data == "G20_KEY_L2") {
            // L2键按下，可触发RLControl状态
        }
    });

5.2 速度指令发布

// 发布归一化速度指令
auto steer_pub = node->create_publisher<drdds::msg::Steer>(
    "/STEER", 10);

// 构造速度指令
drdds::msg::Steer steer_cmd;
steer_cmd.x = 0.5f;      // 前进50%速度
steer_cmd.y = 0.0f;      // 不侧移
steer_cmd.z = 0.0f;      // 不升降
steer_cmd.roll = 0.0f;   // 不翻滚
steer_cmd.pitch = 0.0f;  // 不下俯
steer_cmd.yaw = 0.3f;    // 右转30%角速度

// 发布指令（建议20Hz频率）
steer_pub->publish(steer_cmd);

5.3 实际速度指令发布

// 发布实际速度指令（带单位）
auto real_steer_pub = node->create_publisher<drdds::msg::Steer>(
    "/REAL_STEER", 10);

drdds::msg::Steer real_cmd;
real_cmd.x = 0.5f;    // 前进 0.5 m/s
real_cmd.y = 0.3f;    // 左移 0.3 m/s
real_cmd.yaw = 0.5f;  // 旋转 0.5 rad/s

real_steer_pub->publish(real_cmd);

6. 安全保护机制

6.1 SafeController 监控

SafeController 在独立线程中以 2ms 周期运行，监控以下异常：

检测项	方法	触发条件	safe_control_mode
关节数据异常	IsJointDataNormal()	NaN 值 / 超时 30ms / 数据不变 100 周期	3
电机过温	IsMotorTemperatureNormal()	温度 > 110°C	2
IMU 数据异常	IsImuDataNormal()	NaN / 超时 40ms	4

6.2 错误状态码

union RobotErrorState {
    struct {
        uint32_t imu_num_error : 1;          // IMU 数值异常
        uint32_t imu_update_overtime : 1;    // IMU 更新超时
        uint32_t motor_heat_warn : 1;        // 电机过温
        uint32_t joint_num_error : 1;        // 关节数值异常
        uint32_t joint_update_error : 1;     // 关节更新超时
        uint32_t reserved : 26;
    };
    uint32_t error_code;
};

6.3 急停机制说明

重要：DR02 Pro 当前没有实现跨进程急停机制（如 /EMERGENCY_STOP_SIGNAL）。

DR02 Pro 的紧急停止仅依赖以下方式：

1. 手柄红色停止按钮（推荐）

   • 发生异常时优先按下手柄红色按钮，直接断开控制信号
   • 机器人底层控制器收到信号后自动进入保护状态

2. safe_control_mode 安全降级

   • SafeController 检测到异常时设置 safe_control_mode != 0
   • 下一周期自动切换到 JointDamping 状态（全身纯阻尼）
   • 持续 3 秒后回到 Idle 状态

3. Ctrl+C 软终止

   • 终端中按 Ctrl+C 停止 SDK 程序
   • 状态机退出后不再发布 /JOINTS_CMD，机器人失去支撑可能跌倒
   • 仅适用于仿真环境或需要立即断电的场景

[!WARNING]

实机运行时若需紧急停止，应优先使用手柄红色停止按钮。