ROS2 + Gazebo 协同控制系统：YOLO 视觉检测道钉 → 轨道小车与龙门架同步运动。
A 3-axis gantry robot with rail cart, depth camera, and YOLO-based spike detection — from URDF modeling to full ROS2 simulation and coordinated control.
铁路轨道上，小车搭载深度相机沿轨道行驶，YOLO 实时检测道钉（fasteren），将像素坐标经深度投影和 TF 变换转换到世界坐标系，最后协调小车（diff_drive）和龙门架（3-axis gantry）同步运动到目标位置。
The goal is to build a simulation-ready gantry robot model with coordinated rail-cart control, and integrate it into a full ROS2 robotics workflow.
Planned development stages / 计划开发阶段：
- URDF Model Creation（URDF 模型建立）✅
- RViz Visualization（RViz 可视化）✅
- Gazebo Simulation Integration（Gazebo 仿真环境导入）✅
- ros2_control Controller Integration（ros2_control 控制器集成）✅
- YOLO Visual Perception（YOLO 视觉感知）🚧
- Coordinated Motion Planning（协同运动规划与控制）🚧
gantry_robot/
├── ros2_ws/ # ROS2 工作空间
│ └── src/
│ ├── yolo_ros_interfaces/ # 自定义 ROS2 消息（Detection, DetectionArray）
│ ├── yolo_ros2/ # YOLO 检测 + 深度估计 + TF 坐标变换
│ │ ├── yolo_ros2/
│ │ │ ├── node_core.py # 共享基类（BaseYOLONode, ControlMixin）
│ │ │ ├── yolo_detector_world.py # 视觉感知核心节点
│ │ │ └── ...
│ │ └── weights/
│ │ └── best.pt # YOLO 模型权重
│ └── railway_with_fasteren_2/ # Gazebo 仿真 + 小车/龙门架控制
│ ├── launch/
│ │ └── simulation.launch.py # 仿真启动入口
│ ├── config/ # ros2_control 配置
│ ├── urdf/ # 机器人模型（xacro）
│ └── meshes/ # 3D 模型（STL）
├── ultralytics/ # 修改版 ultralytics 源码（含自定义网络层）
├── gantry_rebuild/ # 龙门架重建 CAD → URDF 工作流
│ ├── 01_modules/ # 独立机械模块
│ ├── 02_links/ # 连杆结构
│ ├── 03_final_assembly/ # 完整装配体
│ └── 04_urdf/ # 最终 URDF ROS2 包
│ └── gantry_description/
│ ├── config/
│ ├── launch/
│ ├── meshes/
│ └── urdf/
├── cad/ # 原始 SolidWorks CAD 模型
│ ├── gantry/
│ └── rail/
├── scripts/ # 辅助脚本（SW→URDF 转换等）
│ └── sw2urdf/
├── README.md
└── CHANGELOG.md
龙门架为 笛卡尔 PPP 结构，由三个直线关节组成：
The robot is a Cartesian gantry robot (PPP configuration) composed of three prismatic joints.
| Axis / 轴 | Joint Type / 类型 | Direction / 方向 |
|---|---|---|
| Y axis | Prismatic | Rail movement / 轨道方向 |
| Z axis | Prismatic | Vertical / 垂直方向 |
| X axis | Prismatic | Horizontal / 水平方向 |
Kinematic chain / 运动链结构：
base_link
└── y_axis_joint
└── y_carriage
└── z_axis_joint
└── z_carriage
└── x_axis_joint
└── x_carriage
RGB 相机 + 深度相机
│ message_filters 时间同步
▼
YOLO 检测道钉 (ultralytics)
│ pixel → 3D (相机内参投影)
▼
camera_link 坐标 ──TF2 变换──► odom 世界坐标
│
▼
CartGantryCoordDemo
协同控制（时间同步）
┌─ 小车 diff_drive 前进
└─ 龙门架 joint_trajectory 移动
路径: ros2_ws/src/yolo_ros2/yolo_ros2/node_core.py
所有检测/控制节点的共享基础设施：
- BaseYOLONode — YOLO 模型加载、视频/JSON 保存、检测解析、图像显示
- ControlMixin — 速度控制发布、里程计记录、相机内参、深度提取、像素→3D 投影 (
pixel_to_3d) - run_node() — 通用 main 入口包装
路径: ros2_ws/src/yolo_ros2/yolo_ros2/yolo_detector_world.py
将相机图像转换为世界坐标系下的道钉 3D 坐标：
- 通过
message_filters同步 RGB 和深度图像 - 使用 ultralytics YOLO 模型检测道钉
- 从深度图中提取目标中心深度值（3层回退策略：精确像素 → 3×3邻域 → bounding box中心区域）
- 通过相机内参将像素坐标投影为相机坐标系下的 3D 坐标
- 使用 TF2 将
camera_link坐标变换到odom世界坐标系 - 发布两个话题：
target_position_3d_camera和target_position_3d_odom
路径: ros2_ws/src/railway_with_fasteren_2/railway_with_fasteren_2/cart_gantry_coord_demo.py
小车与龙门架的时间同步协同运动算法：
- 订阅
target_position_3d_odom等待道钉检测 - 计算小车前进距离和龙门架各轴行程，取最长时间作为协同总时间
- 同步执行：小车 diff_drive 前进 + 龙门架 joint_trajectory 运动
- 核心公式：
cart_speed = remaining / max(cart_time, gantry_time)保证两者同时完成
⚠️ 当前协同控制为简单 demo，未使用高级路径规划算法，后续需要改进。
- Ubuntu 22.04 + ROS 2 Humble
- Gazebo 11+
- Python 3.10+, ultralytics (修改版), OpenCV, NumPy
sudo apt install gazebo ros-humble-gazebo-ros2-control python3-opencv python3-numpy
pip install ultralyticscd ros2_ws
colcon build --symlink-install
source install/setup.bash# 终端1：启动仿真（Gazebo + 小车 + 龙门架）
ros2 launch railway_with_fasteren_2 simulation.launch.py
# 终端2：启动 YOLO 道钉检测
ros2 run yolo_ros2 yolo_detector_world
# 终端3：执行协同控制
ros2 run railway_with_fasteren_2 cart_gantry_coord_democolcon build
source install/setup.bash
ros2 launch gantry_description display.launch.py| 话题 | 类型 | 说明 |
|---|---|---|
target_position_3d_odom |
PointStamped |
道钉在 odom 坐标系下的 3D 位置 |
target_position_3d_camera |
PointStamped |
道钉在 camera_link 坐标系下的 3D 位置 |
/cart_drive_controller/cmd_vel_unstamped |
Twist |
小车速度指令 |
/cart_drive_controller/odom |
Odometry |
小车里程计 |
/gantry_controller/joint_trajectory |
JointTrajectory |
龙门架轨迹指令 |
detections |
DetectionArray |
YOLO 检测结果（含深度） |
三个 ros2_control 控制器通过 controller_manager 统一管理：
- cart_drive_controller — diff_drive 类型，控制小车轨道上的直线运动
- gantry_controller — joint_trajectory 类型，控制龙门架 X/Y/Z 三轴
- joint_state_broadcaster — 发布所有关节状态供 TF 树使用
best.pt 基于 ultralytics 但添加了自定义网络层，直接使用官方 ultralytics 包加载会报错。需要使用 ultralytics/ 目录下的修改版源码，或在官方 ultralytics 中注册自定义网络结构。
由于 GitHub 文件大小限制，部分 CAD 文件可能不会直接存储在仓库中。
从 OneDrive 下载 CAD 模型：
OneDrive Link: https://1drv.ms/f/c/3c28f7a366113b48/IgD0npFhsFTSTL47JCOQn_N8AfvSmUbKAafBUksmfndOePk
- 改进
cart_gantry_coord_demo.py的路径规划算法 - 完善 YOLO 自定义网络在 ultralytics 中的注册流程
- 补全缺失的 detector variant 文件
- 实现从仿真到 RealSense 硬件的无缝迁移
- 多道钉场景下的目标选择与排序
本项目用于 ROS2 机器人仿真与开发研究。
This project is part of a robotics simulation and development study using ROS2.