diff --git a/.gitignore b/.gitignore index ed514b8..eb9b2f2 100644 --- a/.gitignore +++ b/.gitignore @@ -13,5 +13,3 @@ __pycache__/ *.pyc *.pyo -# YOLO model (auto-downloaded on first run) -yolov8n.pt diff --git a/run_all.sh b/run_all.sh index d65843e..d713d30 100755 --- a/run_all.sh +++ b/run_all.sh @@ -1,14 +1,13 @@ -#!/usr/bin/env bash +#!/bin/bash set -euo pipefail -export PYTHONPATH="/workspace/.site-packages:$PYTHONPATH" -export DISPLAY=:1 - -# Auto-download YOLO model if not present -if [[ ! -f "\${ROOT}/yolov8n.pt" ]]; then - echo "[run_all] Downloading yolov8n.pt..." - wget -q -O "\${ROOT}/yolov8n.pt" https://github.com/ultralytics/assets/releases/download/v8.3.0/yolov8n.pt || \ curl -sL -o "\${ROOT}/yolov8n.pt" https://github.com/ultralytics/assets/releases/download/v8.3.0/yolov8n.pt +# Link pip deps from EFS vendor if /workspace/.site-packages is missing +ROOT="/workspace/" +if [[ ! -d "${ROOT}/.site-packages" && -d "/opt/vendor/yolov8" ]]; then + ln -sf /opt/vendor/yolov8 "${ROOT}/.site-packages" fi +export PYTHONPATH="${ROOT}/.site-packages:${PYTHONPATH:-}" +export DISPLAY=:1 # Unified launcher for TurtleBot3 office workflow: # - Terminal A: simulation / bridge / (optional SLAM) / RViz @@ -123,6 +122,7 @@ start_bg() { cleanup() { echo "[run_all] stopping all processes..." + rm -f /workspace/.run_all_stop for pid in "${PIDS[@]:-}"; do kill "$pid" 2>/dev/null || true done @@ -328,25 +328,49 @@ if [[ "$MODE" == "mapping" ]]; then fi if [[ "${WITH_EXPLORE}" == true && -n "${EXPLORE_PID:-}" ]]; then - echo "[run_all] exploration is running (pid=${EXPLORE_PID}), waiting for stop signal (timeout=${EXPLORE_WAIT_TIMEOUT}s)..." - if wait_for_explore_stop_log_with_timer "${EXPLORE_LOG}" "${EXPLORE_WAIT_TIMEOUT}"; then - echo "[run_all] detected exploration stop from log, finalizing explore process..." - 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 - echo "[run_all] explore_lite stopped, now saving map." + # Loop mode: explore -> save map -> wait -> restart explore (runs forever until .run_all_stop) + while true; do + if [[ -f /workspace/.run_all_stop ]]; then + echo "[run_all] stop signal detected, exiting exploration loop." + rm -f /workspace/.run_all_stop + break + fi + + echo "[run_all] exploration is running (pid=${EXPLORE_PID}), waiting for stop signal (timeout=${EXPLORE_WAIT_TIMEOUT}s)..." + if wait_for_explore_stop_log_with_timer "${EXPLORE_LOG}" "${EXPLORE_WAIT_TIMEOUT}"; then + echo "[run_all] detected exploration stop from log, finalizing explore process..." + 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 + echo "[run_all] explore_lite stopped, now saving map." + + echo "[run_all] auto-saving map to ${MAP_BASENAME}.yaml/.pgm ..." + ros2 run nav2_map_server map_saver_cli -f "$MAP_BASENAME" --ros-args -p use_sim_time:="$USE_SIM_TIME" + echo "[run_all] map saved successfully:" + echo " - ${MAP_BASENAME}.yaml" + echo " - ${MAP_BASENAME}.pgm" + + if [[ -f /workspace/.run_all_stop ]]; then + echo "[run_all] stop signal detected, exiting exploration loop." + rm -f /workspace/.run_all_stop + break + fi + + echo "[run_all] waiting 30s before restarting exploration (loop mode)..." + sleep 30 + + start_bg "C_explore" ros2 launch explore_lite explore.launch.py use_sim_time:="$USE_SIM_TIME" + last_index=$(( ${#PIDS[@]} - 1 )) + EXPLORE_PID="${PIDS[$last_index]}" + EXPLORE_LOG="${LOG_DIR}/C_explore.log" + echo "[run_all] exploration restarted (pid=${EXPLORE_PID})." + done fi if [[ "${WITH_EXPLORE}" == true ]]; then - echo "[run_all] auto-saving map to ${MAP_BASENAME}.yaml/.pgm ..." - ros2 run nav2_map_server map_saver_cli -f "$MAP_BASENAME" --ros-args -p use_sim_time:="$USE_SIM_TIME" - echo "[run_all] map saved successfully:" - echo " - ${MAP_BASENAME}.yaml" - echo " - ${MAP_BASENAME}.pgm" - echo "[run_all] mapping workflow completed, exiting now." - exit 0 + echo "[run_all] exploration loop ended. Final map saved." fi cat </dev/null || true -pkill -f "nav2_bringup navigation_launch.py" 2>/dev/null || true -pkill -f "nav2_bringup bringup_launch.py" 2>/dev/null || true -pkill -f "explore_lite explore.launch.py" 2>/dev/null || true -pkill -f "nav2_map_server map_saver_cli" 2>/dev/null || true - -# Gazebo websocket and frontend -pkill -f "${ROOT}/websocket.sdf" 2>/dev/null || true -pkill -f "vite --host 0.0.0.0 --port 5173" 2>/dev/null || true -pkill -f "npm run dev:host" 2>/dev/null || true - -# Gazebo processes usually started by ros_gz_sim / gz sim -pkill -f "ros_gz_sim" 2>/dev/null || true -pkill -f "gz sim" 2>/dev/null || true - -# Free common ports if still occupied -for port in 5173 9002; do - pids="$(ss -ltnp 2>/dev/null | grep ":${port} " | sed -E 's/.*pid=([0-9]+).*/\1/' | sort -u)" - if [[ -n "${pids}" ]]; then - echo "[run_all_stop] killing processes on port ${port}: ${pids}" - kill ${pids} 2>/dev/null || true - fi -done - -echo "[run_all_stop] done." +# Also send SIGTERM to run_all.sh directly (excluding this script) +PIDS=$(ps aux | grep -E "run_all\.sh" | grep -v grep | grep -v run_all_stop | awk '{print $2}' || true) +if [[ -n "$PIDS" ]]; then + echo "[run_all_stop] sending SIGTERM to run_all.sh pids: $PIDS" + kill $PIDS 2>/dev/null || true +fi diff --git a/src/vision_yolo/vision_yolo/detector.py b/src/vision_yolo/vision_yolo/detector.py index 35a79ad..24eba8e 100644 --- a/src/vision_yolo/vision_yolo/detector.py +++ b/src/vision_yolo/vision_yolo/detector.py @@ -5,34 +5,42 @@ from vision_msgs.msg import Detection2DArray, Detection2D, ObjectHypothesisWithP from cv_bridge import CvBridge from ultralytics import YOLO import cv2 +import time class YoloDetector(Node): def __init__(self): - super().__init__('yolo_detector') + super().__init__("yolo_detector") self.bridge = CvBridge() - self.get_logger().info('Loading YOLOv8n model...') - self.model = YOLO('yolov8n.pt') - self.get_logger().info('YOLOv8n model loaded') + self.get_logger().info("Loading YOLOv8n model...") + self.model = YOLO("yolov8n.pt") + self.get_logger().info("YOLOv8n model loaded") self.sub = self.create_subscription( - Image, '/camera/image_raw', self.on_image, 10) + Image, "/camera/image_raw", self.on_image, 10) self.pub = self.create_publisher( - Detection2DArray, '/vision/detections', 10) + Detection2DArray, "/vision/detections", 10) self.pub_vis = self.create_publisher( - Image, '/vision/image_annotated', 10) + Image, "/vision/image_annotated", 10) - self.get_logger().info('YOLO detector node started, subscribing to /camera/image_raw') + self.last_infer_time = 0.0 + self.infer_interval = 1.0 + self.get_logger().info("YOLO detector node started, subscribing to /camera/image_raw") def on_image(self, msg): + now = time.time() + if now - self.last_infer_time < self.infer_interval: + return + self.last_infer_time = now + try: - cv_img = self.bridge.imgmsg_to_cv2(msg, 'bgr8') - # CPU 降采样加速 - cv_img = cv2.resize(cv_img, (320, 240)) + 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"cv_bridge failed: {e}") return - results = self.model(cv_img, verbose=False, imgsz=320) + results = self.model(cv_img, verbose=False, imgsz=640) dets = Detection2DArray() dets.header = msg.header @@ -41,7 +49,6 @@ class YoloDetector(Node): cls_id = int(box.cls[0]) conf = float(box.conf[0]) label = self.model.names[cls_id] - x1, y1, x2, y2 = map(float, box.xyxy[0]) det = Detection2D() @@ -58,14 +65,15 @@ class YoloDetector(Node): dets.detections.append(det) 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), + cv2.putText(cv_img, f"{label} {conf:.2f}", (int(x1), int(y1) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) self.pub.publish(dets) if dets.detections: - self.get_logger().info(f'Detected: {[d.results[0].hypothesis.class_id for d in dets.detections]}') + classes = [d.results[0].hypothesis.class_id for d in dets.detections] + self.get_logger().info(f"Detected: {classes}") - vis_msg = self.bridge.cv2_to_imgmsg(cv_img, 'bgr8') + vis_msg = self.bridge.cv2_to_imgmsg(cv_img, "bgr8") vis_msg.header = msg.header self.pub_vis.publish(vis_msg) @@ -79,5 +87,5 @@ def main(args=None): node.destroy_node() rclpy.shutdown() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/yolov8n.pt b/yolov8n.pt new file mode 100644 index 0000000..0db4ca4 Binary files /dev/null and b/yolov8n.pt differ