diff --git a/utils/fisheye.py b/utils/fisheye.py new file mode 100644 index 0000000..4cc5167 --- /dev/null +++ b/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)}") \ No newline at end of file diff --git a/utils/image_raw.py b/utils/image_raw.py index d61fe44..a7b4de6 100755 --- a/utils/image_raw.py +++ b/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("异步扫描已经在运行中", "警告") - print('[ImageProcessor] scan,warn') + self.log.warning("异步扫描已经在运行中", "警告") + 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 码扫描线程", "启动") - print('[ImageProcessor] start async scan') + self.log.info("启动异步 QR 码扫描线程", "启动") + 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 码扫描线程已停止", "停止") - print('[ImageProcessor] stop async scan') + self.log.info("异步 QR 码扫描线程已停止", "停止") + 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}", "扫描") - print(f"[ImageProcessor] 异步扫描到 QR 码: {qr_data}") + self.log.success(f"异步扫描到 QR 码: {qr_data}", "扫描") + print(f"异步扫描到 QR 码: {qr_data}") except Exception as e: self.is_scanning = False - # self.log.error(f"异步 QR 码扫描出错: {e}", "错误") - print(f"[ImageProcessor] 异步 QR 码扫描出错: {e}") + self.log.error(f"异步 QR 码扫描出错: {e}", "错误") + print(f"异步 QR 码扫描出错: {e}") else: - print('[ImageProcessor] no img') + print('no img') last_scan_time = current_time