一、概述

fd_api 提供机器人外设（升降、头部、腰部、手部）的 Python 便捷接口封装。基于 ROS 2 rclpy 运行时， 通过简洁的 Python API 实现控制指令下发与状态读取 / 订阅。

核心模块

架构分层

二、安装与导入

编译构建

colcon build --packages-select fd_api

导入语句

from fd_api import State, Control, HeadType, HandType, HandBackend
from fd_api.waist_angle import waist_encoder_to_deg, waist_deg_to_encoder

三、Python 类型定义

3.1 枚举类型

3.2 腰部角度编码器转换

四、Control 控制接口

Control 类提供机器人外设的指令下发接口， 包含初始化、升降控制、头部控制、腰部控制、手部控制。

4.1 初始化

# === 初始化示例 ===
from fd_api import Control, State, HeadType, HandType, HandBackend

ctrl = Control()
ctrl.init({
    "node_name": "my_control",
    "hand_backend": HandBackend.kInspire,
    "hand_type": HandType.kBoth,
    "waist_servo_id": 3,
    "inspire_gripper_id": 1,
})
print("Control 初始化完成")

4.2 升降控制

# === 升降速度控制示例 ===
# 上升：速度 0.05 m/s
ctrl.lift_up(0.05)
print("升降上升中...")

# 下降：速度 -0.05 m/s
ctrl.lift_down(0.05)
print("升降下降中...")

# 停止
ctrl.lift_stop()
print("升降已停止")

# === 升降位置控制示例 ===
ctrl.lift_pos(0.05, 200.0)  # 速度 0.05m/s, 目标位置 200mm
print("升降移动至 200mm 位置")

4.3 头部控制

# === 头部控制示例 ===
# 下头（yaw）转到 30 度
ctrl.head_pos(HeadType.kLower, 30.0)
print("下头转到 30°")

# 上头（pitch）转到 10 度
ctrl.head_pos(HeadType.kUpper, 10.0)
print("上头转到 10°")

# 停止下头
ctrl.head_stop(HeadType.kLower)
print("下头已停止")

4.4 腰部控制

# === 腰部控制示例 ===
# 速度模式：前倾
ctrl.waist_front(720, 100, 100)
print("腰部前倾中...")

# 速度模式：后仰
ctrl.waist_back(720, 100, 100)
print("腰部后仰中...")

# 位置模式：转到 15 度
ctrl.waist_pos(720, 100, 100, 15.0)
print("腰部转至 15°")

# 停止
ctrl.waist_stop()
print("腰部已停止")

4.5 手部控制

# === 手部控制示例 ===
# RoHand 手部：传入 6 个关节值
ctrl.hand_move(HandType.kLeft, [0.0, 0.5, 0.3, 0.1, 0.0, 0.0])
print("左手动作已发送")

# Inspire 夹爪：取首值控制开合
ctrl.hand_move(HandType.kRight, [500.0])
print("右手夹爪已控制")

# 停止左手
ctrl.hand_stop(HandType.kLeft)
print("左手已停止")

五、State 状态接口

State 类提供机器人外设状态的读取与订阅功能。 支持同步缓存读取与异步回调订阅两种模式。

5.1 初始化

# === State 初始化示例 ===
state = State()
state.init({
    "node_name": "my_state",
    "callback_thread_count": 2,
    "waist_servo_id": 3,
    "inspire_gripper_id": 1,
})
print("State 初始化完成")

5.2 同步读取接口

# === 同步读取状态示例 ===
lift_pos = state.lift_get_current_pos()
print(f"当前升降位置: {lift_pos:.1f} mm")

head_lower = state.head_get_current_pos(HeadType.kLower)
print(f"下头角度: {head_lower:.1f} deg")

head_upper = state.head_get_current_pos(HeadType.kUpper)
print(f"上头角度: {head_upper:.1f} deg")

waist_pos = state.waist_get_current_pos()
print(f"腰部角度: {waist_pos:.1f} deg")

hand_pos = state.hand_get_current_pos(HandType.kLeft)
print(f"左手位置: {hand_pos}")

5.3 异步订阅接口

# === 异步订阅状态示例 ===
def on_lift_pos(mm):
    print(f"升降位置更新: {mm:.1f} mm")

sub_id = state.lift_sub_current_pos(on_lift_pos)
print(f"已订阅升降位置, sub_id={sub_id}")

# 取消订阅
state.unsubscribe(sub_id)
print("已取消订阅")

5.4 取消订阅

5.5 完整使用示例

# === 完整示例：初始化 → 控制 → 读取 → 订阅 ===
import time
from fd_api import Control, State, HeadType, HandType, HandBackend

def main():
    # 1. 初始化 Control
    ctrl = Control()
    ctrl.init({
        "node_name": "demo_control",
        "hand_backend": HandBackend.kInspire,
        "hand_type": HandType.kBoth,
        "waist_servo_id": 3,
        "inspire_gripper_id": 1,
    })

    # 2. 初始化 State
    state = State()
    state.init({
        "node_name": "demo_state",
        "callback_thread_count": 2,
        "waist_servo_id": 3,
        "inspire_gripper_id": 1,
    })

    # 3. 控制示例
    ctrl.lift_up(0.05)       # 升降上升
    time.sleep(1)
    ctrl.lift_stop()         # 停止升降

    ctrl.head_pos(HeadType.kLower, 30.0)   # 下头转 30°
    ctrl.waist_pos(720, 100, 100, 10.0)    # 腰部转 10°
    ctrl.hand_move(HandType.kLeft, [0.0, 0.5, 0.3])  # 左手动作

    # 4. 状态读取
    lift_mm = state.lift_get_current_pos()
    print(f"升降位置: {lift_mm:.1f} mm")

    head_deg = state.head_get_current_pos(HeadType.kLower)
    print(f"下头角度: {head_deg:.1f} deg")

    # 5. 异步订阅
    def on_lift(mm):
        print(f"升降位置更新: {mm:.1f} mm")

    sub_id = state.lift_sub_current_pos(on_lift)
    time.sleep(0.5)
    state.unsubscribe(sub_id)

    print("Demo 运行完毕")

if __name__ == "__main__":
    main()

六、测试工具

fd_api 包内含一个 Python 交互测试节点，可用于验证各接口功能：

# 编译
colcon build --packages-select fd_api

# 运行测试节点
ros2 run fd_api api_test_node_for_python_api.py

七、注意事项