【ROS2】一个用于控制斯图尔特（Stewart）平台的ROS2功能包

该 ROS2 项目实现了一个完整的六自由度斯图尔特（Stewart）平台的控制系统。系统的核心逻辑是将用户在图形界面（HMI）上设定的期望运动参数，通过一系列数学变换，最终转化为底层电机驱动器可以执行的 CAN 总线指令。

源码解析

canbus.h

此 canbus.h 文件是一个 C++ 头文件，它声明（但未实现）了一个名为 CANbus 的类。这个类的主要目的是封装 Linux 环境下基于 SocketCAN 的底层 CAN 总线通信功能。

核心功能和设计思想如下：

1. 封装性：它将复杂的、特定于 Linux 系统的 SocketCAN 编程细节（如创建套接字、绑定到 can0 等接口、配置 ioctl）隐藏在类的内部（私有成员）。这使得其他代码（例如 actuator_node.cpp）可以非常简单地使用 CAN 通信，而无需关心底层实现。

2. 接口定义：它提供了一个简洁的公共接口：

• 构造函数 CANbus() 用于初始化 CAN 连接。

• 析构函数 ~CANbus() 用于在对象销毁时安全地关闭连接。

• 核心方法 send_data() 用于向 CAN 总线发送控制指令。该方法接收上层应用（如运动学节点）计算出的各执行器（推杆）的目标行程和速度。

• 数据分帧：从私有成员变量 tx_len1, tx_len2, tx_vel1, tx_vel2 可以推断出，要发送的6个推杆的完整数据（长度和速度）无法装入一个标准的 8 字节 CAN 数据帧中。因此，该类内部会将这些数据分割（或打包）到多个 CAN 帧中进行发送。

• 总而言之，这个头文件是斯图尔特平台项目中硬件抽象层的一部分。它为上层控制节点（如 actuator_node）提供了一个标准化的、易于使用的接口，用于与底层的 CAN 总线硬件（电机驱动器等）进行通信，是连接软件控制逻辑和物理硬件的关键桥梁。

  canbus.cpp

  这个 C++ 文件实现了 CANbus 类的具体功能，它是一个专门用于与 Linux 系统下的 CAN 总线硬件进行底层交互的模块。

  其核心功能可以概括为以下几点：

  1. 初始化CAN接口 (CANbus() 构造函数)：

  • 创建一个标准的 Linux SocketCAN 套接字。

  • 通过 ioctl 将该套接字与物理 CAN 接口（如 can0）进行绑定。

  • 预先设置好将要发送的四种 CAN 帧的 CAN ID 和数据长度等固定信息。

• 数据编码与打包 (send_data() 方法)：

• • 定点化

    ：接收上层传入的以米和米/秒为单位的浮点数（float）行程和速度，并将它们乘以一个很大的系数，转换成无符号16位整数（uint16_t），这是一种定点化处理，便于在低带宽的 CAN 总线上传输。

  • 符号处理

    ：对于速度值，通过一个位掩码 sign 单独记录其正负号。

  • 序列化/打包

    ：将12个16位整数（6个长度，6个速度）和符号信息，按照预定义的协议，拆分成字节，并打包进4个独立的 CAN 帧 (tx_len1, tx_len2, tx_vel1, tx_vel2) 的数据区。

• 数据发送：调用 write() 系统调用，将这四个打包好的 CAN 帧通过已初始化的套接字发送到 CAN 总线上。

• 接收反馈（被注释）：代码中包含一段被注释掉的逻辑，其设计意图是接收来自执行器的反馈报文（CAN ID 为 0xF0 和 0xF1），解析出实际的执行器位置，并将其作为函数返回值。目前这部分功能未启用。

