🤖 roboto_origin_03 Wiki
首页 / IMU 子模块 / C++ SDK 快速上手

本文档面向具备 C++11/14 基础、希望将 roboto_imu 集成到自有应用中的开发者。阅读完本页后,你将掌握如何通过工厂方法创建 IMU 实例、在 CAN 与串口两种接口下读取传感器数据,以及如何将库链接到自己的 CMake 工程。全部示例均基于当前支持的 HiPNUC(超核电子) 系列设备。

前置依赖

在使用 C++ SDK 之前,请确保系统已安装以下依赖。该库基于 C++17 标准构建,核心外部依赖为 spdlogfmtpybind11;若处于 ROS 2 环境,则通过 ament_cmake 进行构建。

依赖 最低版本建议 用途
CMake 3.12+ 构建系统
C++ Compiler GCC/Clang (C++17) 编译
spdlog 结构化日志
fmt 格式化字符串
pybind11 Python 绑定(不影响纯 C++ 使用)
ament_cmake ROS 2 工作空间构建(可选)

如果你尚未编译本库,可参考 环境依赖与安装 完成环境准备;ROS 2 用户可直接在工作空间执行 colcon build --packages-select roboto_imu

Sources: CMakeLists.txt, package.xml

设计概览:工厂模式与统一接口

C++ SDK 采用工厂模式对外暴露唯一的入口点 IMUDriver::create_imu()。该静态方法返回 std::shared_ptr<IMUDriver>,内部根据 imu_type 参数实例化对应的派生驱动(当前仅 HipnucIMUDriver)。这种设计将上层应用与底层协议(J1939、CANopen、HiPNUC 私有串口协议)完全解耦,使你无需关心 CAN 帧解析或串口字节流重组的细节。

下图展示了从应用到硬件的数据流:应用层通过统一基类调用 get_quat() 等方法,底层则由独立的接收线程通过 SocketCAN 或 UART 持续解析数据,并通过读写锁(std::shared_mutex)保证并发安全。

flowchart LR
    A[应用层<br/>IMUDriver] -->|get_quat / get_ang_vel| B[HipnucIMUDriver]
    B --> C{接口类型}
    C -->|can| D[SocketCAN 单例<br/>实时接收线程]
    C -->|serial| E[UART 接收线程]
    D --> F[CAN 帧解析<br/>J1939 / CANopen]
    E --> G[私有协议解码<br/>HI91]
    F --> H[传感器数据缓冲区<br/>shared_mutex 保护]
    G --> H
    H --> B

Sources: include/imu_driver.hpp, src/imu_driver.cpp, src/drivers/hipnuc/hipnuc_imu_driver.cpp

快速开始:最小可运行示例

只需包含头文件 imu_driver.hpp,并调用工厂方法即可创建实例。构造函数内部会立即打开指定接口、启动后台接收线程;对象析构时则自动关闭接口并清理回调。

CAN 接口(J1939)

CAN 场景下,interface 参数为 Linux SocketCAN 网络接口名(如 can0can1),imu_id 对应设备的节点地址,baudrate0 即可(CAN 波特率由系统 ip link 配置决定)。

#include "imu_driver.hpp"
#include <iostream>

int main() {
    // 创建 IMU 实例:imu_id=0x08, 接口 can0
    auto imu = IMUDriver::create_imu(
        0x08,           // uint16_t imu_id
        "can",          // const std::string& interface_type
        "can0",         // const std::string& interface
        "HIPNUC",       // const std::string& imu_type
        0               // int baudrate(CAN 场景忽略)
    );

    // 读取传感器数据
    auto quat   = imu->get_quat();         // {w, x, y, z}
    auto gyro   = imu->get_ang_vel();      // {x, y, z} rad/s
    auto accel  = imu->get_lin_acc();      // {x, y, z} m/s²
    float temp  = imu->get_temperature();  // °C

    std::cout << "Quaternion: ["
              << quat[0] << ", " << quat[1] << ", "
              << quat[2] << ", " << quat[3] << "]\n";

    return 0;
} // 作用域结束时自动析构,关闭 CAN 回调与 socket

串口接口(UART)

串口场景下,interface 为设备文件路径(如 /dev/ttyUSB0),并必须通过 baudrate 指定波特率(常见值为 115200921600)。

#include "imu_driver.hpp"
#include <iostream>

