📚 课程目录第1章课程导入

1. 课程导入

1.1 ROS2核心概念与通信机制

ROS2（Robot Operating System 2）并非传统意义上的操作系统，而是一个分布式机器人中间件框架，它运行在Linux、Windows或macOS之上，为机器人开发者提供标准化的通信、进程管理、硬件抽象与工具链支持。与ROS1相比，ROS2最大的架构变革在于引入了DDS（Data Distribution Service）作为底层通信中间件，从而实现了真正的去中心化、实时性与跨平台能力。

在ROS2中，最核心的通信模型是节点（Node）。每个节点是一个独立的可执行进程，负责完成某一项具体功能，例如读取激光雷达数据、执行运动学解算或发布速度指令。节点之间通过以下四种通信方式协作：

话题（Topic）：异步、单向、多对多的数据流通道，适用于传感器数据、状态信息等高频数据流。发布者（Publisher）与订阅者（Subscriber）通过话题名称解耦。
服务（Service）：同步、双向的请求-应答模式，适用于一次性调用，如“开启机械臂夹爪”或“查询当前位姿”。
动作（Action）：适用于长时间执行且可反馈的任务，如“导航到目标点”。动作由目标（Goal）、反馈（Feedback）和结果（Result）三个部分组成。
参数（Parameter）：节点运行时可动态读取或修改的配置项，如PID增益、最大速度限制等。

核心要点：ROS2的节点是独立进程，彼此通过DDS进行通信。话题用于持续数据流，服务用于同步请求，动作用于长时任务，参数用于运行时配置。理解这四种通信方式，是读懂控制器节点源码的前提。

1.2 控制器节点在ROS2系统中的角色定位

控制器节点是机器人系统中连接“感知-决策-执行”闭环的关键枢纽。它接收来自规划层或用户指令的目标值，结合传感器反馈的当前状态，通过控制算法（如PID、模型预测控制或状态机逻辑）计算出精确的执行指令，并发送给底层执行器。控制器节点的质量直接决定了机器人运动的稳定性、精度与安全性。

以一个典型的差速驱动机器人为例，控制器节点需要完成以下任务：

订阅目标速度：从 /cmd_vel 话题接收线速度与角速度指令。
订阅里程计反馈：从 /odom 话题获取当前实际位姿与速度。
执行控制算法：计算目标速度与当前速度的误差，通过PID控制器输出补偿量。
发布电机指令：将补偿后的速度指令转换为左右轮的目标转速，发布到 /motor_cmd 话题。
提供诊断服务：通过服务接口响应“使能/失能”“重置控制器”等外部调用。

通信接口	类型	消息格式	方向	说明
`/cmd_vel`	话题	geometry_msgs/Twist	订阅	接收上层速度指令
`/odom`	话题	nav_msgs/Odometry	订阅	获取里程计反馈
`/motor_cmd`	话题	custom_msgs/MotorCmd	发布	输出电机控制指令
`/controller_enable`	服务	std_srvs/SetBool	提供	使能或失能控制器
`/controller_status`	话题	diagnostic_msgs/DiagnosticStatus	发布	上报控制器健康状态

1.3 项目源码整体架构解析

本课程所分析的控制器节点项目源码，采用ROS2标准的功能包（Package）结构组织。功能包是ROS2中代码复用的基本单元，包含可执行文件、库、配置文件与启动文件。整体架构遵循分层解耦的设计原则，将硬件驱动、控制算法与ROS2接口层分离，便于单元测试与算法替换。

源码目录结构如下：

robot_controller/
├── CMakeLists.txt              # 构建规则
├── package.xml                 # 包元数据与依赖声明
├── include/robot_controller/
│   ├── controller_node.hpp     # 节点类头文件
│   ├── pid_controller.hpp      # PID算法封装
│   └── motor_driver_interface.hpp  # 电机驱动抽象接口
├── src/
│   ├── controller_node.cpp     # 节点主逻辑实现
│   ├── pid_controller.cpp      # PID算法实现
│   └── main.cpp                # 程序入口
├── config/
│   └── controller_params.yaml  # 控制器参数配置
├── launch/
│   └── controller_launch.py    # 启动文件
└── test/
    ├── test_pid.cpp            # PID单元测试
    └── test_controller.cpp     # 节点集成测试

各模块职责划分如下：

controller_node.hpp/cpp：核心节点类，负责话题订阅、定时器回调、状态机管理与对外服务接口。它是整个控制器的“大脑”。
pid_controller.hpp/cpp：独立的PID控制算法实现，不依赖ROS2头文件，便于在纯C++环境中进行单元测试。
motor_driver_interface.hpp：定义电机驱动的纯虚接口，实际硬件驱动通过继承该接口实现，从而支持仿真与真实硬件切换。
controller_params.yaml：以YAML格式存储PID增益、控制频率、速度限制等参数，运行时通过ROS2参数服务器动态加载。
controller_launch.py：启动文件，负责加载参数、运行节点并配置必要的环境变量。