• 总而言之，这个类是典型的通信协议驱动。它将高层的、抽象的物理量（如长度、速度）严格按照特定硬件的通信协议要求，编码成底层的、具体的字节流（CAN 帧），并负责发送和（预留了）接收这些字节流，是连接软件算法和物理硬件的最终执行环节。

  hmi_node1.h

  此 hmi_node.h 文件定义了一个名为 HmiNode 的 C++ 类，这个类是整个斯图尔特平台项目中人机交互的核心。它巧妙地将 Qt 图形界面框架 和 ROS2 机器人操作系统框架 融合在了一起。

  其主要功能和设计目标如下：

  1. 双重身份：通过继承 QObject 和 rclcpp::Node，HmiNode 的实例既是一个能与 QML 界面交互的 Qt 对象，又是一个能与其他 ROS2 节点通信的 ROS2 节点。这是实现 Qt+ROS2 集成的关键技术。

  2. 接收用户输入：它定义了一系列 Qt 槽函数（如 set_surge_A）。这些函数旨在从 QML 图形界面接收用户输入，例如用户通过拖动滑块来设置运动的振幅和频率。

  3. 生成运动指令：该类内部维护了一套参数（振幅、偏置、相位），用于数学上生成六个自由度（6-DoF）的平滑、连续的正弦/余弦运动轨迹。

  4. 定时发布：它使用一个 ROS2 定时器 (timer_) 来周期性地执行 timer_callback 函数。在这个回调函数里，它会：

  • 根据当前时间 t 和用户设定的参数计算出平台在这一时刻的期望位姿（位置和姿态）。

  • 将计算出的位姿数据打包成一个自定义的 ROS2 消息 (TransformsPosVel)。

  • 通过 ROS2 发布者 (transform_publisher) 将这个消息发布出去。

• 系统中的角色：在整个系统中，HmiNode 扮演着指令源的角色。它将用户的抽象操作（“让平台以 0.5Hz 的频率、10度的幅度摇摆”）转化为具体的、连续的数学指令，并广播给系统中的其他节点（如 kinematics_node）去执行后续的计算和控制。

• hmi_node1.cpp

  这个 C++ 文件实现了 HmiNode 类，它是整个斯图尔特平台控制系统的指令源和轨迹生成器。它完美地融合了 Qt 和 ROS2 的功能，将用户的界面操作转化为机器人可以执行的连续运动指令。

  其核心功能如下：

  1. 接收用户输入：通过一系列 Qt 槽函数（set_... 方法），它从 QML 图形界面接收用户通过滑块设定的运动参数（如频率和振幅）。它还将 GUI 传来的归一化值（如 0.0 到 1.0）缩放为有意义的物理单位（如赫兹、米、度）。

  2. 高频轨迹生成：节点内有一个高频（100 Hz）的 ROS2 定时器。每次触发时：

  • timer_callback

     函数被调用。

  • 它调用 sample_sine 函数，根据用户设定的参数和当前时间 t，利用正弦和余弦函数计算出平台在这一精确时刻期望的6自由度位姿（位置+姿态）和速度旋量（线速度+角速度）。

• 发布运动指令：计算出的位姿和速度被打包成一个自定义的 TransformsPosVel ROS2 消息。

• 广播指令：最后，该消息通过 ROS2 发布者发布到 /dof_ref 主题上，供下游的 kinematics_node 订阅和处理。

• 总而言之，HmiNode 扮演着一个数字信号发生器的角色。它将用户的静态、离散设置，实时地、连续地转换为平滑的运动轨迹，是连接人机交互与机器人运动控制的第一个关键环节。

  qt_executor.h

  此 qt_executor.h 文件定义了一个名为 QtExecutor 的自定义 ROS2 执行器。它的核心目标是解决 Qt 事件循环与 ROS2 事件循环之间的冲突，实现二者的无缝集成。

  问题背景：

  • 一个典型的 Qt 程序需要调用 app.exec() 来启动其事件循环，处理 GUI 事件（如按钮点击、窗口重绘）。这是一个阻塞调用。

  • 一个典型的 ROS2 节点需要调用 rclcpp::spin(node) 来启动其事件循环，处理 ROS2 事件（如接收消息、执行定时器回调）。这也是一个阻塞调用。

  • 在同一个线程中，你无法同时运行这两个阻塞的循环。如果先运行 app.exec()，ROS2 的回调就不会执行；如果先运行 rclcpp::spin()，GUI 就会冻结。

  QtExecutor 的解决方案：

  这个类通过精巧的线程与信号-槽机制解决了上述问题：

  1. 双重继承：QtExecutor 同时继承自 QObject 和 rclcpp::Executor，这使它既能使用 Qt 的信号-槽，又能作为 ROS2 的执行器。它是一个完美的**“桥梁”**。

  2. 后台线程 (m_thread)：start() 方法会创建一个后台线程。这个线程专门负责等待 ROS2 的事件。它会调用 ROS2 执行器的底层函数来检查是否有新的工作（如消息到达），这个等待过程是阻塞的，但因为它在后台线程，所以不会冻结主线程的 GUI。

  3. 信号-槽通信 (onNewWork 和 processWork)：

  • 当后台线程检测到有新的 ROS2 工作时，它不会立即执行这个工作。相反，它会发射一个 Qt 信号 onNewWork()。

  • 这个信号被连接到 processWork() 这个槽函数上。

  • 由于 Qt 信号-槽的跨线程机制，processWork() 会被安全地调度到**主线程（GUI 线程）**中执行。

