ROS2 + CANopen + MoveIt2 实战：从零搭建机械臂控制系统

在工业自动化领域，CANopen 是一种广泛应用的通信协议，而 ROS2 作为机器人操作系统的新标准，如何将两者完美结合一直是开发者关注的热点。本文将通过一个完整的实战项目，详细介绍如何在 ROS2 Jazzy 上实现 CANopen 与 MoveIt2 的集成，控制 Pilz PRBT 机械臂。

项目地址：ROS2 CANopen Demo
环境要求：Ubuntu 24.04 + ROS2 Jazzy

项目背景

CANopen 作为基于 CAN 总线的应用层协议，在工业机械臂、伺服驱动器等设备中占据重要地位。ROS2 虽然在导航、感知等领域生态丰富，但在工业现场总线支持方面一直缺乏成熟的解决方案。

本项目的目标是：在 ROS2 中通过 CANopen 协议控制机械臂，并使用 MoveIt2 进行运动规划，提供一个完整的、可直接运行的示例。

核心挑战与解决方案

1. 启动时序问题（Boot Timeout）

问题描述：在调试过程中发现，CANopen 主站会报Boot Timeout错误。原因是 ros2_control_node 启动过早，在 CANopen 从站发送 Boot-Up 帧之前就开始尝试通信。

解决方案：

# 在 bus.yml 中配置
master:
    boot_timeout: 10000  # 主站超时 10s

defaults:
    boot_timeout_ms: 5000  # 从站 boot 等待时间 5s

同时优化启动顺序：

  • 先启动虚拟从站（fake_slaves）
  • 等待 Boot-Up 帧发送完成
  • 再启动 ros2_control_node
  • 最后加载控制器

使用 launch 文件中的 TimerAction 实现自动化的时序控制：

# moveit_ros2canopen_headless.launch.py
TimerAction(period=5.0, actions=[ros2_control_node]),  # 延迟 5s 启动

2. 虚拟 CAN 接口配置

实现方式：

# 加载内核模块
sudo modprobe vcan

# 创建 vcan0 接口
sudo ip link add dev vcan0 type vcan
sudo ip link set up vcan0

项目提供自动化配置 launch 文件：

ros2 launch prbt_robot_moveit_config vcan.launch.py

3. CANopen 与 ros2_control 的桥接

架构设计：

┌─────────────────────────────────────────┐
│         MoveIt2 运动规划                │
└──────────────┬──────────────────────────┘
               │
┌──────────────▼──────────────────────────┐
│    Joint Trajectory Controller          │
└──────────────┬──────────────────────────┘
               │
┌──────────────▼──────────────────────────┐
│    ros2_control Hardware Interface      │
│  (canopen_ros2_control/RobotSystem)     │
└──────────────┬──────────────────────────┘
               │ CANopen
┌──────────────▼──────────────────────────┐
│     CANopen Master + Slaves             │
└─────────────────────────────────────────┘

关键配置：

URDF 中集成 ros2_control：

<!-- prbt_canopen.urdf.xacro -->
<ros2_control name="prbt" type="system">
  <hardware>
    <plugin>canopen_ros2_control/RobotSystem</plugin>
    <param name="bus_config">$(find prbt_robot_support)/config/prbt/bus.yml</param>
    <param name="master_dcf">$(find prbt_robot_support)/config/prbt/master.dcf</param>
    <param name="can_interface_name">vcan0</param>
  </hardware>
  <!-- 关节配置 -->
</ros2_control>

CANopen 总线配置：

# bus.yml
master:
  node_id: 1

nodes:
  joint_1:
    node_id: 3
    dcf: "ZeroErr_Driver_V1.5.eds"
    # ... joint_2 到 joint_6

项目结构

