从内核到驱动: ROS 2 ros2_control 实时硬件接口开发与 PREEMPT_RT 调优实践

sudo ubuntu-advantage attach <YOUR_PRO_TOKEN>
sudo pro enable realtime-kernel
sudo apt install linux-realtime
sudo reboot

uname -a
# 输出中包含 PREEMPT_RT 标志即为成功

sudo apt install rt-tests stress-ng

# 终端 1：跑满 CPU 和 IO
stress-ng --cpu 4 --io 2 --vm 1 --vm-bytes 1G --timeout 60s

# 终端 2：运行 cyclictest 测 10 分钟
sudo cyclictest -l100000 -m -n -a -t 4 -p 90 -h 100 -q

GRUB_CMDLINE_LINUX_DEFAULT="quiet splash isolcpus=2,3 nohz_full=2,3 rcu_nocbs=2,3 intel_idle.max_cstate=0 processor.max_cstate=1"

sudo update-grub
sudo reboot

@realtime soft rtprio 99
@realtime hard rtprio 99
@realtime soft memlock unlimited
@realtime hard memlock unlimited

sudo groupadd realtime
sudo usermod -aG realtime $USER

#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/state.hpp"

namespace my_robot_hardware
{

class RobotArmHardware : public hardware_interface::SystemInterface
{
public:
  hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override
  {
    if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) {
      return hardware_interface::CallbackReturn::ERROR;
    }

    // 初始化固定长度数组，避免运行时分配内存
    hw_commands_positions_.resize(info_.joints.size(), 0.0);
    hw_states_positions_.resize(info_.joints.size(), 0.0);
    hw_states_velocities_.resize(info_.joints.size(), 0.0);

    return hardware_interface::CallbackReturn::SUCCESS;
  }

  std::vector<hardware_interface::StateInterface> export_state_interfaces() override
  {
    std::vector<hardware_interface::StateInterface> state_interfaces;
    for (size_t i = 0; i < info_.joints.size(); ++i) {
      state_interfaces.emplace_back(
        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_positions_[i]);
      state_interfaces.emplace_back(
        info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocities_[i]);
    }
    return state_interfaces;
  }

  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override
  {
    std::vector<hardware_interface::CommandInterface> command_interfaces;
    for (size_t i = 0; i < info_.joints.size(); ++i) {
      command_interfaces.emplace_back(
        info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_positions_[i]);
    }
    return command_interfaces;
  }

  hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
  {
    // 关键路径：严禁分配内存、严禁 I/O 阻塞
    // 通过 EtherCAT / CAN 总线驱动读取当前电机编码器数据
    // 示例：bus_driver_->receive_processdata(hw_states_positions_.data(), hw_states_velocities_.data());
    
    return hardware_interface::return_type::OK;
  }

  hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override
  {
    // 关键路径：下发指令到总线
    // 示例：bus_driver_->send_processdata(hw_commands_positions_.data());

    return hardware_interface::return_type::OK;
  }

private:
  std::vector<double> hw_commands_positions_;
  std::vector<double> hw_states_positions_;
  std::vector<double> hw_states_velocities_;
};

}  // namespace my_robot_hardware

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(my_robot_hardware::RobotArmHardware, hardware_interface::SystemInterface)

#include <sched.h>
#include <sys/mman.h>
#include <pthread.h>
#include "rclcpp/rclcpp.hpp"
#include "controller_manager/controller_manager.hpp"

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);

  // 1. 内存锁定，防止因为 Swap 或缺页异常导致延迟突增
  if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
    perror("mlockall failed");
  }

  // 2. 绑定 CPU 隔离核心（例如绑定到 Core 2）
  cpu_set_t cpuset;
  CPU_ZERO(&cpuset);
  CPU_SET(2, &cpuset);
  pthread_t current_thread = pthread_self();
  if (pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset) != 0) {
    perror("pthread_setaffinity_np failed");
  }

  // 3. 设置 SCHED_FIFO 实时调度策略与优先级
  struct sched_param param;
  param.sched_priority = 90; // 高优先级
  if (pthread_setschedparam(current_thread, SCHED_FIFO, &param) != 0) {
    perror("pthread_setschedparam failed");
  }

  auto executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
  auto cm = std::make_shared<controller_manager::ControllerManager>(executor, "controller_manager");

  // 独立起一个实时线程跑控制循环
  std::thread cm_thread([cm]() {
    rclcpp::Rate rate(1000.0); // 1 kHz 闭环
    while (rclcpp::ok()) {
      cm->read(rclcpp::Clock().now(), rate.period());
      cm->update(rclcpp::Clock().now(), rate.period());
      cm->write(rclcpp::Clock().now(), rate.period());
      rate.sleep();
    }
  });

  executor->add_node(cm);
  executor->spin();

  cm_thread.join();
  rclcpp::shutdown();
  return 0;
}