• 在主线程执行工作：processWork() 函数在主线程中被调用后，它会调用 ROS2 执行器的函数来执行一个或多个当前待处理的回调。

• 最终效果：
  Qt 的事件循环 (app.exec()) 在主线程中顺畅运行，保证了 GUI 的响应性。同时，QtExecutor 在后台监视 ROS2 事件，并通过信号-槽机制将 ROS2 回调的执行任务“注入”到 Qt 的事件循环中。这样，ROS2 的通信和 Qt 的 GUI 就在一个应用程序中和谐共存，协同工作。

  总而言之，QtExecutor 是一个高级且健壮的解决方案，是构建复杂的 ROS2-Qt 应用程序的基石。

  qt_executor.cpp

  这个 qt_executor.cpp 文件实现了 QtExecutor 类的逻辑，它是连接 Qt 事件循环和 ROS2 事件循环的桥梁。其工作流程可以概括为以下几个步骤：

  1. 启动 (start)：当 start() 方法被调用时，它会创建一个后台线程。

  2. 后台等待 (wait_for_work)：这个后台线程进入一个循环，其核心是调用阻塞函数 wait_for_work()。这个线程会在此“休眠”，直到有 ROS2 事件发生（如新消息到达），从而高效地等待工作，不空耗 CPU。

  3. 通知主线程 (onNewWork 信号)：一旦 wait_for_work() 返回（表示有工作了），后台线程就会发射 onNewWork() 这个 Qt 信号。

  4. 在主线程执行 (processWork 槽)：

  • 由于在构造函数中设置了 BlockingQueuedConnection，onNewWork() 信号会触发 processWork() 槽函数在**主线程（GUI 线程）**中执行。

  • processWork()

     函数内部调用 spin_some()，这个函数会处理当前所有待处理的 ROS2 回调。因为是在主线程中执行，所以这些回调可以安全地更新 GUI 界面，避免了多线程访问 GUI 的复杂性和风险。

• 同步与关闭：

• • BlockingQueuedConnection

     确保了后台线程会等待主线程处理完当前的 ROS2 任务后，才会回去继续等待下一批任务，这保证了任务处理的顺序性。

  • 当 ROS2 关闭时（例如用户按下了 Ctrl+C），后台线程的循环会结束，并向 Qt 的主事件循环发送一个“退出”请求，从而优雅地关闭整个应用程序。析构函数中的 m_thread.join() 则确保了在程序最终退出前，后台线程已经完全停止。

  总而言之，QtExecutor 通过“后台等待，主线程处理”的模式，完美地将 ROS2 的异步事件处理逻辑集成到了 Qt 的单线程事件模型中，是实现功能强大、响应迅速的 ROS2-Qt 融合应用程序的典范实现。

  inverse_kinematics.py

  该 InverseKinematics 类封装了 Stewart 平台的逆运动学计算逻辑：

  1. 在初始化中定义了底座和平台板上六个连杆连接点的固定坐标，以及平台最小升高 z_min；

  2. calc_output

      方法接受六自由度的位姿 pose（3 轴平移 + 3 轴旋转）和对应的速度向量 twist，

  • 先构造总旋转矩阵 R 并计算平台中心在全局坐标下的位置 p；

  • 求出各连杆向量 s_i 及其长度 l_i，再减去初始长度得到杆长偏移 d_i；

  • 调用私有方法 __jacobian 计算配置相关的雅可比矩阵 J，

  • 最终将平台空间速度 twist 投影到杆长速度空间，得到 d_dot；

• 返回值包含杆长偏移 d 和杆长速度 d_dot，上层控制器可据此驱动执行器实现目标运动。

