Merge branch 'main-v2' of ssh://120.27.199.238:222/Havoc420mac/mi-task into main-v2

utils/fisheye.py

@@ -0,0 +1,119 @@
+import cv2
+import numpy as np
+import os
+
+def detect_yellow_distance_from_bottom(image_path, visualize=False):
+    """
+    检测鱼眼图像中垂线上最靠近下方的黄点到图像底部的距离
+    
+    参数:
+        image_path: 图像路径
+        visualize: 是否显示检测过程可视化结果
+        
+    返回:
+        distance: 黄点到图像底部的距离(像素)
+        center_x: 黄点的x坐标(用于垂线参考)
+        mask: 黄色区域掩模(可视化时使用)
+    """
+    # 1. 读取图像并转换色彩空间
+    img = cv2.imread(image_path)
+    if img is None:
+        raise ValueError(f"无法读取图像，请检查路径: {image_path}")
+    
+    hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+    height, width = img.shape[:2]
+    
+    # 2. 定义黄色颜色范围 (考虑不同光照条件)
+    lower_yellow = np.array([20, 100, 100])
+    upper_yellow = np.array([30, 255, 255])
+    
+    # 3. 创建黄色区域掩模
+    mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
+    
+    # 4. 形态学处理去除噪声
+    kernel = np.ones((5,5), np.uint8)
+    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
+    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
+    
+    # 5. 寻找轮廓
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    
+    if not contours:
+        print("未检测到黄色区域")
+        return None, None, mask
+    
+    # 6. 找到所有黄色区域的中心点
+    yellow_points = []
+    for cnt in contours:
+        M = cv2.moments(cnt)
+        if M["m00"] > 100:  # 忽略太小的区域
+            cx = int(M["m10"] / M["m00"])
+            cy = int(M["m01"] / M["m00"])
+            yellow_points.append((cx, cy))
+    
+    if not yellow_points:
+        print("未找到有效的黄色中心点")
+        return None, None, mask
+    
+    # 7. 计算图像中心垂线 (考虑鱼眼畸变，使用图像中心作为参考)
+    center_x = width // 2
+    vertical_line_threshold = width * 0.1  # 垂线左右10%的容差范围
+    
+    # 8. 筛选在垂线附近的黄点
+    vertical_points = [p for p in yellow_points if abs(p[0] - center_x) < vertical_line_threshold]
+    
+    if not vertical_points:
+        # 如果没有完全垂直的点，选择最接近垂线的点
+        vertical_points = sorted(yellow_points, key=lambda p: abs(p[0] - center_x))[:1]
+        print(f"警告: 没有严格垂直的点，使用最接近垂线的点: {vertical_points[0]}")
+    
+    # 9. 找出最下方的黄点
+    lowest_point = max(vertical_points, key=lambda p: p[1])
+    
+    # 10. 计算到图像底部的距离
+    distance = height - lowest_point[1]
+    
+    # 可视化结果
+    if visualize:
+        vis = img.copy()
+        # 标记所有黄点
+        for (cx, cy) in yellow_points:
+            cv2.circle(vis, (cx, cy), 5, (0, 255, 255), -1)
+        # 标记垂线区域
+        cv2.line(vis, (center_x, 0), (center_x, height), (0, 255, 0), 1)
+        cv2.line(vis, (int(center_x - vertical_line_threshold), 0), 
+                (int(center_x - vertical_line_threshold), height), (0, 255, 0), 1)
+        cv2.line(vis, (int(center_x + vertical_line_threshold), 0), 
+                (int(center_x + vertical_line_threshold), height), (0, 255, 0), 1)
+        # 标记最低黄点
+        cv2.circle(vis, lowest_point, 10, (0, 0, 255), -1)
+        cv2.line(vis, (lowest_point[0], lowest_point[1]), 
+                (lowest_point[0], height), (0, 0, 255), 2)
+        cv2.putText(vis, f"Distance: {distance}px", (10, 30), 
+                   cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
+        
+        # 确保保存目录存在
+        os.makedirs("saved_images", exist_ok=True)
+        
+        # 保存原始图像和结果图像
+        cv2.imwrite("saved_images/Original_Image.jpg", img)
+        cv2.imwrite("saved_images/Detection_Result.jpg", vis)
+        
+    
+    return distance, center_x, mask
+
+# 使用示例
+try:
+    distance, center_x, _ = detect_yellow_distance_from_bottom(
+        "/home/mi-task/saved_images/right_20250820_120541_263023.jpg", 
+        visualize=True
+    )
+    
+    if distance is not None:
+        print(f"黄点到图像底部的距离: {distance} 像素")
+        print(f"参考垂线x坐标: {center_x}")
+    else:
+        print("未能检测到有效的黄色点")
+        
+except Exception as e:
+    print(f"发生错误: {str(e)}")

utils/image_raw.py

@@ -63,6 +63,7 @@ from threading import Thread, Lock
 import time
 import queue
 from datetime import datetime 
+from utils.log_helper import get_logger
 # 导入AI相机服务
 from protocol.srv import CameraService
 # qrcode
@@ -80,8 +81,7 @@ class ImageSubscriber(Node):
         # 创建服务客户端
         self.camera_client = self.create_client(CameraService, '/camera_service')
         while not self.camera_client.wait_for_service(timeout_sec=1.0):
-            print('waiting for camera service...')
+            self.get_logger().info('等待AI相机服务...')
-            # self.get_logger().info('等待AI相机服务...')
         
         # 图像订阅
         self.image_sub = self.create_subscription(
@@ -137,14 +137,14 @@ class ImageSubscriber(Node):
         rclpy.spin_until_future_complete(self, future)
         
         result = future.result()
-        print(f'服务返回: [code={result.result}, msg="{result.msg}"]')
+        self.get_logger().info(f'服务返回: [code={result.result}, msg="{result.msg}"]')
         
         if result.result == 0:
-            print('相机启动成功')
+            self.get_logger().info('相机启动成功')
             self.camera_started = True
             return True
         else:
-            print(f'启动失败 (错误码 {result.result})')
+            self.get_logger().error(f'启动失败 (错误码 {result.result})')
             return False
 
     def stop_camera(self):
@@ -160,14 +160,14 @@ class ImageSubscriber(Node):
             rclpy.spin_until_future_complete(self, future, timeout_sec=2.0)
             
             if future.result().result == 0:
-                print('相机已停止')
+                self.get_logger().info('相机已停止')
                 self.camera_started = False
                 return True
             else:
-                print(f'停止失败: {future.result().msg}')
+                self.get_logger().error(f'停止失败: {future.result().msg}')
                 return False
         except Exception as e:
-            print(f'停止异常: {str(e)}')
+            self.get_logger().error(f'停止异常: {str(e)}')
             return False
 
     def save_image(self, image, prefix):
@@ -179,10 +179,10 @@ class ImageSubscriber(Node):
             timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")
             filename = f"{self.save_dir}/{prefix}_{timestamp}.jpg"
             cv2.imwrite(filename, image)
-            print(f"已保存 {prefix} 图像: {filename}")
+            # self.get_logger().info(f"已保存 {prefix} 图像: {filename}")
             return True
         except Exception as e:
-            print(f"保存{prefix}图像失败: {str(e)}")
+            self.get_logger().error(f"保存{prefix}图像失败: {str(e)}")
             return False
 
     def image_callback_rgb(self, msg):
@@ -200,7 +200,7 @@ class ImageSubscriber(Node):
                 self.save_image(self.cv_image_rgb, 'rgb')
                 self.last_save_time['rgb'] = time.time()
         except Exception as e:
-            print(f"RGB图像处理错误: {str(e)}")
+            self.get_logger().error(f"RGB图像处理错误: {str(e)}")
 
     def image_callback_left(self, msg):
         try:
@@ -209,7 +209,7 @@ class ImageSubscriber(Node):
                 self.save_image(self.cv_image_left, 'left')
                 self.last_save_time['left'] = time.time()
         except Exception as e:
-            print(f"左图像处理错误: {str(e)}")
+            self.get_logger().error(f"左图像处理错误: {str(e)}")
 
     def image_callback_right(self, msg):
         try:
@@ -218,7 +218,7 @@ class ImageSubscriber(Node):
                 self.save_image(self.cv_image_right, 'right')
                 self.last_save_time['right'] = time.time()
         except Exception as e:
-            print(f"右图像处理错误: {str(e)}")
+            self.get_logger().error(f"右图像处理错误: {str(e)}")
 
     def image_callback_ai(self, msg):
         try:
@@ -227,7 +227,7 @@ class ImageSubscriber(Node):
                 self.save_image(self.cv_image_ai, 'ai')
                 self.last_save_time['ai'] = time.time()
         except Exception as e:
-            print(f"ai图像处理错误: {str(e)}")
+            self.get_logger().error(f"ai图像处理错误: {str(e)}")
 
     def safe_spin(self):
         """安全spin循环"""
@@ -366,8 +366,8 @@ class ImageProcessor:
             interval: 扫描间隔，单位秒
         """
         if self.scan_thread is not None and self.scan_thread.is_alive():
-            # self.log.warning("异步扫描已经在运行中", "警告")
+            self.log.warning("异步扫描已经在运行中", "警告")
-            print('[ImageProcessor] scan,warn')
+            print('scan,warn')
             return
         
         self.enable_async_scan = True
@@ -375,16 +375,16 @@ class ImageProcessor:
         self.scan_thread = Thread(target=self._async_scan_worker, args=(interval,))
         self.scan_thread.daemon = True  # 设为守护线程，主线程结束时自动结束
         self.scan_thread.start()
-        # self.log.info("启动异步 QR 码扫描线程", "启动")
+        self.log.info("启动异步 QR 码扫描线程", "启动")
-        print('[ImageProcessor] start async scan')
+        print('start')
     
     def stop_async_scan(self):
         """停止异步 QR 码扫描"""
         self.enable_async_scan = False
         if self.scan_thread and self.scan_thread.is_alive():
             self.scan_thread.join(timeout=1.0)
-            # self.log.info("异步 QR 码扫描线程已停止", "停止")
+            self.log.info("异步 QR 码扫描线程已停止", "停止")
-            print('[ImageProcessor] stop async scan')
+            print('stop')
     
     def _async_scan_worker(self, interval):
         """异步扫描工作线程"""
@@ -400,21 +400,21 @@ class ImageProcessor:
                     try:
                         self.is_scanning = True
                         qr_data = self.decode_all_qrcodes(img)
-                        print(f"[ImageProcessor] 异步扫描到 QR 码: {qr_data}")
+                        print(qr_data)
                         self.is_scanning = False
                         
                         with self.scan_lock:
                             if qr_data:
                                 self.last_qr_result = qr_data
                                 self.last_qr_time = current_time
-                                # self.log.success(f"异步扫描到 QR 码: {qr_data}", "扫描")
+                                self.log.success(f"异步扫描到 QR 码: {qr_data}", "扫描")
-                                print(f"[ImageProcessor] 异步扫描到 QR 码: {qr_data}")
+                                print(f"异步扫描到 QR 码: {qr_data}")
                     except Exception as e:
                         self.is_scanning = False
-                        # self.log.error(f"异步 QR 码扫描出错: {e}", "错误")
+                        self.log.error(f"异步 QR 码扫描出错: {e}", "错误")
-                        print(f"[ImageProcessor] 异步 QR 码扫描出错: {e}")
+                        print(f"异步 QR 码扫描出错: {e}")
                 else:
-                    print('[ImageProcessor] no img')
+                    print('no img')
 
                 last_scan_time = current_time