diff --git a/README.md b/README.md index db04ba5..9109de2 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ > 本仓库为 **[中国机器人及人工智能大赛](https://www.caairobot.com)**（CRAIC）**机器人任务挑战赛（小型桌面级）** 参赛代码。 -ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机器人系统。 +ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取 + 激光 SLAM 定位导航的一体化机器人系统。 ## 赛事信息 @@ -19,24 +19,6 @@ ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取的一体化机 | 参赛赛项 | 机器人任务挑战赛（小型桌面级） | | 官方网站 | [www.caairobot.com](https://www.caairobot.com) | -## 项目结构 - -``` -craic/ -├── jxbeye/ # ESP32-S3 固件 (PlatformIO) -│ ├── src/main.cpp # 双核摄像头推流 + UDP 控制 -│ └── platformio.ini # ESP32-S3-WROOM-1-N16R8 配置 -├── ros2/ # ROS 2 控制系统 -│ ├── src/arm_control_msgs/ # 消息和服务定义 -│ └── src/udp_teleop/ # 机械臂控制和视觉抓取节点 -├── tools/ # 独立工具脚本 -│ ├── udp_control.py # 命令行机械臂控制（带逆运动学） -│ ├── camera_to_base.py # 相机坐标到基坐标变换 -│ └── camera_capture.py # MJPEG 流采集 -└── docs/ # 技术文档 - └── arm.md # 机械臂运动学推导 -``` - ## 硬件 | 组件 | 型号 | @@ -44,34 +26,100 @@ craic/ | 主控 | ESP32-S3-WROOM-1-N16R8 (16MB Flash, 8MB PSRAM) | | 摄像头 | OV2640 (XGA 1024×768, JPEG) | | 机械臂 | 6-DOF，飞特 SCS/STS 串行舵机 | +| 激光雷达 | YDLiDAR TminiPro (360°, 10Hz) | +| 底盘 | 麦克纳姆轮底盘，LZUCAR 底盘 MCU（串口里程计） | +| 激光 | 激光模块（LASERON / LASEROFF） | + +## 项目结构 + +``` +craic/ +├── jxbeye/ # ESP32-S3 固件 (PlatformIO) +│ ├── src/main.cpp # 双核：Core 0 摄像头采集，Core 1 WiFi推流 + UDP控制 +│ ├── platformio.ini # 开发板与 PSRAM 配置 +│ └── lib/FTServo/ # 飞特串行舵机库 +├── ros2/ # ROS 2 控制系统 +│ ├── build.sh # 一键编译脚本 +│ ├── src/arm_control_msgs/ # 机械臂消息与服务定义 (ament_cmake) +│ │ ├── msg/ # JointState, TCPPose +│ │ └── srv/ # MoveJoints, MovePose, GetPose, SetGripper +│ ├── src/udp_teleop/ # UDP 遥控与机械臂控制 (ament_python) +│ │ ├── udp_teleop/ # arm_control, vision_grasp, keyboard_control, box_detection_grasp +│ │ ├── launch/ # vision_grasp.launch.py（统一启动） +│ │ ├── config/ # arm / vision / keyboard / box_detection 参数 +│ │ └── models/ # YOLO 模型权重 (box_detection.pt) +│ ├── src/craic_localization/ # 定位与导航 (ament_python) +│ │ ├── launch/ # mapping / localization / lidar / bringup +│ │ ├── config/ # amcl / gmapping / lidar 参数 +│ │ └── rviz/ # 定位专用 RViz 配置 +│ ├── src/rf2o_laser_odometry/ # 激光里程计 (ament_cmake, C++14) +│ └── src/ydlidar_ros2_driver/ # YDLiDAR 驱动 (ament_cmake) +├── tools/ # 独立命令行工具（不依赖 ROS） +│ ├── udp_control.py # 机械臂控制（逆运动学 + 插值） +│ ├── camera_to_base.py # 相机→基座坐标系变换 +│ ├── camera_capture.py # MJPEG 流帧采集 +│ ├── udp_server.py # UDP 回显调试服务器 +│ └── README.md # 工具详细文档 +├── docs/ # 技术文档 +│ ├── arm.md # 机械臂运动学推导 +│ ├── localization.md # 定位系统完整文档 +│ └── box_detection_grasp.md # 方框检测与自动抓取 +└── dataset/ # 竞赛训练数据（黑方块图像 + 标定） +``` ## 核心功能 ### 1. ESP32-S3 固件 (`jxbeye/`) -- **双核架构**：Core 0 采集，Core 1 推流 -- **MJPEG 推流**：`http://` 实时查看 -- **UDP 控制**：端口 8888，非阻塞异步处理 -- **WiFi 配置**：串口发送 `WIFI:SSID:PASSWORD` 配置 +- **双核架构**：Core 0 采集 OV2640 JPEG，Core 1 WiFi 推流 + 异步 UDP 命令接收 +- **MJPEG 推流**：`http://` 实时查看，`/stream` 端点供 OpenCV/YOLO 消费 +- **UDP 控制**：端口 8888，非阻塞中断回调处理底盘 + 机械臂 + 激光指令 +- **WiFi 配置**：首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），串口发送 `WIFI:SSID:PASSWORD` 切换 Station 模式 -### 2. ROS 2 机械臂控制 (`ros2/`) +### 2. 机械臂控制与视觉抓取 (`ros2/src/udp_teleop/`) -**arm_control 节点**： -- 关节空间和笛卡尔空间运动控制 -- 完整逆运动学和正运动学 -- 服务接口：`move_joints`, `move_pose`, `get_pose`, `set_gripper` -- 状态发布（10Hz） +| 节点 | 功能 | +|------|------| +| `arm_control` | 完整逆运动学/正运动学，关节空间 + 笛卡尔空间运动，4 个 ROS 服务，状态发布 (10Hz)，自动归零，动态 J5/z4 适配 | +| `vision_grasp` | 相机坐标→基坐标变换，自动抓取/释放流程（松开→移动→夹取→回收），多线程服务调用 | +| `box_detection_grasp` | YOLO 实时方框检测（MJPEG 流），单目深度估计，自动/手动模式，检测到即触发抓取 | +| `keyboard_control` | 键盘 UDP 遥控（底盘 WASD/QE + 机械臂 ↑↓←→ + 关节选择 2-6），3 种输入后端 | -**vision_grasp 节点**： -- 相机坐标到基坐标系自动转换 -- 自动抓取：释放 → 移动 → 抓取 → 回收 -- 自动释放：移动 → 释放 → 回收 +**机械臂服务接口**： +```bash +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose +ros2 service call /arm_control/set_gripper arm_control_msgs/srv/SetGripper "{grip: true}" +``` -### 3. 工具脚本 (`tools/`) +### 3. 激光 SLAM 定位与导航 (`ros2/src/craic_localization/`) -- `udp_control.py` - 命令行机械臂控制（支持逆运动学） -- `camera_to_base.py` - 坐标变换工具 -- `camera_capture.py` - 相机帧采集（支持自动扫描） +三个阶段式比赛流程： + +| 阶段 | Launch 文件 | 功能 | TF 链 | +|------|------------|------|-------| +| P3 标定 | `bringup.launch.py` | 轮式里程计 + 激光驱动 + RViz | `odom → base_footprint → laser_frame` | +| P4 建图 | `mapping.launch.py` | 激光 + rf2o 里程计 + slam_gmapping | `map → odom → base_footprint → laser_frame` | +| P5 导航 | `localization.launch.py` | 激光 + rf2o + map_server + AMCL | `map → odom → base_footprint → laser_frame` | + +**定位系统节点**： + +| 节点 | 功能 | +|------|------| +| `chassis_odometry` | 串口读取 LZUCAR 底盘 MCU 轮式里程计（32 字节协议） | +| `teach_points` | 交互式示教工具：回车记录 map 帧位姿，支持预设点位序列 (B_1..F_1)，自动存 YAML | +| `navigate_to_point` | 全向 P 控制器自主导航到示教点，UDP XYW 指令闭环驱动，干运行/单次模式可选 | +| `show_points` | RViz MarkerArray 可视化示教点（箭头 + 标签），实时更新 | + +### 4. 命令行工具 (`tools/`) + +| 工具 | 功能 | +|------|------| +| `udp_control.py` | 机械臂关节/笛卡尔空间控制，逆运动学，轨迹插值，状态缓存，干运行 | +| `camera_to_base.py` | 相机→TCP→基座完整变换链，支持相机安装偏移与旋转 | +| `camera_capture.py` | ESP32 MJPEG 流帧采集，自动子网扫描寻找相机 | +| `udp_server.py` | UDP 回显服务器，用于调试协议通信 | ## 快速开始 @@ -83,8 +131,6 @@ pio run -t upload pio device monitor ``` -首次启动创建热点 `ESP32-S3-Camera`（密码 `12345678`），访问 `http://192.168.4.1`。 - ### ROS 2 控制系统 ```bash @@ -92,124 +138,187 @@ pio device monitor conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop conda activate ros2_humble -# 编译 +# 编译（一键） cd ros2 -export PYTHON_EXECUTABLE=$CONDA_PREFIX/bin/python -export PYTHON_INCLUDE_DIR=$CONDA_PREFIX/include/python3.12 -export PYTHON_LIBRARY=$CONDA_PREFIX/lib/libpython3.12.so - -colcon build --packages-select arm_control_msgs \ - --cmake-args \ - -DPython_EXECUTABLE=$PYTHON_EXECUTABLE \ - -DPython_INCLUDE_DIR=$PYTHON_INCLUDE_DIR \ - -DPython_LIBRARY=$PYTHON_LIBRARY - -colcon build --packages-select udp_teleop +./build.sh source install/setup.bash -# 运行机械臂控制 +# 或按需编译指定包 +./build.sh --packages-select craic_localization udp_teleop +``` + +**启动机械臂控制**： +```bash +# 终端 1：机械臂控制器 ros2 run udp_teleop arm_control \ --ros-args --params-file src/udp_teleop/config/arm_control.yaml -# 运行视觉抓取（新终端） +# 终端 2：视觉抓取 ros2 run udp_teleop vision_grasp \ --ros-args --params-file src/udp_teleop/config/vision_grasp.yaml + +# 或一键启动全部（含 YOLO 检测） +ros2 launch udp_teleop vision_grasp.launch.py detection:=true auto_grasp:=true +``` + +**启动键盘遥控**： +```bash +ros2 run udp_teleop keyboard_control \ + --ros-args --params-file src/udp_teleop/config/params.yaml +``` + +**启动定位与导航**： +```bash +# P4 建图 +ros2 launch craic_localization mapping.launch.py +# 建图完成后保存 +ros2 run nav2_map_server map_saver_cli -f src/craic_localization/maps/craic + +# P5 定位 + 导航 +ros2 launch craic_localization localization.launch.py + +# 新终端：示教记录点位 +ros2 run craic_localization teach_points \ + --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml + +# 新终端：自主导航到示教点 +ros2 run craic_localization navigate_to_point ``` ### 命令行工具 ```bash -# 机械臂控制（关节空间） -python tools/udp_control.py joints \ - --height -100 --j2 10 --j3 20 --j4 30 --duration 2.0 - -# 机械臂控制（笛卡尔空间） -python tools/udp_control.py pose \ - --x 200 --y 100 --z -100 --phi 45 --duration 2.0 +# 机械臂控制 +python tools/udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0 +python tools/udp_control.py joints --height -100 --j2 10 --j3 20 --j4 30 # 相机采集 python tools/camera_capture.py --ip 192.168.4.1 python tools/camera_capture.py --scan # 自动扫描 + +# UDP 调试 +python tools/udp_server.py +echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888 ``` ## UDP 协议 -所有指令通过 UDP 端口 `8888` 发送： +所有指令通过 UDP 端口 `8888` 发送，ASCII 文本协议： -```bash -# 底盘控制（麦克纳姆轮） -XYW::::XZHY\n +``` +# 底盘控制（麦克纳姆轮全向速度） +XYW::::XZHY\n -# 机械臂控制（6 轴） +# 机械臂控制（6 轴角度 + 高度） JXB:::::::0:0:EZHY\n # 激光控制 -LASERON / LASEROFF +LASERON\n +LASEROFF\n ``` +- 角度单位：度（已包含零点偏移） +- 高度单位：mm +- 速度单位：X/Y mm/s，W deg/s + ## 使用示例 ### ROS 服务调用 ```bash -# 查询位姿 -ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose - -# 移动到目标位置 +# 机械臂移动到目标位置 ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, duration: 2.0}" +# 快速抓取 +ros2 service call /arm_control/move_pose arm_control_msgs/srv/MovePose \ + "{x: 200.0, y: 100.0, z: -100.0, phi: 45.0, grip: true, duration: 2.0}" + +# 查询位姿 +ros2 service call /arm_control/get_pose arm_control_msgs/srv/GetPose + # 视觉抓取（发布相机坐标） ros2 topic pub --once /vision_grasp/grasp_target geometry_msgs/Point \ "{x: 10.0, y: 5.0, z: 250.0}" ``` +### 定位导航工作流 + +```bash +# 1. 启动定位系统 +ros2 launch craic_localization localization.launch.py + +# 2. 键盘遥控到目标位置（另一终端） +ros2 run udp_teleop keyboard_control \ + --ros-args --params-file src/udp_teleop/config/params.yaml + +# 3. 示教记录点位（第三终端） +ros2 run craic_localization teach_points \ + --ros-args -p output_file:=$PWD/src/craic_localization/config/taught_points.yaml +# 交互命令：回车记录 → r 记录 → p 查看 → save 保存 → q 退出 + +# 4. 自主导航到示教点 +ros2 run craic_localization navigate_to_point +# 输入目标点名（如 B_1），机器人自动导航到位 +``` + ### Python 集成 ```python import rclpy from geometry_msgs.msg import Point -class VisionDetector: +class VisionDetector(rclpy.node.Node): def __init__(self): - self.grasp_pub = self.create_publisher(Point, 'vision_grasp/grasp_target', 10) - + super().__init__('vision_detector') + self.grasp_pub = self.create_publisher( + Point, 'vision_grasp/grasp_target', 10) + def on_detection(self, camera_x, camera_y, camera_z): msg = Point() - msg.x = camera_x - msg.y = camera_y - msg.z = camera_z + msg.x = camera_x; msg.y = camera_y; msg.z = camera_z self.grasp_pub.publish(msg) ``` -## 文档 - -- **ROS 2 系统**：[ros2/README.md](ros2/README.md) - 完整的 ROS 节点文档 -- **运动学推导**：[docs/arm.md](docs/arm.md) - 机械臂逆运动学数学推导 -- **视觉标定**：[docs/vision_calibration_horizontal.md](docs/vision_calibration_horizontal.md) - 相机标定指南 - ## 坐标系说明 **机械臂基坐标系**（Z 轴朝上）： - 原点：J1 线性滑轨底部 -- X 轴：基座正前方 -- Y 轴：基座左侧 -- Z 轴：竖直向上（高度） +- X 轴：基座正前方，Y 轴：基座左侧，Z 轴：竖直向上 +- 单位：mm **相机坐标系**（水平安装）： -- X 轴：右侧 -- Y 轴：向下 -- Z 轴：正前方（光轴） +- X 轴：右侧，Y 轴：向下，Z 轴：正前方（光轴） +- 单位：mm -变换由 `camera_to_base.py` 和 `vision_grasp` 节点自动处理。 +**底盘坐标系**（ROS TF）： +``` +map ──(AMCL)──> odom ──(rf2o)──> base_footprint ──(static)──> laser_frame + ──(static)──> base_link +``` +- `map`：全局固定坐标系（AMCL 定位输出） +- `odom`：连续里程计坐标系（rf2o 激光里程计） +- `base_footprint`：底盘投影中心 +- `laser_frame`：激光雷达安装位置（外参可调：`lidar_x`, `lidar_y`, `lidar_yaw`） + +## 文档 + +| 文档 | 说明 | +|------|------| +| [docs/arm.md](docs/arm.md) | 机械臂逆运动学完整数学推导（2 连杆平面臂 + z4 偏移） | +| [docs/localization.md](docs/localization.md) | 定位系统文档：SLAM 建图、AMCL 定位、示教导航、故障排查 | +| [docs/box_detection_grasp.md](docs/box_detection_grasp.md) | YOLO 方框检测与自动抓取：配置、服务、深度估计原理 | +| [ros2/README.md](ros2/README.md) | ROS 2 节点详细文档 | +| [tools/README.md](tools/README.md) | 命令行工具完整参数与工作流示例 | ## 依赖 | 环境 | 依赖 | |------|------| | ESP32 | PlatformIO, Arduino framework, esp32-camera | -| ROS 2 | ROS 2 Humble (robostack), Python 3.12 | -| 工具 | NumPy, OpenCV (可选) | +| ROS 2 | ROS 2 Humble, `slam_gmapping`, `nav2_amcl`, `nav2_map_server`, `nav2_lifecycle_manager`, `serial` | +| Python | NumPy, OpenCV, Ultralytics (YOLO), pynput (可选) | +| 系统 | YDLidar-SDK, colcon, conda (robostack) | ## 许可