基于 ROS2 + ros2_control + MoveIt 2 的四轴机械臂控制系统，将 达妙 DM-J4340 无刷伺服电机 通过 USB-CAN 适配器 接入 ros2_control 框架，并集成 气动吸盘 完成箱子抓取任务。
[PC] ─┬─ /dev/ttyACM0 ─ USB-CAN 适配器 ─ CAN 总线 ─ DM-J4340 电机 ×4
├─ /dev/ttyUSB0 ─ USB 继电器 1 ─ 吸盘电磁阀
└─ /dev/ttyUSB1 ─ USB 继电器 2 ─ 气阀
| 关节 | 电机 CAN ID | 限位 (rad) | 说明 |
|---|---|---|---|
base_yaw_joint |
1 | [-3.14, 3.14] | 底座旋转 |
shoulder_pitch_joint |
2 | [-0.42, 3.58] | 大臂俯仰 |
elbow_pitch_joint |
3 | [0.0, 3.14] | 小臂俯仰 |
wrist_pitch_joint |
4 | [-3.14, 0.3] | 手腕俯仰 |
┌─────────────────────────────────────────┐
│ 应用层 box_motion / IK 服务 │
├─────────────────────────────────────────┤
│ 控制器层 JointTrajectoryController │ 100Hz
├─────────────────────────────────────────┤
│ 硬件抽象层 ArmSystemHardware │ pluginlib 插件
├─────────────────────────────────────────┤
│ 协议层 DM-J4340 CAN 协议编解码 │
├─────────────────────────────────────────┤
│ 传输层 USB-CAN 串口适配器 │ 921600bps
├─────────────────────────────────────────┤
│ 物理层 DM-J4340 电机 / 继电器 │
└─────────────────────────────────────────┘
- Ubuntu 22.04
- ROS2 Humble
- MoveIt 2
- ros2_control
cd ~/Desktop/ros2_project/arm_control
colcon build --symlink-install
source install/setup.bashros2 launch arm_movelt real_robot.launch.py \
serial_port:=/dev/ttyACM0 \
serial_baudrate:=921600 \
suction_relay1_port:=/dev/ttyUSB0 \
suction_relay2_port:=/dev/ttyUSB1# 箱子抓取
ros2 topic pub /arm_box_motion/start std_msgs/msg/Empty "{}"
# 回零
ros2 topic pub /arm_home_motion/start std_msgs/msg/Empty "{}"
# 交互式目标点
ros2 topic pub /controller_command std_msgs/msg/String "data: '0.3 0.0 0.15'"
# 手动吸盘
ros2 topic pub /suction/enable std_msgs/msg/Bool "data: true"src/
├── arm_control/ # 核心控制包
│ ├── include/arm_control/
│ │ ├── arm_system_hardware.hpp # ros2_control 硬件接口
│ │ └── dm_j4340_motor_driver/
│ │ ├── dm_j4340_protocol.hpp # CAN 协议编解码
│ │ └── usbcan_serial.hpp # USB-CAN 串口传输层
│ ├── src/
│ │ ├── arm_system_hardware.cpp # ★ 硬件接口核心实现
│ │ ├── arm_controller.cpp # target_to_moveit 终端节点
│ │ ├── moveit_plan_service.cpp # 自定义 IK 求解服务
│ │ ├── box_motion_node.cpp # 箱子抓取流程编排
│ │ └── dm_j4340_scan_node.cpp # CAN 总线电机扫描
│ ├── srv/PlanMove.srv # 运动规划服务定义
│ ├── config/box_motion.yaml # 箱子抓取参数
│ └── arm_hardware_plugins.xml # ★ pluginlib 插件注册
│
├── arm_model/ # 机器人模型包
│ └── urdf/
│ ├── robot_arm.urdf # 纯 URDF 运动学结构
│ └── robot_arm.real.xacro # ★ ros2_control 真实硬件配置
│
├── arm_movelt/ # MoveIt 2 配置与启动
│ ├── launch/
│ │ └── real_robot.launch.py # ★ 主启动文件
│ └── config/
│ └── ros2_controllers.yaml # 控制器配置 (100Hz)
│
└── suction_controller/ # 吸盘控制包
├── src/suction_controller/
│ └── suction_controller_node.py # 继电器串口控制
└── config/suction_controller.yaml
通过 pluginlib 将 ArmSystemHardware 注册为 hardware_interface::SystemInterface 插件：
<!-- arm_hardware_plugins.xml -->
<library path="arm_system_hardware">
<class name="arm_control/ArmSystemHardware"
type="arm_control::ArmSystemHardware"
base_class_type="hardware_interface::SystemInterface"/>
</library>在 robot_arm.real.xacro 的 <ros2_control> 标签中完成插件名、串口参数、关节-电机映射：
<ros2_control name="RealSystem" type="system">
<hardware>
<plugin>arm_control/ArmSystemHardware</plugin>
<param name="serial_port">/dev/ttyACM0</param>
<param name="serial_baudrate">921600</param>
<param name="control_mode">position_velocity</param>
</hardware>
<joint name="base_yaw_joint">
<param name="motor_id">1</param>
<command_interface name="position">
<param name="min">-3.14</param>
<param name="max">3.14</param>
</command_interface>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<!-- 其余关节类似 -->
</ros2_control>on_init() → 解析 URDF 参数
on_configure() → 打开串口 + 检测电机在线
on_activate() → 保存零点 + 切换控制模式 + 使能电机
[read/write 循环 @100Hz]
on_deactivate() → 回零 + 失能电机
on_shutdown() → 安全停机 + 关闭串口
用户命令
→ moveit_plan_service (IK 求解)
→ JointTrajectoryController (100Hz 插值)
→ ArmSystemHardware::write() (坐标转换 + CAN 帧封装)
→ USB-CAN 串口 → CAN 总线 → DM-J4340 电机
← ArmSystemHardware::read() (解析反馈帧 + 逆坐标转换)
← JointStateBroadcaster
← /joint_states → 下一次规划的起点
本系统使用 位置-速度模式（PositionVelocity），通过 CAN 帧发送 IEEE 754 float 格式的目标位置和速度限制。
| CAN ID 用途 | 计算公式 |
|---|---|
| 位速控制帧 | motor_id + 0x100 |
| 寄存器读写 | 0x7FF |
| MIT 控制帧 | motor_id |
电机反馈为 16 位定点量化数据，由协议层还原为浮点值：
吸盘/气阀不走 ros2_control，而是通过 ROS2 Topic + Python 串口独立控制：
| 继电器 | 设备 | 命令 |
|---|---|---|
| 继电器1 | /dev/ttyUSB0 (吸盘) |
ON: A0 01 01 A2 / OFF: A0 01 00 A1 |
| 继电器2 | /dev/ttyUSB1 (气阀) |
ON: A0 01 01 A2 / OFF: A0 01 00 A1 |
/suction/enable=true → 继电器1闭合（真空吸附）
/suction/enable=false → 继电器1断开 + 继电器2脉冲3秒（正压释放）
t=0s ros2_control_node → on_init → on_configure (打开串口, 检测电机)
robot_state_publisher → 发布 /tf /robot_description
t=2s spawner → on_activate (使能电机, 保存零点, 切换模式)
arm_controller 就绪
joint_state_broadcaster → /joint_states
t=2s+ move_group / plan_service / box_motion_node / suction 就绪
| 设计点 | 实现 | 效果 |
|---|---|---|
| 插件机制 | PLUGINLIB_EXPORT_CLASS |
ros2_control 动态加载，不修改框架代码 |
| 非阻塞 I/O | O_NONBLOCK + select() |
100Hz 控制循环不卡顿 |
| 零拷贝接口 | 裸指针导出 state/command | 控制器与硬件间无额外拷贝开销 |
| 多控制模式 | MIT/PV/Vel/ForcePosition | 灵活切换刚性/柔顺控制 |
| 安全停机 | 回零 → 失能 → 关串口 | 防止意外断电后飞车 |
| 坐标转换 | direction + offset + gear_ratio | 适配任意安装方向和减速比 |
| 吸盘解耦 | 独立 ROS2 节点 + 串口 | 不干扰实时控制回路 |
| 参数 | 默认值 | 说明 |
|---|---|---|
serial_port |
/dev/ttyACM0 |
USB-CAN 串口设备 |
serial_baudrate |
921600 |
串口波特率 |
control_mode |
position_velocity |
控制模式 |
enable_on_activate |
true |
激活时自动使能 |
home_on_deactivate |
true |
停用时自动回零 |
shutdown_home_duration_s |
4.0 |
回零持续时间 |
timeout_ms |
10 |
CAN 帧超时 |
retry_count |
3 |
通信重试次数 |
| 参数 | 默认值 | 说明 |
|---|---|---|
box_center_distance_m |
0.42 |
箱子中心 X 距离 |
box_height_m |
0.25 |
箱子高度 |
box_lift_height_m |
0.08 |
抬起高度 |
box_suction_settle_s |
0.8 |
吸附稳定时间 |
home_joint_positions |
[0,0,0,0] |
回零关节角 |
MIT