工业协议►EtherCAT

06 ROS 2 与 EtherCAT 集成

06 ROS 2 与 EtherCAT 集成

本文重点：如何把 EtherCAT 主站接入 ros2_control——使用现成的 ethercat_driver_ros2，或自写 hardware_interface::SystemInterface；周期链路、DC 与控制周期对齐、MoveIt 到伺服 CSP 的完整数据流。
前置阅读：05-主站实现-IgH与SOEM.md。
涉及第三方包的接口细节以其官方文档/仓库为准。

1. ros2_control 架构回顾

EtherCAT 在 ROS 2 里的接入点是 ros2_control 的硬件抽象层：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
┌──────────────────────────────────────────────┐
│ Controller Manager（update_rate，如 1000Hz）    │
│   ├─ joint_trajectory_controller (JTC)         │
│   ├─ joint_state_broadcaster                   │
│   └─ ...                                       │
│        ↑ state interfaces   ↓ command interfaces
├──────────────────────────────────────────────┤
│ Resource Manager                               │
├──────────────────────────────────────────────┤
│ hardware_interface::SystemInterface（插件）      │
│   on_init / on_configure / on_activate         │
│   read() ←── 反馈       write() ──→ 指令        │
└──────────────┬───────────────────────────────┘
               │  ← EtherCAT 主站（IgH / SOEM）
          伺服/IO 从站

每个控制周期 Controller Manager 依次执行：read()（硬件 → state interface）→ 各 controller 的 update() → write()（command interface → 硬件）。EtherCAT 集成的本质就是：在 read/write 里完成一次过程数据交换。

硬件通过 URDF 的 <ros2_control> 标签声明：

1
2
3
4
5
6
7
8
9
10
11
<ros2_control name="my_arm" type="system">
  <hardware>
    <plugin>my_pkg/MyEthercatSystem</plugin>
    <param name="master_id">0</param>
  </hardware>
  <joint name="joint1">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <state_interface name="velocity"/>
  </joint>
</ros2_control>

2. 路线一：ethercat_driver_ros2（推荐起点）

ICube-Robotics/ethercat_driver_ros2 是社区较成熟的通用方案（细节、支持的 ROS 发行版与 API 以官方仓库文档为准）：

特性 说明
主站后端 基于 IgH EtherCAT Master（需先按上一篇部署内核模块）
架构 提供通用 EthercatDriver SystemInterface + ec_module 插件体系（pluginlib）
内置模块 通用 CiA402 驱动器模块、通用 GPIO 模块等；厂商专用从站可自写插件
配置方式 URDF <ros2_control> 标签指定从站插件 + YAML 文件描述 PDO 通道映射，无需写 C++（标准从站的情况下）

2.1 配置模式（示意）

URDF 中每个 joint/gpio 关联一个 ec_module 及其 YAML：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
<ros2_control name="ec_system" type="system">
  <hardware>
    <plugin>ethercat_driver/EthercatDriver</plugin>
    <param name="master_id">0</param>
    <param name="control_frequency">1000</param>
  </hardware>
  <joint name="joint1">
    <command_interface name="position"/>
    <state_interface name="position"/>
    <ec_module name="my_servo">
      <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
      <param name="alias">0</param>
      <param name="position">0</param>
      <param name="slave_config">$(find my_pkg)/config/servo.yaml</param>
    </ec_module>
  </joint>
</ros2_control>

YAML 把从站的 PDO 通道映射到 state/command interface（示意结构，键名以官方文档为准）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
vendor_id: 0x00000000        # 从站厂商 ID（ethercat slaves -v 可查）
product_id: 0x00000000
assign_activate: 0x0300      # DC SYNC0 使能（来自 ESI）
rpdo:                        # 主站 → 从站（输出）
  - index: 0x1607
    channels:
      - {index: 0x607A, sub_index: 0, type: int32,
         command_interface: position, factor: 1000, offset: 0}
      - {index: 0x6040, sub_index: 0, type: uint16}   # 控制字由 CiA402 状态机管理
tpdo:                        # 从站 → 主站（输入）
  - index: 0x1A07
    channels:
      - {index: 0x6064, sub_index: 0, type: int32,
         state_interface: position, factor: 0.001}
      - {index: 0x6041, sub_index: 0, type: uint16}   # 状态字

factor/offset 完成编码器计数 ↔ 弧度的线性换算；CiA402 插件内部处理 Operation Enabled 状态机（控制字/状态字握手），应用只看到 position 接口。

2.2 何时不够用

  • 厂商从站有非标对象/特殊初始化序列（SDO 启动脚本超出配置能力时）→ 写自定义 ec_module 插件；
  • 需要 SOEM 后端、或对主站生命周期有特殊管理需求 → 路线二。

