bug
/
office_gzweb
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: detector 中文日志 + 图像归档 + 机器人位姿; README; run_all.sh 循环模式 + 进程组清理

This commit is contained in:
Bug 2026-05-07 17:12:28 +08:00
parent 870a5b38ea
commit c209618f83
3 changed files with 238 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
README.md
View File

@ -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
```
# 实时查看视觉检测日志
tail -f /workspace/logs/run_all_*/D_vision_yolo.log
# 查看最新相机图像
ls -lh /workspace/camera_latest.jpg
# 查看视觉归档目录
ls -la /workspace/vision_output/
```

8
run_all.sh
View File

@ -342,8 +342,12 @@ if [[ "$MODE" == "mapping" ]]; then
else
echo "[run_all] WARN: explore stop not detected within timeout; forcing stop and saving current map."
fi
kill "${EXPLORE_PID}" 2>/dev/null || true
wait "${EXPLORE_PID}" 2>/dev/null || true
# 杀掉 explore_lite 整个进程组（包括 ros2 launch 的子进程）
EXPLORE_PGID=$(ps -o pgid= ${EXPLORE_PID} | tr -d ' ' 2>/dev/null || echo "")
if [[ -n "${EXPLORE_PGID}" ]]; then
kill -- -${EXPLORE_PGID} 2>/dev/null || true
fi
sleep 2
echo "[run_all] explore_lite stopped, now saving map."
echo "[run_all] auto-saving map to ${MAP_BASENAME}.yaml/.pgm ..."

127
src/vision_yolo/vision_yolo/detector.py
View File

@ -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()