SDK 接口

机器人基类接口

RobotInterface 是所有机器人接入的抽象基类。它负责：

创建 ROS2 node（节点名 rl_deploy）
提供机器人状态读取接口
提供关节命令下发接口
提供电池、IMU、温度等扩展读取接口

构造示例

RobotInterface(const std::string& robot_name, int dof_num)

状态读取示例

virtual VecXf GetJointPosition() = 0;    // 获取 12 个关节位置
virtual VecXf GetJointVelocity() = 0;    // 获取 12 个关节速度
virtual VecXf GetJointTorque() = 0;      // 获取 12 个关节力矩
virtual Vec3f GetImuRpy() = 0;           // 获取 IMU 横滚/俯仰/偏航角
virtual Vec3f GetImuAcc() = 0;           // 获取 IMU 三轴加速度
virtual Vec3f GetImuOmega() = 0;         // 获取 IMU 三轴角速度
virtual VecXf GetContactForce() = 0;     // 获取四足足端接触力

命令下发示例

virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) = 0;

其中输入矩阵每行 5 列分别对应：kp、goal_angle_pos、kd、goal_vel、torque_feedforward。

DDS 实现接口

DdsInterface 是 ROS2 话题连接实现。它订阅机器人反馈并发布关节命令。

订阅与发布示例

joint_cmd_pub_ = node_->create_publisher<drdds::msg::JointsDataCmd>("/JOINTS_CMD", 10);
                                        // 发布关节控制命令
joint_data_sub_ = node_->create_subscription<drdds::msg::JointsData>("/JOINTS_DATA", 10, ...);
                                        // 订阅关节状态反馈
imu_data_sub_ = node_->create_subscription<drdds::msg::ImuData>("/IMU_DATA", 10, ...);
                                        // 订阅 IMU 数据
health_data_sub_ = node_->create_subscription<drdds::msg::BatteryData>("/BATTERY_DATA", 10, ...);
                                        // 订阅电池数据

关节命令示例

SetJointCommand 会把输入矩阵映射到 drdds::msg::JointsDataCmd：

msg.data.joints_data[i].position = (input(i, 1) - joint_config_[i].offset) * joint_config_[i].dir;
                                        // 目标位置（含偏置和方向映射）
msg.data.joints_data[i].velocity = input(i, 3) * joint_dir_[i];
                                        // 目标速度
msg.data.joints_data[i].torque = input(i, 4) * joint_dir_[i];
                                        // 前馈力矩
msg.data.joints_data[i].kp = input(i, 0);
                                        // 位置刚度增益
msg.data.joints_data[i].kd = input(i, 2);
                                        // 速度阻尼增益
msg.data.joints_data[i].control_word = kIndexMotorControl;
                                        // 控制字

状态回调示例

void Handler(const drdds::msg::JointsData::SharedPtr msg)       // 关节数据回调
void HandlerIMU(const drdds::msg::ImuData::SharedPtr msg)       // IMU 数据回调
void HandlerHealth(const drdds::msg::BatteryData::SharedPtr msg) // 电池数据回调

机器人控制接口

Lite3Interface 继承 DdsInterface，12 自由度四足机器人，关节数据不做偏置和方向转换。

初始化示例

Lite3Interface(const std::string &robot_name) : DdsInterface(robot_name, 12) {
    ...
}

真机模式下关节命令示例

Lite3Interface 的 SetJointCommand 直接使用输入值，不做偏置和方向映射：

virtual void SetJointCommand(Eigen::Matrix<float, Eigen::Dynamic, 5> input) {
    for (int i = 0; i < dof_num_; ++i) {
        msg.data.joints_data[i].position = input(i, 1);
        msg.data.joints_data[i].velocity = input(i, 3);
        msg.data.joints_data[i].torque = input(i, 4);
        msg.data.joints_data[i].kp = input(i, 0);
        msg.data.joints_data[i].kd = input(i, 2);
        msg.data.joints_data[i].control_word = kIndexMotorControl;
    }
}

真机模式下关节回调示例

Handler 同样直接读取原始值，不做偏置和方向映射：

virtual void Handler(const drdds::msg::JointsData::SharedPtr msg) override {
    for (int i = 0; i < dof_num_; ++i) {
        joint_pos_(i) = msg->data.joints_data[i].position;
        joint_vel_(i) = msg->data.joints_data[i].velocity;
        joint_tau_(i) = msg->data.joints_data[i].torque;
    }
}

ROS2 通信接口

ROS2 话题

主要话题列表：

/JOINTS_CMD — 关节控制命令
/JOINTS_DATA — 关节状态反馈
/IMU_DATA — IMU 数据
/BATTERY_DATA — 电池数据（暂未开放）
/GAMEPAD_DATA — Retroid 手柄数据
/EMERGENCY_STOP_SIGNAL — 跨进程急停信号

消息类型

当前代码直接使用 drdds 中的消息类型：

drdds/msg/JointsData
drdds/msg/JointsDataCmd
drdds/msg/ImuData
drdds/msg/BatteryData
drdds/msg/GamepadData
drdds/msg/StdMsgInt32

数据流示例

键盘控制模式下仿真数据流：

graph LR
    A["/rl_deploy"] -->|/JOINTS_CMD| B["/mujoco_simulation(Sim) or /lite3(Real)"]
    B -->|/IMU_DATA| A
    B -->|/JOINTS_DATA| A

手柄控制模式下真机数据流：

graph LR
    A["/rl_deploy"] -->|/JOINTS_CMD| B["/lite3"]
    B -->|/IMU_DATA| A
    B -->|/JOINTS_DATA| A
    C["/retroid_gamepad"] -->|/GAMEPAD_DATA| A

ROS2 服务

lite3_transfer 提供以下 ROS2 服务：

/SDK_MODE：设置发布频率或退出 SDK 模式（drdds/srv/StdSrvInt32）
/EMERGENCY_STOP：触发急停（drdds/srv/StdSrvInt32）