3. 路线二：自写 SystemInterface

直接在 SystemInterface 里调用 IgH ecrt_*（或 SOEM ec_*）API。生命周期回调与 EtherCAT 阶段的对应：

回调 EtherCAT 侧动作 注意
on_init(HardwareInfo) 只解析 URDF <ros2_control> 参数（从站位置、PDO 表、换算系数），分配好所有缓冲 不碰硬件；info_.joints 里拿 param
on_configure ecrt_request_master、创建 domain、slave_config、注册 PDO entry 失败要返回 ERROR 而不是崩溃
on_activate ecrt_activate、取 domain 数据指针、SDO 写运行模式（如 0x6060=8 CSP）、驱动器使能状态机 激活后配置冻结；首个 write 前 command 要初始化为当前反馈值，防止跳变
read() ecrt_master_receive + ecrt_domain_process，从过程映像读 0x6064 等 → state interface
write() command interface → 过程映像 0x607A 等，ecrt_domain_queue + ecrt_master_send
on_deactivate 驱动器下使能、停发目标 从站看门狗会兜底，但主动下使能更干净

3.1 read/write 周期与 update_rate 的关系

  • Controller Manager 的 update_rate（controller_manager.yaml）决定 read/update/write 的调用频率——它就是 EtherCAT 的总线周期，因为每次 write 发一帧。
  • 因此 update_rate 必须等于你规划的 EtherCAT cycle time（如 1000Hz = 1ms），并且 controller_manager 所在线程/进程要按上一篇 §5 做实时化（SCHED_FIFO、绑核、mlockall；ros2_control 支持在具备权限时以实时优先级运行 update 循环，具体机制以其文档为准）。
  • receive 在 read() 开头、send 在 write() 结尾，天然形成 “上周期帧在本周期开头收回” 的流水线，与 IgH 推荐用法一致。

3.2 实时循环纪律

read/write 运行在控制主循环里，等同硬实时代码：

  • 禁止内存分配：所有 vector 在 on_init 里 resize 好；不要用会分配的字符串操作。
  • 禁止常规日志：RCLCPP_INFO 涉及锁和分配。需要日志用节流宏（低频）或无锁环形缓冲 + 后台线程落盘；错误状态先记标志位，出循环再报。
  • 禁止阻塞调用：SDO 读写（ecrt_slave_config_create_sdo_request 的异步方式除外）、service、参数读取都不进循环。
  • WKC / domain state 异常时不要立即抛异常终止，先按帧计数容忍与降级策略处理（见下一篇）。

4. 端到端周期链路

以 1ms 周期、CSP 模式伺服为例：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
t=0.0ms  controller_manager 周期唤醒
   ├─ hw.read()
   │    ecrt_master_receive / ecrt_domain_process   ← 收回上周期帧
   │    过程映像 0x6064(实际位置) → state_interface
   ├─ JTC.update()
   │    轨迹采样插值 → command_interface(position)
   ├─ hw.write()
   │    command → 过程映像 0x607A(目标位置)
   │    ecrt_domain_queue / ecrt_master_send        → 帧发出
   │
   ├─ 帧在总线上"飞行"，逐个从站 on-the-fly 读写（数十µs 量级）
   │
t≈DC相位  各伺服 SYNC0 事件（由 DC 同步，全轴同时刻）
   │    伺服锁存 0x607A 作为本周期目标 → 内部位置环执行
   │    同时锁存编码器 → 写入 0x6064 供下周期上送
   │
t=1.0ms  下一周期 read() 收到上述反馈  →  循环

三个”周期”必须对齐：controller_manager update_rate = 主站发帧周期 = 从站 SYNC0 周期。

5. DC 与 ros2_control 周期对齐

问题 说明 对策
cycle time 不一致 SYNC0 周期（ecrt_slave_config_dc / YAML assign_activate 配套参数）≠ update_rate 周期，会导致目标丢失或重复执行 三处配置写同一个值，代码里加断言
主站相位漂移 clock_nanosleep(CLOCK_MONOTONIC) 与 DC 参考时钟（从站 0 的本地时钟）是两个时钟源，会缓慢漂移 周期性 ecrt_master_application_time() + ecrt_master_sync_reference_clock()，或实现主站时钟伺服（微调睡眠周期跟踪 DC）；IgH 亦支持以参考从站为准调整（版本相关，以官方文档为准）
发帧抖动预算 帧必须在 SYNC0 之前送达从站；抖动过大 → 从站报 DC 同步丢失（AL 0x001A 等） 经验预算：发帧时刻 + 最大抖动 + 总线传输时间 < SYNC0 时刻。用 sync0_shift 把 SYNC0 相位后移（如周期的 30%~50%），给主站抖动留裕量
抖动来源排序 调度延迟 ≫ 网络栈 ≫ 总线传播 cyclictest 先量调度抖动；若 max > 100µs 量级，先解决 OS 配置再怀疑其他

