From 5487a0a2ad56e4539f955c5eebefedd7212ddb9b Mon Sep 17 00:00:00 2001 From: Bug Date: Thu, 7 May 2026 17:12:28 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20detector=20=E4=B8=AD=E6=96=87=E6=97=A5?= =?UTF-8?q?=E5=BF=97=20+=20=E5=9B=BE=E5=83=8F=E5=BD=92=E6=A1=A3=20+=20?= =?UTF-8?q?=E6=9C=BA=E5=99=A8=E4=BA=BA=E4=BD=8D=E5=A7=BF=E8=BE=93=E5=87=BA?= =?UTF-8?q?;=20=E6=9B=B4=E6=96=B0=20README.md?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 152 ++++++++++++++++++------ src/vision_yolo/vision_yolo/detector.py | 127 ++++++++++++++++++-- 2 files changed, 232 insertions(+), 47 deletions(-) diff --git a/README.md b/README.md index 57490ce..84b895e 100644 --- a/README.md +++ b/README.md @@ -5,13 +5,15 @@ 本项目用于 Ubuntu + ROS 2 Humble 下的 TurtleBot3 办公室场景自动探索与建图，包含： - Gazebo Sim 8 (Harmonic) 仿真 -- Nav2 导航栈 +- Nav2 导航栈（速度优化版） - SLAM Toolbox 在线建图 -- explore_lite 自动探索 +- explore_lite 自动探索（调优版） +- YOLOv8 视觉检测（640×480，中文输出，带图像归档） - gzweb 网页端 3D 可视化 -- 自动保存地图并输出路径 +- 循环探索模式（永不退出，自动保存地图后重启） +- 一键停止脚本 -核心目标：一条命令启动，探索结束后自动保存地图并退出。 +核心目标：一条命令启动，机器人自动探索、检测、建图，循环运行直到手动停止。 --- @@ -21,6 +23,7 @@ - ROS 2 Humble - Gazebo Sim Harmonic - Node.js / npm（用于 `gzweb-demo`） +- Python 依赖：`ultralytics`, `opencv-python`, `numpy<2` --- @@ -28,12 +31,14 @@ - 仿真与模型：`turtlebot3_simulations/turtlebot3_gazebo/` - 探索节点：`m-explore-ros2/explore/` +- 视觉检测：`src/vision_yolo/vision_yolo/detector.py` - 一键启动脚本：`run_all.sh` - 一键停止脚本：`run_all_stop.sh` -- A+B 启动脚本：`run_ab.sh` +- 自动运行入口：`autorun.sh` - WebSocket 配置：`websocket.sdf` - gzweb 前端：`gzweb-demo/gzweb-demo/` - 地图输出目录：`turtlebot3_simulations/turtlebot3_gazebo/map/` +- 视觉输出目录：`/workspace/vision_output/YYYYMMDD_HHMMSS/` --- @@ -42,19 +47,25 @@ ### 1) 启动自动流程 ```bash -cd /home/xh2204/office_gzweb/vlm_office-main -./run_all.sh mapping --explore --gzweb +cd /workspace +./autorun.sh +``` + +等效于： + +```bash +./run_all.sh mapping --explore --gzweb --no-gzweb-vite --vision ``` 执行内容： -1. 启动 Gazebo Sim + ROS bridge + SLAM + RViz -2. 启动 gzweb websocket 与前端 -3. 启动 Nav2 -4. 启动 explore_lite 自动探索 -5. 自动检测探索结束（带超时） -6. 自动保存地图并打印路径 -7. 自动停止全部进程并退出 +1. 启动 Gazebo Sim + ROS bridge + SLAM + RViz +2. 启动 gzweb websocket（foundation /opt/gzweb，端口 8000） +3. 启动 Nav2（快速参数：max_vel_x 0.5 m/s） +4. 启动 explore_lite 自动探索（planner_frequency 1.0，min_frontier_size 0.3） +5. 启动 YOLOv8 视觉检测（1 秒/帧，640×480，中文日志，图像归档） +6. 自动检测探索结束（带超时） +7. **自动保存地图 → 等待 30 秒 → 重新启动探索（循环模式，永不退出）** ### 2) 停止全部进程 @@ -62,16 +73,25 @@ cd /home/xh2204/office_gzweb/vlm_office-main ./run_all_stop.sh ``` +或通过停止信号： + +```bash +touch /workspace/.run_all_stop +``` + --- ## 端口说明 -- `5173`：gzweb 前端（Vite） +- `5173`：gzweb 前端（Vite，仅当不使用 `--no-gzweb-vite`） +- `8000`：foundation gzweb（`--no-gzweb-vite` 时使用） - `9002`：Gazebo WebSocket +- `3000`：后端 API（如启用） 浏览器访问： -- [http://localhost:5173](http://localhost:5173) +- 本地开发：[http://localhost:5173](http://localhost:5173) +- 镜像内 foundation：[http://localhost:8000](http://localhost:8000) --- @@ -82,7 +102,30 @@ cd /home/xh2204/office_gzweb/vlm_office-main - `turtlebot3_simulations/turtlebot3_gazebo/map/office_map.yaml` - `turtlebot3_simulations/turtlebot3_gazebo/map/office_map.pgm` -`run_all.sh` 保存完成后会在终端打印绝对路径。 +每次循环结束时自动覆盖保存最新地图。 + +--- + +## 视觉检测输出 + +YOLO 检测结果保存在 `/workspace/vision_output/YYYYMMDD_HHMMSS/` 目录下： + +- `frame_0001.jpg` — 原始相机图像 +- `frame_0001_annotated.jpg` — 带检测框和中文标签的图像 +- `/workspace/camera_latest.jpg` — 最新一帧快速查看 + +日志输出示例： + +``` +[vision] 第42帧 | 位置(1.23, 0.87, 0.15rad) | 检测到 3 个目标: 椅子(87%), 沙发(92%), 人(76%) +[vision] 第43帧 | 位置(1.25, 0.89, 0.16rad) | 未检测到目标 +``` + +实时查看检测日志： + +```bash +tail -f /workspace/logs/run_all_*/D_vision_yolo.log +``` --- @@ -91,8 +134,8 @@ cd /home/xh2204/office_gzweb/vlm_office-main ### `run_all.sh` ```bash -./run_all.sh mapping [--explore] [--gzweb] [--no-rviz] [--world PATH] -./run_all.sh localization [--gzweb] [--no-rviz] [--map PATH] [--world PATH] +./run_all.sh mapping [--explore] [--vision] [--gzweb] [--no-gzweb-vite] [--no-rviz] [--world PATH] +./run_all.sh localization [--gzweb] [--no-gzweb-vite] [--no-rviz] [--map PATH] [--world PATH] ``` 可配置环境变量： @@ -104,32 +147,57 @@ cd /home/xh2204/office_gzweb/vlm_office-main --- -## 关键实现（重要代码点） +## 关键实现 -### 1) 仿真/导航/探索总控 +### 1) 仿真/导航/探索/视觉总控 - `run_all.sh` - - 统一启动 A(sim)、B(nav2)、C(explore)、D(websocket)、E(gzweb 前端) + - 统一启动 A(sim)、B(nav2)、C(explore)、D(websocket)、E(gzweb 前端)、D_vision_yolo - 自动端口检查与冲突清理（`5173/9002`） - 探索阶段终端计时输出（explore elapsed） - - 探索结束自动保存地图并退出 + - **循环模式**：探索结束自动保存地图 → 等待 30 秒 → 重新启动 explore_lite + - 通过 `/workspace/.run_all_stop` 文件接收停止信号 ### 2) Gazebo + ROS 集成启动 - `turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_office_gz.launch.py` - office world 启动 - - `/clock`、`/scan`、`/odom`、`/tf` 等桥接 + - `/clock`、`/scan`、`/odom`、`/tf`、`/camera/image_raw` 等桥接 - SLAM 与 RViz 延迟启动控制 - `GZ_PARTITION` 隔离避免串到其他 Gazebo 会话 -### 3) gzweb 纹理与材质修复 +### 3) YOLO 视觉检测 + +- `src/vision_yolo/vision_yolo/detector.py` + - YOLOv8n 实时推理，订阅 `/camera/image_raw` + - 推理节流：1 秒/帧（降低 CPU 占用，避免影响 Nav2） + - 分辨率 640×480（比 320×240 检测效果更好） + - 订阅 `/odom` 获取机器人实时位姿 + - **中文标签输出**：COCO 80 类中英文映射 + - **图像归档**：每帧保存原始图 + 标注图到时间戳目录 + - 发布 `/vision/detections`（Detection2DArray）和 `/vision/image_annotated`（Image） + +### 4) Nav2 速度优化 + +- `nav2_params_fast.yaml` + - `max_vel_x: 0.5`（默认 0.26） + - `max_speed_xy: 0.5` + - 加速办公室场景探索 + +### 5) explore_lite 调优 + +- `m-explore-ros2/explore/config/params.yaml` + - `planner_frequency: 1.0`（默认 0.2，提高响应速度） + - `min_frontier_size: 0.3`（默认 0.5，更容易发现小 frontier） + +### 6) gzweb 纹理与材质修复 - `gzweb/gzweb/include/ColladaLoader.js` - 修复 `RGBFormat is not defined` 问题 - - 修复 `model://` 纹理 URI 拼接错误（绝对 URI 不再错误拼接） + - 修复 `model://` 纹理 URI 拼接错误 - 重新构建输出到 `gzweb/gzweb/dist/` -### 4) 模型颜色兜底 +### 7) 模型颜色兜底 - `office_desk/model.sdf` - `office_chair/model.sdf` @@ -141,10 +209,12 @@ cd /home/xh2204/office_gzweb/vlm_office-main ## 典型问题与处理 -- gzweb 无法连接：检查 `9002` 是否被占用，`run_all.sh` 会自动清理并重启 -- 前端打不开：检查 `5173`，并查看 `logs/run_all_*/E_gzweb_frontend.log` -- 纹理丢失/颜色异常：检查 `ColladaLoader.js` 与 `GZ_SIM_RESOURCE_PATH` -- 探索长期不结束：`EXPLORE_WAIT_TIMEOUT` 超时后自动强制收尾并保存地图 +- **gzweb 无法连接**：检查 `9002` 是否被占用，`run_all.sh` 会自动清理并重启 +- **前端打不开**：检查 `5173`/`8000`，并查看 `logs/run_all_*/E_gzweb_frontend.log` +- **纹理丢失/颜色异常**：检查 `ColladaLoader.js` 与 `GZ_SIM_RESOURCE_PATH` +- **探索长期不结束**：`EXPLORE_WAIT_TIMEOUT` 超时后自动强制收尾并保存地图，30 秒后重启 +- **YOLO 误识别多**：预训练模型基于 COCO 真实照片，Gazebo 3D 渲染场景存在 domain gap，属于正常现象 +- **机器人不动**：Nav2 生命周期初始化需要 20-40 秒，请耐心等待 --- @@ -158,18 +228,28 @@ cd /home/xh2204/office_gzweb/vlm_office-main - `C_explore.log` - `D_websocket.log` - `E_gzweb_frontend.log` + - `D_vision_yolo.log` --- ## 快速命令参考 ```bash -# 启动（推荐） -./run_all.sh mapping --explore --gzweb +# 启动（推荐，循环模式） +./autorun.sh -# 强制停止全部 +# 停止全部 ./run_all_stop.sh -# 仅 A+B（不含 explore/gzweb） +# 仅 A+B（不含 explore/vision/gzweb） ./run_ab.sh mapping -``` \ No newline at end of file + +# 实时查看视觉检测日志 + tail -f /workspace/logs/run_all_*/D_vision_yolo.log + +# 查看最新相机图像 + ls -lh /workspace/camera_latest.jpg + +# 查看视觉归档目录 + ls -la /workspace/vision_output/ +``` diff --git a/src/vision_yolo/vision_yolo/detector.py b/src/vision_yolo/vision_yolo/detector.py index 24eba8e..04c4431 100644 --- a/src/vision_yolo/vision_yolo/detector.py +++ b/src/vision_yolo/vision_yolo/detector.py @@ -2,18 +2,57 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import Image from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithPose +from nav_msgs.msg import Odometry from cv_bridge import CvBridge from ultralytics import YOLO import cv2 import time +import os +from datetime import datetime + +# COCO 80 类中文映射（YOLOv8n 使用） +COCO_CN = { + "person": "人", "bicycle": "自行车", "car": "汽车", "motorcycle": "摩托车", + "airplane": "飞机", "bus": "巴士", "train": "火车", "truck": "卡车", + "boat": "船", "traffic light": "红绿灯", "fire hydrant": "消防栓", + "stop sign": "停车标志", "parking meter": "停车计时器", "bench": "长椅", + "bird": "鸟", "cat": "猫", "dog": "狗", "horse": "马", "sheep": "羊", + "cow": "牛", "elephant": "大象", "bear": "熊", "zebra": "斑马", + "giraffe": "长颈鹿", "backpack": "背包", "umbrella": "雨伞", + "handbag": "手提包", "tie": "领带", "suitcase": "行李箱", + "frisbee": "飞盘", "skis": "滑雪板", "snowboard": "滑雪板", + "sports ball": "运动球", "kite": "风筝", "baseball bat": "棒球棒", + "baseball glove": "棒球手套", "skateboard": "滑板", "surfboard": "冲浪板", + "tennis racket": "网球拍", "bottle": "瓶子", "wine glass": "酒杯", + "cup": "杯子", "fork": "叉子", "knife": "刀", "spoon": "勺子", + "bowl": "碗", "banana": "香蕉", "apple": "苹果", "sandwich": "三明治", + "orange": "橙子", "broccoli": "西兰花", "carrot": "胡萝卜", + "hot dog": "热狗", "pizza": "披萨", "donut": "甜甜圈", "cake": "蛋糕", + "chair": "椅子", "couch": "沙发", "potted plant": "盆栽", + "bed": "床", "dining table": "餐桌", "toilet": "马桶", + "tv": "电视", "laptop": "笔记本", "mouse": "鼠标", "remote": "遥控器", + "keyboard": "键盘", "cell phone": "手机", "microwave": "微波炉", + "oven": "烤箱", "toaster": "烤面包机", "sink": "水槽", + "refrigerator": "冰箱", "book": "书", "clock": "时钟", "vase": "花瓶", + "scissors": "剪刀", "teddy bear": "泰迪熊", "hair drier": "吹风机", + "toothbrush": "牙刷", +} + + +def euler_from_quaternion(qx, qy, qz, qw): + """从四元数提取 yaw（偏航角）。""" + siny_cosp = 2.0 * (qw * qz + qx * qy) + cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) + return time.math.atan2(siny_cosp, cosy_cosp) if hasattr(time, "math") else 0.0 + class YoloDetector(Node): def __init__(self): super().__init__("yolo_detector") self.bridge = CvBridge() - self.get_logger().info("Loading YOLOv8n model...") + self.get_logger().info("[vision] 正在加载 YOLOv8n 模型...") self.model = YOLO("yolov8n.pt") - self.get_logger().info("YOLOv8n model loaded") + self.get_logger().info("[vision] YOLOv8n 模型加载完成") self.sub = self.create_subscription( Image, "/camera/image_raw", self.on_image, 10) @@ -21,34 +60,75 @@ class YoloDetector(Node): Detection2DArray, "/vision/detections", 10) self.pub_vis = self.create_publisher( Image, "/vision/image_annotated", 10) + self.sub_odom = self.create_subscription( + Odometry, "/odom", self.on_odom, 10) self.last_infer_time = 0.0 self.infer_interval = 1.0 - self.get_logger().info("YOLO detector node started, subscribing to /camera/image_raw") + self.frame_counter = 0 + + # 当前机器人位姿 + self.robot_x = 0.0 + self.robot_y = 0.0 + self.robot_yaw = 0.0 + self.has_odom = False + + # 创建输出目录：/workspace/vision_output/YYYYMMDD_HHMMSS/ + self.output_dir = os.path.join( + "/workspace", "vision_output", + datetime.now().strftime("%Y%m%d_%H%M%S")) + os.makedirs(self.output_dir, exist_ok=True) + self.get_logger().info(f"[vision] 图像保存目录: {self.output_dir}") + + self.get_logger().info( + "[vision] YOLO 检测节点已启动，订阅 /camera/image_raw，" + "推理间隔 1.0 秒，分辨率 640x480") + + def on_odom(self, msg): + self.robot_x = msg.pose.pose.position.x + self.robot_y = msg.pose.pose.position.y + q = msg.pose.pose.orientation + # 计算 yaw + siny_cosp = 2.0 * (q.w * q.z + q.x * q.y) + cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) + import math + self.robot_yaw = math.atan2(siny_cosp, cosy_cosp) + self.has_odom = True def on_image(self, msg): now = time.time() if now - self.last_infer_time < self.infer_interval: return self.last_infer_time = now + self.frame_counter += 1 try: cv_img = self.bridge.imgmsg_to_cv2(msg, "bgr8") cv_img = cv2.resize(cv_img, (640, 480)) - cv2.imwrite("/workspace/camera_latest.jpg", cv_img) except Exception as e: - self.get_logger().error(f"cv_bridge failed: {e}") + self.get_logger().error(f"[vision] cv_bridge 转换失败: {e}") return + # 保存原始图像到时间戳目录 + raw_path = os.path.join(self.output_dir, f"frame_{self.frame_counter:04d}.jpg") + cv2.imwrite(raw_path, cv_img) + # 同时更新快速查看文件 + cv2.imwrite("/workspace/camera_latest.jpg", cv_img) + + # YOLO 推理 results = self.model(cv_img, verbose=False, imgsz=640) dets = Detection2DArray() dets.header = msg.header + detections_cn = [] # (中文标签, 置信度) + detections_en = [] # (英文标签, 置信度) + for r in results: for box in r.boxes: cls_id = int(box.cls[0]) conf = float(box.conf[0]) - label = self.model.names[cls_id] + label_en = self.model.names[cls_id] + label_cn = COCO_CN.get(label_en, label_en) x1, y1, x2, y2 = map(float, box.xyxy[0]) det = Detection2D() @@ -59,24 +139,48 @@ class YoloDetector(Node): det.bbox.size_y = y2 - y1 hyp = ObjectHypothesisWithPose() - hyp.hypothesis.class_id = label + hyp.hypothesis.class_id = label_en hyp.hypothesis.score = conf det.results.append(hyp) dets.detections.append(det) + detections_en.append((label_en, conf)) + detections_cn.append((label_cn, conf)) + + # 在图像上画框和标签（中文标签） cv2.rectangle(cv_img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2) - cv2.putText(cv_img, f"{label} {conf:.2f}", (int(x1), int(y1) - 10), + text = f"{label_cn} {conf:.2f}" + cv2.putText(cv_img, text, (int(x1), int(y1) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) self.pub.publish(dets) - if dets.detections: - classes = [d.results[0].hypothesis.class_id for d in dets.detections] - self.get_logger().info(f"Detected: {classes}") + # 保存标注图像 + ann_path = os.path.join( + self.output_dir, f"frame_{self.frame_counter:04d}_annotated.jpg") + cv2.imwrite(ann_path, cv_img) + + # 发布可视化图像 vis_msg = self.bridge.cv2_to_imgmsg(cv_img, "bgr8") vis_msg.header = msg.header self.pub_vis.publish(vis_msg) + # 构建中文日志 + pos_str = f"({self.robot_x:.2f}, {self.robot_y:.2f}, {self.robot_yaw:.2f}rad)" if self.has_odom else "(未知)" + if detections_cn: + items = [f"{label}({conf:.0%})" for label, conf in detections_cn] + log_msg = ( + f"[vision] 第{self.frame_counter}帧 | 位置{pos_str} | " + f"检测到 {len(items)} 个目标: {', '.join(items)}" + ) + else: + log_msg = ( + f"[vision] 第{self.frame_counter}帧 | 位置{pos_str} | " + f"未检测到目标" + ) + self.get_logger().info(log_msg) + + def main(args=None): rclpy.init(args=args) node = YoloDetector() @@ -87,5 +191,6 @@ def main(args=None): node.destroy_node() rclpy.shutdown() + if __name__ == "__main__": main()