硬件桥接层

概述

lite3_transfer 是 Lite3 的底层硬件与 ROS2 桥接层，负责：

通过 UDP 与 Lite3 机器人固件通信（使用 Lite3_MotionSDK）
将机器人状态转换为 ROS2 话题（/JOINTS_DATA、/IMU_DATA）
将 ROS2 控制命令转换为 UDP 下发（/JOINTS_CMD）
读取 Retroid 手柄数据并发布到 /GAMEPAD_DATA
提供 /SDK_MODE 和 /EMERGENCY_STOP 服务

节点信息

# ros2 node info /lite3
/lite3_transfer
  Subscribers:
    /JOINTS_CMD: drdds/msg/JointsDataCmd
  Publishers:
    /IMU_DATA: drdds/msg/ImuData
    /JOINTS_DATA: drdds/msg/JointsData
    /EMERGENCY_STOP_SIGNAL: drdds/msg/StdMsgInt32
  Service Servers:
    /SDK_MODE: drdds/srv/StdSrvInt32
    /EMERGENCY_STOP: drdds/srv/StdSrvInt32

抽象基类

lite3_transfer 定义了自己的 RobotInterface 抽象基类，与 Lite3_sdk_deploy 中的同名类互相独立、无继承关系。它面向硬件桥接层的职责，定义了从 UDP 读取状态和发布 ROS2 话题的接口。

类成员

std::string robot_name_;      // 机器人名称
const int dof_num_;           // 自由度数量
Eigen::Matrix<float, Eigen::Dynamic, 5> joint_cmd_;  // 关节命令矩阵
std::atomic<bool> start_flag_;  // 启动标志

状态读取接口

virtual void Start() = 0;
virtual void Stop() = 0;
virtual double GetInterfaceTimeStamp() = 0;
virtual VecXf GetJointPosition() = 0;
virtual VecXf GetJointVelocity() = 0;
virtual VecXf GetJointTorque() = 0;
virtual Vec3f GetImuRpy() = 0;
virtual Vec3f GetImuAcc() = 0;
virtual Vec3f GetImuOmega() = 0;
virtual VecXf GetContactForce() = 0;

数据发布接口

virtual void PublishJointData() = 0;
virtual void PublishImuData() = 0;

硬件接口

HardwareInterface 继承 RobotInterface 和 rclcpp::Node，是 lite3_transfer 的核心类。

初始化示例

auto hardware_node = std::make_shared<HardwareInterface>("lite3");
hardware_node->Start();   // 启动 UDP 接收，切到 SDK 模式

SDK 模式切换

virtual void Start(){
    receiver_->StartWork();
    sender_->ControlGet(2);  // SDK mode
}

virtual void Stop(){
    sender_->ControlGet(1);  // Robot mode
}

数据发布频率

默认以 200Hz 发布关节和 IMU 数据，可通过 /SDK_MODE 服务调整。

UDP 命令发送频率

关节命令通过 1000Hz 定时器发送到机器人固件。

手柄发布接口

手柄实现抽象基类

GamepadInterface 继承 rclcpp::Node，是所有手柄实现的抽象基类。

纯虚接口：

virtual void Start() = 0;
virtual void Stop() = 0;
virtual drdds::msg::GamepadData GetGamepadData() = 0;

基类提供 PublishGamepadData() 默认实现，以 200Hz 循环调用 GetGamepadData() 并发布到 /GAMEPAD_DATA。

手柄数据接收

RetroidGamepadPublisher 通过 UDP（默认端口 12121）接收手柄数据。

class RetroidGamepadPublisher : public GamepadInterface {
private:
    std::shared_ptr<RetroidGamepad> gamepad_ptr_;  // UDP 接收器
    int port_;                                     // UDP 端口
};

数据转换：

drdds::msg::GamepadData GetGamepadData() override {
    RetroidKeys rt_keys_ = gamepad_ptr_->GetKeys();
    drdds::msg::GamepadData msg;
    msg.left_axis_x  = rt_keys_.left_axis_x;
    msg.left_axis_y  = rt_keys_.left_axis_y;
    msg.right_axis_x = rt_keys_.right_axis_x;
    msg.right_axis_y = rt_keys_.right_axis_y;
    msg.buttons      = rt_keys_.value;   // 按键位掩码
    return msg;
}

多线程执行器

lite3_transfer 使用 MultiThreadedExecutor 同时运行硬件接口和手柄发布者：

rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(hardware_node);
executor.add_node(gamepad_pub);
executor.spin();

急停链路

急停通过三层链路实现：

UDP 命令层：lite3_sdk_service 收到 estop 命令，调用 /EMERGENCY_STOP 服务
硬件层：lite3_transfer 向机器人固件发送急停命令码 0x21010C0E
状态机层：通过 /EMERGENCY_STOP_SIGNAL 跨进程通知 Lite3_sdk_deploy 切换到 JointDamping