feat: detector 中文日志 + 图像归档 + 机器人位姿输出; 更新 README.md

2026-05-07 17:12:28 +08:00 · 2026-05-07 17:12:28 +08:00 · 5487a0a2ad
parent 870a5b38ea
commit 5487a0a2ad
2 changed files with 232 additions and 47 deletions
--- 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. 自动停止全部进程并退出
+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
+
+# 实时查看视觉检测日志
+ tail -f /workspace/logs/run_all_*/D_vision_yolo.log
+
+# 查看最新相机图像
+ ls -lh /workspace/camera_latest.jpg
+
+# 查看视觉归档目录
+ ls -la /workspace/vision_output/
 ```
--- 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()