一、概述

fd_api 提供机器人外设（升降、头部、腰部、手部）的 C++ 共享库 接口封装。 采用 PIMPL（Pointer to Implementation） 惯用法设计， 对外头文件不包含任何 ROS 头文件，使用者无需依赖 ROS 2 构建环境即可编译引用。

接口按职责划分为 Control（控制） 与 State（状态） 两大模块， 所有类型定义位于 namespace fd_robot 命名空间下。

二、类型定义

所有枚举和回调类型定义在 namespace fd_robot 下。

2.1 枚举类型

2.2 回调类型

三、Options 配置结构体

3.1 Control::Options

用于 Control::Init() 的初始化配置参数。

3.2 State::Options

用于 State::Init() 的初始化配置参数。

四、Control 控制接口

fd_robot::Control 提供机器人外设的运动控制接口，所有方法为阻塞式调用。

4.1 初始化

使用示例

#include "fd_api/control.h"
#include "fd_api/state.h"
#include "fd_api/types.h"

// Control 初始化
fd_robot::Control ctrl;
fd_robot::Control::Options ctrl_opts;
ctrl_opts.node_name = "my_control";
ctrl_opts.hand_backend = fd_robot::HandBackend::kInspire;
ctrl_opts.hand_type = fd_robot::HandType::kBoth;
ctrl_opts.waist_servo_id = 3;
ctrl_opts.inspire_gripper_id = 1;
ctrl.Init(ctrl_opts);

4.2 升降

使用示例

// 升降速度控制
ctrl.lift_up(0.05f);   // 上升
ctrl.lift_down(0.05f); // 下降
ctrl.lift_stop();      // 停止

// 升降位置控制（速度 m/s, 位置 mm）
ctrl.lift_set_pos(0.05f, 200.0f);

4.3 头部

角度限幅范围

使用示例

// 头部角度控制
ctrl.head_set_pos(fd_robot::HeadType::kLower, 30.0f);  // 下头 30°
ctrl.head_set_pos(fd_robot::HeadType::kUpper, 10.0f);  // 上头 10°
ctrl.head_stop(fd_robot::HeadType::kLower);            // 停止下头

4.4 腰部

使用示例

// 腰部速度模式
ctrl.waist_front(720, 100, 100);  // 前倾
ctrl.waist_back(720, 100, 100);   // 后仰

// 腰部位置模式 (0°~35°)
ctrl.waist_set_pos(720, 100, 100, 15.0f);

ctrl.waist_stop();  // 停止

4.5 手部

使用示例

// RoHand 手部控制
std::vector<float> hand_vals = {0.0f, 0.5f, 0.3f, 0.1f, 0.0f, 0.0f};
ctrl.hand_move(fd_robot::HandType::kLeft, hand_vals);

// Inspire 夹爪
std::vector<float> gripper_val = {500.0f};
ctrl.hand_move(fd_robot::HandType::kRight, gripper_val);

ctrl.hand_stop(fd_robot::HandType::kLeft);

五、State 状态接口

fd_robot::State 提供机器人外设的状态读取与异步订阅接口。

5.1 初始化

使用示例

// State 初始化
fd_robot::State state;
fd_robot::State::Options state_opts;
state_opts.node_name = "my_state";
state_opts.callback_thread_count = 2;
state_opts.waist_servo_id = 3;
state_opts.inspire_gripper_id = 1;
state.Init(state_opts);

5.2 同步读取

同步读取接口从内部缓存获取最新数据，返回 bool 表示是否已成功收到过数据。

使用示例

// 同步读取状态
float lift_pos = 0.0f;
if (state.lift_get_current_pos(lift_pos)) {
    printf("当前升降位置: %.1f mm\n", lift_pos);
}

float head_lower = 0.0f;
state.head_get_current_pos(fd_robot::HeadType::kLower, head_lower);
printf("下头角度: %.1f deg\n", head_lower);

float waist_angle = 0.0f;
state.waist_get_current_pos(waist_angle);
printf("腰部角度: %.1f deg\n", waist_angle);