• 

  

  kinematics_node.py

  这个 Python 脚本实现了一个名为 controller 的 ROS2 节点，它在斯图尔特平台控制系统中扮演着核心数学转换器的角色。

  其主要功能可以概括为以下几点：

  1. 接收高级指令：节点通过订阅 /dof_ref 主题，来接收上层节点（如 hmi_node）发布的平台期望运动指令。这些指令是高级的、描述平台整体运动的位姿（Pose）和速度旋量（Twist）。

  2. 执行逆运动学计算：每当接收到一条新指令，节点就会调用 InverseKinematics 类的 calc_output 方法。这个方法是整个系统的数学核心，它执行逆运动学解算，将平台的整体位姿和速度翻译成六根独立的执行器（连杆）所需要达到的具体行程长度和行程速度。

  3. 发布底层指令：计算完成后，节点将得到的六组长度和速度数据打包成一个 LegsLenVel 类型的消息，然后通过发布到 /leg_ref 主题上，将这些底层、具体的控制目标发送给下游节点（如 actuator_node）。

  总而言之，该节点是连接“平台想做什么”（抽象指令）和“每条腿该怎么动”（具体指令）之间的关键桥梁。它将一个复杂的、耦合的六自由度运动问题，分解为六个独立的、一维的直线运动控制问题，为最终的硬件控制奠定了基础。

  actuator_node.cpp

  这个 C++ 文件实现了 actuator_node，它是整个斯图尔特平台控制系统中最底层的软件节点，扮演着连接 ROS2 世界与物理硬件的桥梁角色。

  其核心功能可以概括为以下三点：

  1. 接收控制指令：该节点通过订阅 /leg_ref 主题，来接收由 kinematics_node 计算得出的、针对六个执行器的具体目标行程长度和速度。

  2. 驱动硬件执行：

  • 在每次收到指令后，回调函数 actuate_callback 会被触发。

  • 它将消息中的数据解包，然后调用一个 CANbus 类的 send_data 方法。

  • CANbus

     类封装了所有与 CAN 总线通信的复杂细节，该方法会将指令通过 CAN 总线发送给平台的电机驱动器，从而命令物理执行器运动。

• 发布反馈数据（闭环控制）：

• • send_data

     方法不仅发送指令，还会从硬件接收反馈数据（很可能是执行器的实际位置编码器读数）并返回。

  • 节点随后将这些真实的反馈数据打包成 LegsLen 消息，并发布到 /leg_feedback 主题上。

  • 这形成了一个闭环控制的关键环节，使得系统的其他部分（或者监控人员）能够知道平台的实际状态，而不仅仅是期望状态。

  总而言之，actuator_node 是一个典型的硬件抽象层（Hardware Abstraction Layer, HAL）节点。它将上层软件（如运动学解算）与具体的硬件通信协议（CAN总线）解耦，使得上层逻辑无需关心底层硬件的具体实现细节。

  main.cpp

  这个 main.cpp 文件是整个应用程序的启动器和协调器，其核心任务是无缝地集成 Qt 图形界面和 ROS2 节点系统，让它们在一个程序中和谐共存。

  其工作流程可以概括为以下几个关键步骤：

  1. 初始化：它首先分别初始化 Qt 应用程序 (QGuiApplication) 和 ROS2 系统 (rclcpp::init)。

  2. 创建核心对象：它创建了一个 HmiNode 的实例。这个对象非常特殊，因为它同时继承了 QObject 和 rclcpp::Node，是连接两个框架的数据和逻辑中心。

  3. 桥接 C++ 与 QML：通过 setContextProperty("oscillator", ...)，它将 C++ 中的 HmiNode 对象实例暴露给了 QML 界面。这使得 QML 中的界面元素（如滑块）可以直接调用 C++ 对象中的方法（槽函数），从而将用户的操作传递给后端逻辑。

  4. 加载 GUI：它创建并加载了主 QML 文件 (main.qml)，从而渲染出用户看到的图形界面。

  5. 启动双循环（核心技巧）：这是整个程序最精妙的部分：

  • 它没有使用标准的、会阻塞主线程的 rclcpp::spin()。

  • 而是使用了自定义的 QtExecutor。executor.start() 在一个后台线程中启动了对 ROS2 事件的等待。

  • 然后，在主线程中，它调用 app.exec() 启动了 Qt 的事件循环，这个循环负责 GUI 的响应。

  • 当后台线程检测到 ROS2 事件（如定时器触发）时，它通过 Qt 的信号-槽机制，将 ROS2 回调函数的执行任务，安全地调度到主线程的事件循环中。

  通过这种方式，main.cpp 成功地解决了“两个阻塞循环无法在同一线程运行”的经典问题，实现了 GUI 响应的流畅性和 ROS2 通信的实时性，是构建复杂 ROS2-Qt 应用程序的典范。

  stewart.launch.py

  这个 Python 脚本是一个标准的 ROS2 启动文件（Launch File）。它的核心功能是自动化地、同时地启动一个机器人系统所需的多个节点。

  具体来说，该脚本完成了以下工作：

  1. 定义了三个节点：

  • actuator_node

    ：一个 C++ 节点，很可能负责与斯图尔特平台（Stewart Platform）的物理执行器（如电机、舵机）进行通信和控制。

  • kinematics_node.py

    ：一个 Python 节点，负责处理平台的运动学计算，例如根据期望的平台姿态计算出每个执行器需要伸缩的长度（逆运动学）。

  • hmi_node

    ：一个 C++ 节点，提供人机交互界面（Human-Machine Interface）。根据文件树中的 .qml 文件推断，这很可能是一个基于 Qt/QML 的图形用户界面（GUI），用户可以通过它来发送控制指令或监控平台状态。