设计亮点：将PID算法与ROS2节点解耦，使得算法不依赖任何ROS2头文件。这意味着开发者可以在本地使用 g++ 直接编译并测试PID逻辑，无需启动roscore或构建整个工作空间，大幅提升了开发效率。

1.4 控制器节点的工作流程与运行机制

控制器节点采用事件驱动 + 定时中断的混合模型。节点初始化时，会创建一个固定频率（如50Hz）的定时器，每次定时器触发时执行一次完整的控制周期。控制周期的流程如下：

数据采集：从内部缓冲区读取最新的 /cmd_vel 指令与 /odom 反馈。由于话题回调与定时器回调运行在不同线程，因此需要使用互斥锁或原子变量保护共享数据。
状态检查：确认控制器处于“使能”状态，且电机驱动无故障报警。
误差计算：分别计算线速度误差 e_v = v_target - v_current 与角速度误差 e_w = w_target - w_current。
PID运算：对两个误差通道分别执行PID计算，得到线速度补偿量 u_v 与角速度补偿量 u_w。
运动学解算：将补偿后的速度通过差速运动学模型转换为左右轮目标角速度。
指令发布：将左右轮目标角速度封装为 MotorCmd 消息，发布到 /motor_cmd 话题。
状态上报：周期性发布诊断信息，包括当前控制模式、跟踪误差、PID输出限幅状态等。

常见误区：许多初学者误以为话题回调函数中可以直接执行耗时操作。实际上，话题回调运行在DDS的调度线程中，若在回调中执行PID运算或阻塞操作，会阻塞其他话题的接收，导致数据丢失或控制周期抖动。正确的做法是：回调中仅做数据拷贝与标记更新，将控制运算放在独立的定时器回调中执行。

1.5 源码中的关键设计模式与工程实践

本项目源码中体现了多个值得学习的工程实践模式，这些模式在大型机器人项目中具有普适性：

依赖注入：ControllerNode 的构造函数接收一个 MotorDriverInterface 的智能指针，而不是直接实例化具体驱动。这使得在仿真环境中可以注入 SimMotorDriver，在真实机器人上注入 RealMotorDriver，无需修改节点逻辑。
参数动态配置：通过ROS2的 add_on_set_parameters_callback 注册参数回调，使得PID增益可以在线调整，无需重启节点。调试时极为方便。
看门狗机制：节点会监控 /cmd_vel 话题的最后接收时间，若超过500ms未收到新指令，则自动将目标速度置零，防止机器人因通信中断而失控。
时间戳对齐：在数据采集阶段，会比较 /cmd_vel 与 /odom 消息头中的时间戳，若两者时间差超过阈值，则丢弃本次控制周期，避免使用过期的反馈数据。

以下代码展示了控制器节点中定时器回调的核心骨架，体现了上述设计思想：

void ControllerNode::controlLoop()
{
  // 1. 加锁读取最新指令与反馈
  std::lock_guard<std::mutex> lock(data_mutex_);
  
  // 2. 看门狗检查
  auto now = this->now();
  if ((now - last_cmd_time_).seconds() > 0.5) {
    target_twist_.linear.x = 0.0;
    target_twist_.angular.z = 0.0;
  }
  
  // 3. 计算误差
  double e_v = target_twist_.linear.x - current_odom_.twist.twist.linear.x;
  double e_w = target_twist_.angular.z - current_odom_.twist.twist.angular.z;
  
  // 4. PID运算
  double u_v = pid_v_.compute(e_v, dt_);
  double u_w = pid_w_.compute(e_w, dt_);
  
  // 5. 运动学解算
  double v = target_twist_.linear.x + u_v;
  double w = target_twist_.angular.z + u_w;
  double wheel_r = (v + w * wheel_base_ / 2.0) / wheel_radius_;
  double wheel_l = (v - w * wheel_base_ / 2.0) / wheel_radius_;
  
  // 6. 发布电机指令
  MotorCmd msg;
  msg.left_rpm = wheel_l * 60.0 / (2.0 * M_PI);
  msg.right_rpm = wheel_r * 60.0 / (2.0 * M_PI);
  motor_cmd_pub_->publish(msg);
}

1.6 本章小结与学习路径建议

本章从ROS2的通信机制出发，明确了控制器节点在机器人系统中的枢纽地位，并剖析了项目源码的目录结构与核心设计模式。读者应重点掌握以下内容：

ROS2中话题、服务、动作与参数四种通信方式的适用场景与代码写法。
控制器节点如何通过订阅目标指令与反馈数据，经过控制算法计算后发布执行指令。
源码中依赖注入、看门狗、时间戳对齐等工程实践的价值与实现方法。
理解 controller_node.cpp 中控制循环的完整流程，并能够画出时序图。