std::vector<float> hand_pos;
state.hand_get_current_pos(fd_robot::HandType::kLeft, hand_pos);

5.3 异步订阅

异步订阅接口注册回调函数，有新数据时由内部线程池异步调用。返回 SubscriptionId 用于取消订阅。

使用示例

// 异步订阅升降位置
fd_robot::SubscriptionId sid = state.lift_sub_current_pos(
    [](float mm) {
        printf("升降位置更新: %.1f mm\n", mm);
    });

// 取消订阅
state.unsubscribe(sid);

5.4 取消订阅

六、C++ 示例代码

以下示例展示 Control 和 State 的完整用法，包含所有外设的控制与状态读取操作：

#include <chrono>
#include <iostream>
#include <thread>
#include <vector>
#include "fd_api/control.h"
#include "fd_api/state.h"
#include "fd_api/types.h"

using namespace fd_robot;

int main() {
    // ── Control 初始化 ──
    Control::Options ctrl_opts;
    ctrl_opts.node_name = "my_control";
    ctrl_opts.hand_backend = HandBackend::kInspire;
    ctrl_opts.hand_type    = HandType::kBoth;
    ctrl_opts.waist_servo_id = 3;
    ctrl_opts.inspire_gripper_id = 1;

    Control ctrl;
    if (!ctrl.Init(ctrl_opts)) {
        std::cerr << "Control init failed" << std::endl;
        return -1;
    }

    // ── 升降控制 ──
    ctrl.lift_up(0.05f);            // 上升（速度模式）
    std::this_thread::sleep_for(std::chrono::milliseconds(300));
    ctrl.lift_stop();

    ctrl.lift_set_pos(0.05f, 200.0f);  // 升降移动到 200 mm（位置模式）

    // ── 头部控制 ──
    ctrl.head_set_pos(HeadType::kLower, 30.0f);  // 下头 30°
    ctrl.head_set_pos(HeadType::kUpper, 10.0f);  // 上头 10°

    // ── 腰部控制 ──
    ctrl.waist_front(720, 100, 100);    // 前倾（速度模式）
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
    ctrl.waist_stop();

    ctrl.waist_set_pos(720, 100, 100, 15.0f);  // 腰部到 15°（位置模式）

    // ── 手部控制 ──
    std::vector<float> hand_vals = {0.0f, 0.5f, 0.3f, 0.1f, 0.0f, 0.0f};
    ctrl.hand_move(HandType::kLeft, hand_vals);
    ctrl.hand_stop(HandType::kLeft);

    // ── State 初始化 ──
    State::Options state_opts;
    state_opts.node_name = "my_state";
    state_opts.callback_thread_count = 2;
    state_opts.waist_servo_id = 3;
    state_opts.inspire_gripper_id = 1;

    State state;
    if (!state.Init(state_opts)) {
        std::cerr << "State init failed" << std::endl;
        return -1;
    }

    // ── 同步读取 ──
    double pos = 0.0;
    if (state.lift_get_current_pos(pos)) {
        std::cout << "Current lift pos: " << pos << " mm" << std::endl;
    }

    double head_lower = 0.0;
    state.head_get_current_pos(HeadType::kLower, head_lower);
    std::cout << "下头角度: " << head_lower << " deg" << std::endl;

    double waist_angle = 0.0;
    state.waist_get_current_pos(waist_angle);
    std::cout << "腰部角度: " << waist_angle << " deg" << std::endl;

    // ── 异步订阅 ──
    auto sub_id = state.lift_sub_current_pos(
        [](double p) {
            std::cout << "Lift pos updated: " << p << " mm" << std::endl;
        });

    // ── 停止操作 ──
    ctrl.lift_stop();
    ctrl.head_stop(HeadType::kLower);
    ctrl.head_stop(HeadType::kUpper);
    ctrl.waist_stop();

    // ── 取消订阅 ──
    state.unsubscribe(sub_id);

    return 0;
}

七、测试工具

项目提供交互式测试节点，可用于验证接口功能：

# 编译
colcon build --packages-select fd_api

# 运行交互测试
ros2 run fd_api api_test_node

八、注意事项