ROS2_jazzy_moveit2_test/
├── src/
│   ├── prbt_robot_moveit_config/     # MoveIt2 配置
│   │   ├── config/
│   │   │   ├── prbt_canopen.urdf.xacro
│   │   │   ├── prbt_canopen_ros2_controllers.yaml
│   │   │   └── moveit_controllers.yaml
│   │   ├── launch/
│   │   │   ├── moveit_ros2canopen.launch.py
│   │   │   ├── moveit_ros2canopen_with_fake_slaves.launch.py  # ⭐ 推荐
│   │   │   └── moveit_ros2canopen_headless.launch.py         # 无头模式
│   │   └── scripts/
│   │       └── test_moveit_headless.py                      # 自动化测试
│   │
│   ├── prbt_robot_support/           # CANopen 支持
│   │   ├── config/prbt/
│   │   │   ├── bus.yml               # CANopen 配置 ⭐
│   │   │   └── prbt_0_1.dcf          # 设备配置
│   │   └── launch/fake_slaves.launch.py
│   │
│   └── arm_prbt/                     # 机器人 URDF
│
└── build/ & install/                 # 编译输出

快速入门

1. 安装依赖

sudo apt update
sudo apt install -y \
    ros-jazzy-moveit \
    ros-jazzy-ros2-canopen \
    ros-jazzy-canopen-fake-slaves \
    ros-jazzy-ros2-control \
    ros-jazzy-ros2-controllers \
    can-utils

2. 编译工作空间

cd ~/ROS2_jazzy_moveit2_test
colcon build
source install/setup.bash

3. 启动系统

方式一：完整演示（带 RViz）

ros2 launch prbt_robot_moveit_config moveit_ros2canopen_with_fake_slaves.launch.py

在 RViz 中：

  1. 拖动交互标记设置目标位姿
  2. 点击 Plan 规划路径
  3. 点击 Execute 执行运动

方式二：无头模式（适用于测试）

终端 1：启动系统

ros2 launch prbt_robot_moveit_config moveit_ros2canopen_headless.launch.py

终端 2：运行测试脚本

python3 src/prbt_robot_moveit_config/scripts/test_moveit_headless.py

4. 验证运行

# 查看 CAN 总线消息
candump -ta vcan0

# 查看关节状态
ros2 topic echo /joint_states --once

# 查看控制器状态
ros2 control list_controllers

深度解析

CANopen 通信流程

  1. NMT 状态机启动：

    • 从站上电后进入 Switch On Disabled 状态
    • 调用/joint_X/init服务切换到Operation Enabled
    • 启动后系统自动完成状态转换

  2. 同步周期通信：

    • SYNC 周期：10ms（100Hz）
    • RPDO：主站发送目标位置
    • TPDO：从站反馈实际位置

  3. CIA 402 驱动器配置：

    • 位置模式：周期同步位置模式（CSP，模式 8）
    • 控制字：6040h
    • 状态字：6041h
    • 目标位置：607Ah
    • 实际位置：6064h

启动时序分析

完整的启动过程（使用合并 launch 文件）：

Time    Component           Action
─────────────────────────────────────────────────
0s      ├─ fake_slaves      6个虚拟从站节点启动
        │                    发送Boot-Up帧（心跳报文）
        │
5s      ├─ ros2_control_node  启动，Master开始配置从站
        │                    读取DCF文件，配置SDO参数
        │                    启动SYNC周期
        │
10s     ├─ joint_state_broadcaster  发布关节状态
        │
        └─ prbt_group_controller    接收运动指令

参数调优经验

  1. boot_timeout_ms：

    • 默认值：2000ms（易超时）
    • 推荐值：5000ms（考虑系统负载）

  2. SYNC 周期：

    • 虚拟 CAN：10ms（100Hz）足够
    • 真实硬件：根据驱动器能力调整（1ms-10ms）

  3. 控制器参数：

    prbt_group_controller:
  type: joint_trajectory_controller/JointTrajectoryController
  joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]
  command_interfaces: [position]
  state_interfaces: [position, velocity]