经验值参考（非规范）：1ms 周期下，主站唤醒抖动控制在 ±20µs 内、sync0_shift 设为 400µs 左右，是常见的稳定工作点；具体以实测为准。

6. MoveIt / JTC 到 CSP 目标位置的数据流

1
2
3
4
5
6
7
8
9
10
11
12
13
14
MoveIt (move_group)
  └─ 规划出 JointTrajectory（离散路点，非实时）
       │ FollowJointTrajectory action
       ▼
joint_trajectory_controller（实时，每周期）
  └─ 按当前时间在路点间样条插值 → 期望位置 q_d
       │ command_interface: position
       ▼
SystemInterface.write()
  └─ q_d × 换算系数（rad → 编码器计数）→ 0x607A Target Position
       ▼
EtherCAT 帧 → 伺服 SYNC0 锁存 → 驱动器内部位置环（通常还有前馈）
       ▼
0x6064 Actual Position → read() → joint_state_broadcaster → /joint_states → MoveIt 监控

要点：

  • CSP 模式下插值责任在主站侧（JTC 每周期给一个新目标），伺服只做位置环跟踪 + 少量内插；轨迹平滑度取决于 JTC 插值 + 周期稳定性。
  • 模式选择：CSP（周期目标位置）最常用；CSV/CST（速度/力矩）对应 velocity/effort command interface；PP（点到点）不适合 JTC 这种周期流式指令。
  • 单位换算与方向（减速比、编码器分辨率、正方向定义）是最常见的”动起来不对”的原因，先在无负载下单轴验证。

7. 实践路线建议（checklist）

按顺序，每步通过再进下一步：

    1. IgH 主站部署完成，ethercat slaves 能看到全部从站且进 PREOP
    1. ethercat upload/download 手动读写对象字典，确认驱动器 CiA402 状态机能走到 Operation Enabled（先不带 ros2_control）
    1. ethercat states OP + 观察 WKC 正常，跑通”裸主站”周期循环（可用 IgH 自带示例）
    1. 接 ros2_control：先只挂 joint_state_broadcaster，验证 read 链路（/joint_states 数值、单位正确）
    1. 单轴 + forward_command_controller 小幅度指令，验证 write 链路与方向/换算
    1. 挂 JTC，ros2 action send_goal 发短轨迹
    1. 最后接 MoveIt；全程用 cyclictest / 主站统计监控抖动

原则：总线问题用总线工具（ethercat 命令、Wireshark）解决，别隔着 ros2_control 猜。排障手段见下一篇。

8. 与本工作区 armRobot 项目的关系

/home/cp/work2/ros2Learn/app/armRobot 是一个 6 轴机械臂 ROS 2 栈（Humble + MoveIt 2 + ros2_control）。就 EtherCAT 而言，其 README 的现状是：

  • arm_hw 提供 mock / UART 两种后端；真机 launch 的 hardware 参数虽然接受 ethercat 取值，但会明确拒绝启动——“当前没有 EtherCAT master、PDO 映射或真实 EtherCAT backend”，即 EtherCAT transport 只是预留的边界，没有实现。
  • 现有真机链路走 UART 帧协议（arm_protocol → MCU 固件），伺服控制在 MCU 侧。

若将来真实接入 EtherCAT 伺服，本篇的两条路线都适用：

路线 做法 对 armRobot 的改动
ethercat_driver_ros2 URDF 里把 arm_hw 插件换成 ethercat_driver/EthercatDriver，每轴配 CiA402 YAML 主要是 description/bringup 配置层；arm_safety 的 command_allowed 门逻辑需要在新硬件层重新落点
自写 SystemInterface 在 arm_hw 内实现 EtherCAT 后端（IgH API），复用其现有接口边界 代码量大，但能保留现有 mock/uart/ethercat 统一的插件选择结构与安全联锁

注意 armRobot 强调的安全边界仍然成立：EtherCAT 化不改变”外部硬接线 STO 切断能量、ROS 安全链不能替代 STO”的原则。

下一篇：07-组网调试与排障.md —— 布线、上电检查、WKC/AL Status/Wireshark 排障与性能验证。

文章互动

阅读 --

留言

0 条留言

正在加载留言…