机器人与 DDS 接口

1. 概述

DR02 Pro SDK 通过 ROS 2 / DDS 中间件与机器人进行数据交互，提供关节控制、状态读取、IMU数据、外设监测等完整接口。

注意： 使用ROS2 工具接收话题、查看频率等前，需要source /opt/robot/scripts/setup_ros2.sh环境变量。

2. 核心 ROS 2 话题

话题	消息类型	说明
/JOINTS_CMD	drdds/msg/JointsCmd	关节控制命令（SDK → 机器人）
/JOINTS_DATA	drdds/msg/Joints	关节状态反馈（机器人 → SDK）
/IMU_DATA_HEAD	sensor_msgs/msg/Imu	头部 IMU 数据
/IMU_DATA_BASE	sensor_msgs/msg/Imu	基座 IMU 数据
/STEER	drdds/msg/Steer	归一化速度指令
/REAL_STEER	drdds/msg/Steer	实际速度指令
/GAMEPAD_KEY	std_msgs/msg/String	手柄按键事件
/BATTERY_DATA	drdds/msg/BatteryData	电池数据
/MOTION_STATE	—	运动状态切换命令
/MOTION_INFO	—	运动状态信息
/GAIT	—	步态切换命令

3. 机器人规格

3.1 基本信息

项目	值
机器人名称	DR2PRO（枚举值 0）
总自由度	31
腰部自由度	3（WaistX, WaistY, WaistZ）
单臂自由度	7（ShoulderY, ShoulderX, ShoulderZ, Elbow, WristZ, WristY, WristX）
单腿自由度	6（HipY, HipX, HipZ, Knee, AnkleY, AnkleX）
颈部自由度	2

关节排列顺序： 腰部(3) → 左臂(7) → 右臂(7) → 左腿(6) → 右腿(6) → 颈部(2)

3.2 关节索引映射表

不同型号机器人的关节配置存在差异，下表定义了每个索引对应的关节名称：

索引	区域	DR02标准版关节名称	DR02Pro版关节名称
0	腰	腰部偏航	腰部偏航
1		x	腰部翻滚
2		x	腰部俯仰
3	左手	左肩俯仰	左肩俯仰
4		左肩翻滚	左肩翻滚
5		左肩偏航	左肩偏航
6		左手肘部	左手肘部
7		x	左腕偏航
8		x	左腕俯仰
9		x	左腕翻滚
10	右手	右肩俯仰	右肩俯仰
11		右肩翻滚	右肩翻滚
12		右肩偏航	右肩偏航
13		右右手肘部	右手肘部
14		x	右腕偏航
15		x	右腕俯仰
16		x	右腕翻滚
17	左腿	左髋俯仰	左髋俯仰
18		左髋翻滚	左髋翻滚
19		左髋偏航	左髋偏航
20		左腿膝关节	左腿膝关节
21		左踝俯仰	左踝俯仰
22		左踝翻滚	左踝翻滚
23	右腿	右髋俯仰	右髋俯仰
24		右髋翻滚	右髋翻滚
25		右髋偏航	右髋偏航
26		右腿膝关节	右腿膝关节
27		右踝俯仰	右踝俯仰
28		右踝翻滚	右踝翻滚
29	头部	x	头部偏航
30		x	头部俯仰

注意： MotorStatus 参数组中，由于 STD 版本与 PRO 版本的关节数量不同，为便于 App 端进行一一对应展示，该数组固定长度为 31。其中，数值 10 表示该关节不存在。

4. 机器人基类接口

4.1 功能说明

RobotInterface 是机器人接入的基类，负责：

• 创建 ROS 2 发布者/订阅者
• 提供关节状态读取（位置、速度、力矩、温度）
• 提供 IMU 数据读取（头部和基座双 IMU）
• 发布关节命令

4.2 构造参数

RobotInterface(robot_name, dof_num=31, leg_dof_num=6, arm_dof_num=7, waist_dof_num=3, neck_dof_num=2, node)

4.3 订阅话题

/JOINTS_CMD    → 发布关节控制命令
/JOINTS_DATA   ← 订阅关节状态反馈
/IMU_DATA_HEAD ← 订阅头部 IMU
/IMU_DATA_BASE ← 订阅基座 IMU

4.4 关节命令格式

每个关节 5 个参数：kp、position、kd、velocity、torque

4.5 关节控制公式

$T = kp \times (pos_{goal} - pos_{real}) + kd \times (vel_{goal} - vel_{real}) + t_{ff}$

4.6 自由度参数设置接口

DR2ProInterface 继承 RobotInterface，仅设置自由度参数：

DR2ProInterface(node, "DR2PRO") : RobotInterface("DR2PRO", 31, 12, 14, 3, 2, node)

不做额外的偏置或方向映射，直接使用原始值。

5. 使用示例

5.1 订阅关节数据

// 订阅关节数据
auto subscription = node->create_subscription<drdds::msg::Joints>(
    "/JOINTS_DATA", 10,
    [](const drdds::msg::Joints::SharedPtr msg) {
        // 处理关节数据
        for (size_t i = 0; i < msg->joint_position.size(); ++i) {
            RCLCPP_INFO(node->get_logger(), "Joint %zu: pos=%.4f vel=%.4f",
                       i, msg->joint_position[i], msg->joint_velocity[i]);
        }
    });

5.2 发布关节命令

// 创建关节命令发布者
auto publisher = node->create_publisher<drdds::msg::JointsCmd>(
    "/JOINTS_CMD", 10);

// 构造关节命令
drdds::msg::JointsCmd cmd;
for (int i = 0; i < 31; ++i) {
    cmd.kp.push_back(600);           // PD增益Kp
    cmd.position.push_back(0.0);      // 目标位置
    cmd.kd.push_back(6);              // PD增益Kd
    cmd.velocity.push_back(0.0);      // 目标速度
    cmd.torque.push_back(0.0);        // 前馈力矩
}

// 发布命令
publisher->publish(cmd);

5.3 订阅IMU数据

// 订阅头部IMU
auto imu_sub = node->create_subscription<sensor_msgs::msg::Imu>(
    "/IMU_DATA_HEAD", 10,
    [](const sensor_msgs::msg::Imu::SharedPtr msg) {
        RCLCPP_INFO(node->get_logger(),
                   "IMU: lin_acc=[%.2f %.2f %.2f] ang_vel=[%.2f %.2f %.2f]",
                   msg->linear_acceleration.x,
                   msg->linear_acceleration.y,
                   msg->linear_acceleration.z,
                   msg->angular_velocity.x,
                   msg->angular_velocity.y,
                   msg->angular_velocity.z);
    });

6. 运行位置

SDK 可在以下位置运行，只要 ROS 2 / DDS 网络互通：

位置	IP	说明
开发主机	用户配置	需通过网线连接机器人背部网口
AOS 主机	10.21.33.103	机器人端计算设备
NOS 主机	10.21.33.106	已预装消息接口库