int main() {
    auto imu = IMUDriver::create_imu(
        0x01,           // uint16_t imu_id
        "serial",       // const std::string& interface_type
        "/dev/ttyUSB0", // const std::string& interface
        "HIPNUC",       // const std::string& imu_type
        115200          // int baudrate
    );

    auto gyro = imu->get_ang_vel();
    std::cout << "Gyro: [" << gyro[0] << ", " << gyro[1] << ", " << gyro[2] << "]\n";

    return 0;
}

Sources: include/imu_driver.hpp, src/drivers/hipnuc/hipnuc_imu_driver.cpp

数据读取 API 参考

所有数据访问方法均为线程安全实现:底层接收线程在写入传感器数据时持有写锁,应用层读取时持有读锁。返回的 std::vector<float> 均为值拷贝,因此你可以在多线程环境中安全地传递和使用。

方法 返回值 含义 单位
get_imu_id() uint16_t 设备节点 ID
get_quat() std::vector<float>(4 元素) 姿态四元数 [w, x, y, z]
get_ang_vel() std::vector<float>(3 元素) 角速度 [x, y, z] rad/s
get_lin_acc() std::vector<float>(3 元素) 线加速度 [x, y, z] m/s²
get_temperature() float 设备温度 °C

值得注意的是,底层驱动在接收到原始数据后已完成单位换算:加速度从 g 转换为 m/s²,角速度从 °/s 转换为 rad/s。因此你拿到的是可直接用于物理计算或控制算法的标准 SI 单位。

Sources: include/imu_driver.hpp, src/drivers/hipnuc/hipnuc_imu_driver.cpp

构建与 CMake 集成

编译安装完成后,库文件与头文件将按标准目录结构部署。根据你的项目类型,可选择以下两种集成方式。

方式一:ROS 2 工作空间(推荐)

若你在 ROS 2 环境中开发,在节点的 CMakeLists.txt 中直接依赖本包即可:

find_package(ament_cmake REQUIRED)
find_package(roboto_imu REQUIRED)

add_executable(my_node src/my_node.cpp)
ament_target_dependencies(my_node roboto_imu)

方式二:非 ROS 的纯 CMake 项目

库安装后会附带 roboto_imuConfig.cmake,支持 find_package 导入接口目标 roboto_imu::roboto_imu

find_package(roboto_imu REQUIRED)

add_executable(my_app src/main.cpp)
target_link_libraries(my_app PRIVATE roboto_imu::roboto_imu)

此方式会自动处理头目录、静态库(imuhipnuc_imuimu_protocol)以及 fmtspdlog 的链接依赖。

Sources: CMakeLists.txt, cmake/roboto_imuConfig.cmake

C++ SDK 与 Python SDK 对比

同一套底层实现通过 pybind11 暴露为 Python 模块 imu_py,两种语言的调用语义高度一致。主要差异仅体现在语法与类型系统上:

维度 C++ SDK Python SDK
模块/头文件 #include "imu_driver.hpp" from imu_py import IMUDriver
工厂方法 IMUDriver::create_imu(...) IMUDriver.create_imu(...)
调用方式 imu->get_quat() imu.get_quat()
返回容器 std::vector<float> list[float]
生命周期 shared_ptr 自动管理 Python GC 自动管理
性能 零拷贝开销最小 存在 pybind 类型转换开销

如果你希望先以脚本方式验证硬件连接,再迁移到 C++ 部署,可阅读 Python SDK 快速上手 作为对照。

Sources: src/pybind_module.cpp, include/imu_driver.hpp

接口配置速查

在运行上述示例前,请确保操作系统层面的接口已经就绪。以下是两种接口的最小准备命令:

接口类型 检查/配置命令 说明
CAN ip link set can0 up type can bitrate 500000 需 root 或使用 sudo
Serial ls /dev/ttyUSB* 确认设备节点存在且有读写权限

关于 CAN 波特率修改、串口权限持久化等进阶配置,请参阅 CAN 接口配置指南串口接口配置指南

下一步

完成本页的快速上手后,建议按以下顺序深入理解内部机制:

  1. 工厂模式与抽象驱动设计 — 了解 IMUDriver 基类如何屏蔽底层差异,以及新增设备型号时的扩展方式。
  2. 传感器数据访问与线程安全 — 深入 std::shared_mutex 的读写锁策略与实时性考量。
  3. SocketCAN 单例与实时接收线程 — 探究 CAN 数据如何从内核 SocketCAN 缓冲区流转到应用层回调。
  4. 串口通信与 UART 接收线程 — 理解串口私有协议的帧边界识别与解码流程。