• 整合启动流程：

• • 脚本将这三个节点的启动配置添加到一个 LaunchDescription 对象中。

  通过使用这个启动文件，开发者无需在三个不同的终端中分别手动运行 ros2 run 命令来启动每个节点。只需执行一条命令 ros2 launch stewart_platform_pkg stewart.launch.py，即可方便地将整个斯图尔特平台控制系统的软件部分全部运行起来，极大地简化了系统的启动和部署过程。

  CMakeLists.txt

  这个 CMakeLists.txt 文件是整个 stewart_platform_pkg 功能包的构建总纲。它详细地描述了如何将所有的源代码和资源文件编译、链接并组织成一个可以被 ROS2 系统正确识别和运行的软件包。

  其核心功能可以总结为以下几点：

  1. 依赖管理：它使用 find_package 命令，清晰地声明了项目的所有依赖，包括 ROS2 的核心库（rclcpp, rclpy）、Qt5 的图形界面库（Qt5Core, Qt5Quick）以及项目自定义的消息包（stewart_platform_interfaces）。

  2. C++/Qt 集成编译：通过设置 CMAKE_AUTO... 变量，它实现了与 Qt 的无缝集成，能够自动处理 MOC、UIC 和 RCC，大大简化了编译包含 Qt 信号槽、.ui.qml 文件和 .qrc 资源文件的复杂过程。

  3. 目标文件定义：它定义了如何生成项目中的所有可执行文件：

  • 创建了两个 C++ 可执行文件：actuator_node 和 hmi_node，并为它们分别指定了源文件和链接库。

  • 指明了 kinematics_node.py 是一个 Python 可执行脚本。

  • 指明了 stewart_platform_pkg 目录下的 Python 文件是一个可导入的模块。

• 安装部署：通过一系列 install 命令，它精确地定义了在构建完成后，如何将生成的可执行文件、Python 脚本、库文件以及 launch 文件等，分别放置到 install 目录下的标准位置。这保证了在使用 colcon 构建工作空间后，可以通过 ros2 run 和 ros2 launch 等命令正确地启动和运行包内的所有节点和功能。

• 总而言之，这个文件是连接源代码与最终可用软件产品的蓝图，是任何基于 CMake 的 ROS2 项目的基石。

  package.xml

  这个 package.xml 文件是 stewart_platform_pkg 功能包的“身份证”和“说明书”。它向 ROS2 生态系统（特别是构建和运行工具）提供了关于这个包的所有关键信息。

  其核心功能可以归纳为以下三点：

  1. 身份标识：定义了包的基本信息，如名称 (stewart_platform_pkg)、版本、描述、维护者和许可证。这使得包可以被人类和机器识别和管理。

  2. 依赖管理：这是此文件最重要的功能。它详细列出了该功能包在不同阶段（编译、运行、测试）所依赖的其他软件包和系统库。例如：

  • 为了编译 C++ 节点，它需要 rclcpp 和 qtbase5-dev。

  • 为了运行 Python 节点和启动文件，它需要 rclpy。

  • 为了与自定义消息交互，它需要 stewart_platform_interfaces。

  • 为了让 HMI 界面正常显示，它需要 libqt5-core 和相关的 QML 模块。

  • 为了与硬件通信，它可能需要系统的 can-utils 工具。
    构建系统 colcon 会根据这些依赖声明，确保所有必需的包都已构建，rosdep 等工具可以用来安装缺失的系统依赖。

• 构建类型声明：通过 <export> 标签，它明确告诉 ROS2 构建系统：“我是一个使用 ament_cmake 流程来构建的包”。这指导构建工具正确地调用 CMake 来编译源代码、安装可执行文件、库和脚本等。

• 总而言之，package.xml 是 ROS2 功能包的基石，它使得功能包能够被模块化、可移植地构建和分发，是实现“一次编写，到处构建”的关键所在。