调试技巧

1. CAN 总线监控

# 实时查看所有报文
candump -ta vcan0

# 筛选特定节点（如 node_id=3）
candump -ta vcan0 | grep " 003"

# 分析SYNC报文
candump -ta vcan0 | grep " 000"

典型输出：

 (169.558018)  vcan0  003   [8]  00 00 00 00 00 00 00 00  # TPDO1: 状态
 (169.558018)  vcan0  000   [0]                          # SYNC
 (169.568018)  vcan0  103   [8]  00 00 00 00 00 00 00 00  # RPDO1: 目标位置

2. ROS2 节点诊断

# 查看节点列表
ros2 node list

# 查看话题
ros2 topic list
ros2 topic hz /joint_states

# 服务调试
ros2 service list | grep init

3. 控制器调试

# 查看控制器状态
ros2 control list_controllers

# 手动发送轨迹（测试用）
ros2 topic pub /joint_trajectory_controller/joint_trajectory \
  trajectory_msgs/msg/JointTrajectory '{...}'

4. 日志分析

# 查看特定节点日志
ros2 node info /controller_manager

# 实时日志过滤
ros2 topic echo /rosout | grep ERROR

常见问题 FAQ

Q1: 出现 Boot Timeout 错误？

A: 检查两点：

  1. bus.yml 中 boot_timeout_ms: 5000
  2. 使用合并 launch 文件启动（自动处理时序）

Q2: 控制器无法启动？

A: 通常是前面的错误导致 ros2_control_node 崩溃：

# 检查前面的日志
ros2 node list  # 确认 /controller_manager 存在

Q3: 如何切换到真实 CAN 接口？

A: 修改两处配置：

  1. URDF: <param name="can_interface_name">can0</param>
  2. bus.yml: master: { can_device: can0 }

Q4: 如何支持更多关节？

A: 修改以下文件：

  1. URDF: 添加 joint_X 和 transmission
  2. bus.yml: 添加对应节点配置
  3. controllers.yaml: 更新 joints 列表

Q5: 无头模式如何扩展？

A: 参考 test_moveit_headless.py，使用 MoveIt Python API：

import moveit_py

# 连接到 MoveGroup
robot = moveit_py.Robot("prbt")
arm = robot.get_planning_component("prbt_arm")

# 设置目标
arm.set_goal_state(...)

# 规划并执行
plan = arm.plan()
arm.execute(plan)

总结

本项目展示了 ROS2 CANopen 集成的完整流程，主要贡献包括：

  1. 解决启动时序问题：通过 TimerAction 和 boot_timeout 配置，确保系统稳定启动
  2. 提供完整示例：包含配置、启动、测试全套流程，开箱即用
  3. 支持无头模式：便于自动化测试和 CI/CD 集成
  4. 详细调试指南：CAN 总线监控、日志分析等实用技巧

下一步工作

  • 接入真实 CANopen 驱动器（非虚拟）
  • 集成视觉伺服
  • 支持多机器人协同
# 开源 # 机器人 # c++
Logo
全球具身智能开发者社区

立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐

cover

从“感知缺失”到“力觉觉醒”：FACTR 2如何让低成本机械臂学会“手感”

avatar 全球具身智能开发者社区

vllm-omni 部署 π0（Pi-Zero）VLA 模型：基于 OpenPI 实时 WebSocket 的机器人动作策略服务实战

本文基于 vllm-omni 仓库中的 [π0 在线服务示例](https://link.gitcode.com/i/7539191769d07ef9a1cfb9f8a273829b)，完整讲解如何将 Physical Intelligence 的 π0（Pi-Zero）Vision-Language-Action（VLA）模型通过 vllm-omni 以 OpenPI 实时 WebSocket

avatar 全球具身智能开发者社区
cover

多链路聚合设备：从“链路备份”到“确定性传输底座”-1

avatar 全球具身智能开发者社区