---

📌 前言

最近在做一个大工程：将差速底盘的驱动架构从 ROS 1 全面迁移到 ROS 2 (Humble)。为了达到工业级的控制标准，我们使用了 ros2_control 框架，底层通过 CAN 卡与 DSRS 伺服驱动器（遵循 CANopen CiA 402 标准）进行通讯。

本以为搭好了 twist_mux 和 diff_drive_controller，把原来的 C++ 驱动封装进 Hardware Interface 就万事大吉了，没想到却开启了一场长达数小时、深挖到底层十六进制字节的极限 Debug 之旅。

在此记录下这个包含看门狗超时、PDO/SDO 映射陷阱、底层状态机时序，以及终极大反转的硬核排查过程，希望能帮到正在做 ROS 2 底盘开发的各位。

---

🐛 诡异的现象：数据完美下发，底盘稳如泰山

节点启动后，twist_mux 完美订阅了各个优先级的话题。通过终端下发 /cmd_vel 速度，控制器也没有报错。但是，车轮就是不动！

为了确认指令到底死在了哪一层，我采用了**“切香肠法”**，在硬件接口的 write 和 read 函数中加入了实时线程安全的探针日志：

RCLCPP_INFO_THROTTLE(
  rclcpp::get_logger("RTCgbotSystemHardware"),
  steady_clock, 1000, 
  "🚀 HW Write | Left target: %.3f rad/s, Right target: %.3f rad/s",
  hw_commands_[0], hw_commands_[1]);

探针结果显示：ROS 2 框架层 100% 正常！diff_drive_controller 成功算出了 9.467 rad/s 的轮端角速度，并准时交给了底层 C++ 驱动。

既然 ROS 2 没问题，那问题一定出在 CAN 通信或者电机驱动器上。

---

🕵️ 排查第一关：500ms 的死亡看门狗与心跳包

工业电机通常有着极其严格的安全机制。 在 CANopen 协议中，有一个 Heartbeat (心跳) 机制。如果电机在 500ms 内没有收到主站发来的心跳包或者 SYNC 同步帧，为了防止机器人失控，它会立刻进入安全状态，强行忽略所有速度指令。

在 ROS 1 的老代码中，这部分可能由独立线程维护。而在 ROS 2 中，由于 ros2_control 的 write 函数是 25Hz（每 40ms 执行一次）极其稳定的实时线程，这简直是天然的**“喂狗神器”**！

解决方案：直接在 write 循环中加入 NMT 状态帧和 0x0F（Enable Operation）控制字作为心跳，保持电机锁死使能状态。

---

🕵️ 排查第二关：手写“黑匣子”，抓出 ROS 2 控制器的隐形刹车

心跳加上了，车还是不动！由于在实时线程（RT Thread）中不能打断点，我直接在底层驱动里手搓了一个文件锁，把收发出去的 CAN 帧全部落盘到 .csv 黑匣子文件中：

// 核心日志埋点 (十六进制打印 CAN 帧)
snprintf(buffer, sizeof(buffer), "%ld,TX,0x%04X,%d,0x%08X,0x%08X\n",
         ms, frame.frame_id, frame.data_length, frame.data0, frame.data1);

打开 CSV 抓包文件一看，惊出一身冷汗：
TX, 0x0601, 8, 0x0060FF23, 0x00000000
发给电机目标速度寄存器（60FFh）的值，居然是绝对的 0！

真相大白：我之前只是在终端里敲了一次 ros2 topic pub 回车。而在 diff_drive_controller 和 twist_mux 中，配置了 0.5s 的安全超时判定（Watchdog Timeout）。单次指令发出去 0.5 秒后没后续，系统为了安全自动下发了速度 0 进行强行刹车！

正确测试姿势：必须带上 -r 参数连续轰炸！

ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.8, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

---

🕵️ 排查第三关：CiA 402 状态机的时序陷阱

加上连续发布后，黑匣子终于抓到了正确的速度值（比如 0.8m/s 转换后为 904），电机的状态字（Status Word）也返回了 0x37 (Operation Enabled)。但车，依然没动！

翻阅了厚厚的原厂通讯手册，最终发现了底盘初始化流程（motorInitial）中的致命时序错误：

❌ 错误时序：先使能电机 (0x0F) -> 然后再往 0x6060 写入模式 (3：PV速度模式)。
在电机已使能的状态下切模式，驱动器会直接拒绝执行！

✅ 正确时序：

1. setRunMode(3); // 必须在 Disabled 状态下切入速度模式
2. setAccAndDec(); // 设加减速
3. setPDOParam(); // 动态 PDO 映射
4. motorPrepare() -> motorDisable() -> motorEnable(); // 06 -> 07 -> 0F 唤醒使能

---

🏆 高手进阶：PDO vs SDO，ROS 2 实时控制的最优解

在排查过程中，为了排除映射问题，我一度退回了用 SDO (0x60FF) 暴力单次写寄存器的方式。但最终，我们还是把代码坚定地改回了 RPDO (0x201)。

• SDO (服务数据对象)：一问一答，带确认机制，延迟高，适合做初始化配置。
• PDO (过程数据对象)：无应答广播，直接映射到底层执行器。在 ros2_control 要求的 25Hz/100Hz 高频控制下，必须使用 PDO 才能保证控制的丝滑与低延迟！

💡 避坑 Tip：在 ROS 2 的硬件接口中，不要再手动写 enforceLimits() 了！速度限幅和加减速插补现在全部由 Controller 层的 yaml 配置文件接管，底层 C++ 代码只要做一个无情的高频“发报机”即可。

---

🤡 终极反转：一切完美的背后…

当所有软件时序、多线程锁、协议组装、大小端转换都调整到完美无瑕，代码精简到堪称艺术品时。

我深吸一口气，再次发送了连续的速度指令。
日志极其完美。
通讯极其完美。
但车轮就是没转。

我沉默地盯着车体，目光缓缓扫过底盘的外壳，突然定格在了一个红色的按钮上。

——急停开关（E-Stop）没有旋开。

急停开关物理切断了电机的强电使能回路，但没有切断 CAN 芯片的弱电通讯。于是，我的软件和电机的通讯主板进行了一场天衣无缝的“纸上谈兵”交流，唯独执行端没有通电。

把急停开关旋开后，恢复最初那套看似“有问题”其实完美运用了 PDO 通讯的代码，键盘遥控按下，底盘瞬间发出悦耳的伺服嗡鸣声，平滑地冲了出去。

📝 总结

虽然被硬件物理急停摆了一道，但这绝对是极其充实的一天。如果不经历这场极限拉扯，我们不可能：

1. 搞懂 ROS 2 控制器看门狗的底层逻辑。
2. 吃透 CANopen 状态机的严格时序。
3. 学会在实时线程里无阻塞地写 CSV 黑匣子日志。
4. 明确 PDO 映射对于高频底盘控制的不可替代性。

ROS 2 底盘迁移，成功通关！