📚 课程目录第4章控制器节点设计

4. 控制器节点设计

控制器节点是机器人系统中的“决策大脑”，它接收来自传感器、里程计、目标指令等输入，经过控制算法计算后输出速度指令。在 ROS2 中，控制器节点通常以自定义 C++ 类为核心，围绕 rclcpp::Node 构建。设计一个健壮的控制器节点，需要从类结构、回调函数组织和状态机三方面入手。

4.1 类结构设计：职责分离与生命周期

控制器节点类不应把所有逻辑堆叠在一个大文件中。推荐采用“一个节点类 + 多个功能组件”的结构，将硬件抽象、控制算法、状态管理、参数配置分离。以下是一个典型的控制器节点类骨架：

class RobotController : public rclcpp::Node
{
public:
  RobotController(const std::string &name);
  ~RobotController();

private:
  // 回调函数组
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
  void timerCallback();

  // 核心控制逻辑
  void updateState();
  void computeControlOutput();
  void publishFeedback();

  // 组件对象
  std::shared_ptr<ControllerAlgorithm> algo_;   // 控制算法
  std::shared_ptr<StateMachine> state_machine_; // 状态机
  std::shared_ptr<ParamManager> params_;        // 参数管理

  // ROS2 接口
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
};

类结构设计的关键原则：

单一职责：算法、状态机、参数管理各自独立，便于单元测试和复用。
显式生命周期：在构造函数中初始化所有资源，在析构函数中释放，避免悬挂指针。
接口清晰：回调函数只做数据接收和格式转换，真正的业务逻辑放在独立方法中。

设计建议：将控制算法封装为纯 C++ 类（不依赖 ROS2 类型），这样可以在仿真环境中脱离 ROS2 单独测试算法正确性。

4.2 回调函数组织：多源输入与优先级

ROS2 节点通过回调函数响应订阅消息、定时器和服务请求。控制器节点通常有多个回调，它们可能在不同线程中执行，因此需要明确回调之间的数据流和优先级。

以下是一个典型的多回调数据流关系：

回调函数设计要点：

数据同步：多个回调可能并发访问共享变量（如目标速度、当前位姿），必须使用 std::mutex 或 ROS2 的 rclcpp::executors::MultiThreadedExecutor 配合锁机制。
回调优先级：定时器回调是控制主循环，应保证其不被长时间阻塞。订阅回调只做轻量级赋值，避免在回调中执行复杂计算。
超时保护：如果长时间未收到 odom 消息，应触发安全状态（如停止运动），防止失控。

4.3 状态机设计：安全与模式切换

控制器节点必须处理多种运行状态，例如：初始化（INIT）→ 待机（IDLE）→ 运行（RUNNING）→ 暂停（PAUSED）→ 错误（ERROR）→ 恢复（RECOVERY）。状态机负责管理这些状态之间的合法转移，并定义每个状态下的行为。

以下是一个简化的状态转移表：

当前状态	触发事件	条件	下一状态	动作
INIT	初始化完成	参数加载成功	IDLE	发布就绪消息
IDLE	收到启动指令	使能信号为 true	RUNNING	清零积分项
RUNNING	收到暂停指令	无	PAUSED	保持当前位置
RUNNING	检测到超限	速度/位置超限	ERROR	急停并报警
PAUSED	收到恢复指令	无	RUNNING	继续控制
ERROR	收到复位指令	故障已排除	IDLE	重新初始化

状态机的实现可以采用 枚举 + switch-case 或 状态模式。对于简单控制器，枚举方式足够清晰：

enum class CtrlState { INIT, IDLE, RUNNING, PAUSED, ERROR };

void StateMachine::update(CtrlEvent event)
{
  std::lock_guard<std::mutex> lock(mtx_);
  switch (state_) {
    case CtrlState::IDLE:
      if (event == CtrlEvent::START) {
        state_ = CtrlState::RUNNING;
        onEnterRunning();
      }
      break;
    case CtrlState::RUNNING:
      if (event == CtrlEvent::PAUSE) {
        state_ = CtrlState::PAUSED;
      } else if (event == CtrlEvent::FAULT) {
        state_ = CtrlState::ERROR;
        emergencyStop();
      }
      break;
    // ... 其他状态处理
  }
}

重要提醒：状态转移必须放在互斥锁保护下执行，因为状态变量会被多个回调线程读取。同时，emergencyStop() 等安全动作应直接写入硬件接口，不能依赖后续回调执行。

4.4 案例：差速驱动机器人控制器

以差速驱动机器人为例，控制器节点需要将 cmd_vel（线速度 + 角速度）转换为左右轮速度指令。核心流程如下：

订阅 /cmd_vel 和 /odom，分别获取目标速度和当前位姿。
定时器以 50Hz 周期触发 timerCallback()。
在 timerCallback() 中调用 updateState() 更新状态机。
若状态为 RUNNING，调用 computeControlOutput() 计算左右轮速度：

void computeControlOutput()
{
  double v = target_twist_.linear.x;
  double w = target_twist_.angular.z;
  double wheel_base = params_->wheel_base;

  // 差速模型逆解
  double v_left = v - w * wheel_base / 2.0;
  double v_right = v + w * wheel_base / 2.0;

  // 限幅保护
  v_left = clamp(v_left, -max_speed_, max_speed_);
  v_right = clamp(v_right, -max_speed_, max_speed_);

  // 发布到电机驱动
  auto msg = geometry_msgs::msg::Twist();
  msg.linear.x = v_left;
  msg.angular.z = v_right;
  cmd_pub_->publish(msg);
}

该案例中，状态机确保在未收到有效 odom 消息超过 500ms 时自动进入 ERROR 状态，防止机器人带病运行。

4.5 常见误区与复盘

在设计控制器节点时，以下问题经常导致调试困难或安全事故：

误区一：回调函数中执行耗时操作。例如在 odomCallback 中直接进行 PID 计算，会阻塞订阅队列，导致消息丢失。正确做法是回调只更新数据，控制计算放在定时器回调中。
误区二：状态机缺少超时保护。如果只依赖外部指令切换状态，一旦通信中断，节点可能永远停留在 RUNNING 状态。应在定时器回调中检查数据新鲜度。
误区三：忽略线程安全。使用 MultiThreadedExecutor 时，多个回调并发访问同一变量会导致数据竞争。必须使用 std::atomic 或互斥锁。
误区四：状态转移动作不完整。例如从 RUNNING 切换到 PAUSED 时，没有保存当前积分项，恢复后导致跳变。

  复盘建议：在开发初期就为状态机编写单元测试，模拟各种事件序列（如“启动→暂停→恢复→故障→复位”），验证每个转移动作是否执行正确。使用 ROS2 的 launch_testing 框架可以自动化验证节点行为。

控制器节点的设计质量直接决定了机器人的安全性和控制性能。通过清晰的类结构划分、合理的回调函数组织以及严谨的状态机管理，可以构建出易于维护、扩展和调试的控制器节点。在实际项目中，建议先绘制状态转移图和类关系图，再动手编写代码，这样能显著减少后期返工。