craic/README.md

CRAIC — Camera-Robot AI Control System

本仓库为 中国机器人及人工智能大赛（CRAIC）机器人任务挑战赛（小型桌面级） 参赛代码。

ESP32-S3 双核摄像头 + ROS 2 机械臂控制 + 视觉抓取 + 激光 SLAM 定位导航的一体化机器人系统。

赛事信息

中国机器人及人工智能大赛（China Robot and Artificial Intelligence Competition，简称 CRAIC）是由 中国人工智能学会（CAAI）主办的全国性学科竞赛，始于 1999 年，已列入全国普通高等学校学科竞赛排行榜及教育部 A 类竞赛名单。

本仓库参加赛项：机器人任务挑战赛（小型桌面级） — Robot Task Challenge (Small Desktop Level)

项目	信息
大赛名称	中国机器人及人工智能大赛
英文名称	China Robot and Artificial Intelligence Competition (CRAIC)
主办单位	中国人工智能学会 (CAAI)
赛事级别	全国普通高等学校学科竞赛排行榜 / 教育部 A 类
参赛赛项	机器人任务挑战赛（小型桌面级）
官方网站	www.caairobot.com

硬件

组件	型号
主控	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 采集 OV2640 JPEG，Core 1 WiFi 推流 + 异步 UDP 命令接收
• MJPEG 推流：http://<IP> 实时查看，/stream 端点供 OpenCV/YOLO 消费
• UDP 控制：端口 8888，非阻塞中断回调处理底盘 + 机械臂 + 激光指令
• WiFi 配置：首次启动创建热点 ESP32-S3-Camera（密码 12345678），串口发送 WIFI:SSID:PASSWORD 切换 Station 模式

2. 机械臂控制与视觉抓取 (ros2/src/udp_teleop/)

节点	功能
arm_control	完整逆运动学/正运动学，关节空间 + 笛卡尔空间运动，4 个 ROS 服务，状态发布 (10Hz)，自动归零，动态 J5/z4 适配
vision_grasp	相机坐标→基坐标变换，自动抓取/释放流程（松开→移动→夹取→回收），多线程服务调用
box_detection_grasp	YOLO 实时方框检测（MJPEG 流），单目深度估计，自动/手动模式，检测到即触发抓取
keyboard_control	键盘 UDP 遥控（底盘 WASD/QE + 机械臂 ↑↓←→ + 关节选择 2-6），3 种输入后端

机械臂服务接口：

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. 激光 SLAM 定位与导航 (ros2/src/craic_localization/)

三个阶段式比赛流程：

阶段	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 回显服务器，用于调试协议通信

快速开始

ESP32 固件

cd jxbeye
pio run -t upload
pio device monitor

ROS 2 控制系统

# 环境准备（首次）
conda create -n ros2_humble -c robostack-staging -c conda-forge ros-humble-desktop
conda activate ros2_humble

# 编译（一键）
cd ros2
./build.sh
source install/setup.bash

# 或按需编译指定包
./build.sh --packages-select craic_localization udp_teleop

启动机械臂控制：

# 终端 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

启动键盘遥控：

ros2 run udp_teleop keyboard_control \
    --ros-args --params-file src/udp_teleop/config/params.yaml

启动定位与导航：

# 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

命令行工具

# 机械臂控制
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 发送，ASCII 文本协议：

# 底盘控制（麦克纳姆轮全向速度）
XYW:<X_speed>:<Y_speed>:<W_angular>:XZHY\n

# 机械臂控制（6 轴角度 + 高度）
JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n

# 激光控制
LASERON\n
LASEROFF\n

• 角度单位：度（已包含零点偏移）
• 高度单位：mm
• 速度单位：X/Y mm/s，W deg/s

使用示例

ROS 服务调用

# 机械臂移动到目标位置
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}"

定位导航工作流

# 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 集成

import rclpy
from geometry_msgs.msg import Point

class VisionDetector(rclpy.node.Node):
    def __init__(self):
        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
        self.grasp_pub.publish(msg)

坐标系说明

机械臂基坐标系（Z 轴朝上）：

• 原点：J1 线性滑轨底部
• X 轴：基座正前方，Y 轴：基座左侧，Z 轴：竖直向上
• 单位：mm

相机坐标系（水平安装）：

• X 轴：右侧，Y 轴：向下，Z 轴：正前方（光轴）
• 单位：mm

底盘坐标系（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	机械臂逆运动学完整数学推导（2 连杆平面臂 + z4 偏移）
docs/localization.md	定位系统文档：SLAM 建图、AMCL 定位、示教导航、故障排查
docs/box_detection_grasp.md	YOLO 方框检测与自动抓取：配置、服务、深度估计原理
ros2/README.md	ROS 2 节点详细文档
tools/README.md	命令行工具完整参数与工作流示例

依赖

环境	依赖
ESP32	PlatformIO, Arduino framework, esp32-camera
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)

许可

MIT License