import os import threading import config from camera_manager import camera_manager from laser_manager import laser_manager from logger_manager import logger_manager from network import network_manager from triangle_target import load_camera_from_xml, load_triangle_positions, try_triangle_scoring from vision import estimate_distance, detect_circle_v3, enqueue_save_shot from maix import image, time # 缓存相机标定与三角形位置，避免每次射箭重复读磁盘 _tri_calib_cache = None def _get_triangle_calib(): """返回 (K, dist, marker_positions)；首次调用时从磁盘加载并缓存。""" global _tri_calib_cache if _tri_calib_cache is not None: return _tri_calib_cache calib_path = getattr(config, "CAMERA_CALIB_XML", "") tri_json = getattr(config, "TRIANGLE_POSITIONS_JSON", "") if not (os.path.isfile(calib_path) and os.path.isfile(tri_json)): _tri_calib_cache = (None, None, None) return _tri_calib_cache K, dist = load_camera_from_xml(calib_path) pos = load_triangle_positions(tri_json) _tri_calib_cache = (K, dist, pos) return _tri_calib_cache def preload_triangle_calib(): """ 启动阶段预加载三角形标定与坐标文件，避免首次射箭触发时的读盘/解析开销。 """ try: _get_triangle_calib() except Exception: # 预加载失败不影响主流程；射箭时会再次按需尝试 pass def analyze_shot(frame, laser_point=None): """ 分析射箭结果（算法部分，可迁移到C++） :param frame: 图像帧 :param laser_point: 激光点坐标 (x, y) :return: 包含分析结果的字典 优先级： 1. 三角形单应性（USE_TRIANGLE_OFFSET=True 时）— 成功则直接返回，跳过圆形检测 2. 圆形检测（三角形不可用或识别失败时兜底） """ logger = logger_manager.logger from datetime import datetime # ── Step 1: 确定激光点 ──────────────────────────────────────────────────── laser_point_method = None distance_m_first = None if config.HARDCODE_LASER_POINT: laser_point = laser_manager.laser_point laser_point_method = "hardcode" elif laser_manager.has_calibrated_point(): laser_point = laser_manager.laser_point laser_point_method = "calibrated" if logger: logger.info(f"[算法] 使用校准值: {laser_manager.laser_point}") else: # 动态模式：先做一次无激光点检测以估算距离，再推算激光点 _, _, _, _, best_radius1_temp, _ = detect_circle_v3(frame, None) distance_m_first = estimate_distance(best_radius1_temp) if best_radius1_temp else None if distance_m_first and distance_m_first > 0: laser_point = laser_manager.calculate_laser_point_from_distance(distance_m_first) laser_point_method = "dynamic" if logger: logger.info(f"[算法] 使用比例尺: {laser_point}") else: laser_point = laser_manager.laser_point laser_point_method = "default" if logger: logger.info(f"[算法] 使用默认值: {laser_point}") if laser_point is None: return {"success": False, "reason": "laser_point_not_initialized"} x, y = laser_point # ── Step 2: 提前转换一次图像，两个检测线程共享（只读）──────────────────────── img_cv = image.image2cv(frame, False, False) # ── Step 3: 检查三角形是否可用 ──────────────────────────────────────────────── use_tri = getattr(config, "USE_TRIANGLE_OFFSET", False) K = dist_coef = pos = None if use_tri: K, dist_coef, pos = _get_triangle_calib() use_tri = K is not None and dist_coef is not None and pos def _build_circle_result(cdata): """从圆形检测结果构建 analyze_shot 返回值。""" r_img, center, radius, method, best_radius1, ellipse_params = cdata dx, dy = None, None d_m = distance_m_first if center and radius: dx, dy = laser_manager.compute_laser_position(center, (x, y), radius, method) d_m = estimate_distance(best_radius1) if best_radius1 else distance_m_first return { "success": True, "result_img": r_img, "center": center, "radius": radius, "method": method, "best_radius1": best_radius1, "ellipse_params": ellipse_params, "dx": dx, "dy": dy, "distance_m": d_m, "laser_point": laser_point, "laser_point_method": laser_point_method, "offset_method": "yellow_ellipse" if ellipse_params else "yellow_circle", "distance_method": "yellow_radius", } if not use_tri: # 三角形未配置，直接跑圆形检测 return _build_circle_result( detect_circle_v3(frame, laser_point, img_cv=img_cv) ) # ── Step 4: 三角形 + 圆形并行检测 ───────────────────────────────────────────── # 两个线程共享只读的 img_cv，互不干扰 tri_result = {} circle_result = {} def _run_triangle(): try: logger.info(f"[TRI] begin {datetime.now()}") logger.info(f"[TRI] K: {K}, dist: {dist_coef}, pos: {pos}, {datetime.now()}") tri = try_triangle_scoring( img_cv, (x, y), pos, K, dist_coef, size_range=getattr(config, "TRIANGLE_SIZE_RANGE", (8, 500)), ) logger.info(f"[TRI] tri: {tri}, {datetime.now()}") tri_result['data'] = tri except Exception as e: logger.error(f"[TRI] 三角形路径异常: {e}") tri_result['data'] = {'ok': False} def _run_circle(): try: circle_result['data'] = detect_circle_v3(frame, laser_point, img_cv=img_cv) except Exception as e: logger.error(f"[CIRCLE] 圆形检测异常: {e}") circle_result['data'] = (frame, None, None, None, None, None) t_tri = threading.Thread(target=_run_triangle, daemon=True) t_cir = threading.Thread(target=_run_circle, daemon=True) t_tri.start() t_cir.start() # 最多等待三角形 TRIANGLE_TIMEOUT_MS（默认 1000ms） tri_timeout_s = float(getattr(config, "TRIANGLE_TIMEOUT_MS", 1000)) / 1000.0 t_tri.join(timeout=tri_timeout_s) if t_tri.is_alive(): # 超时：直接放弃三角形结果，回退圆心（圆心线程通常已跑完） logger.warning(f"[TRI] timeout>{tri_timeout_s:.2f}s，回退圆心算法") t_cir.join() return _build_circle_result( circle_result.get('data') or (frame, None, None, None, None, None) ) tri = tri_result.get('data', {}) if tri.get('ok'): logger.info(f"[TRI] end {datetime.now()}") return { "success": True, "result_img": frame, "center": None, "radius": None, "method": tri.get("offset_method") or "triangle_homography", "best_radius1": None, "ellipse_params": None, "dx": tri["dx_cm"], "dy": tri["dy_cm"], "distance_m": tri.get("distance_m") or distance_m_first, "laser_point": laser_point, "laser_point_method": laser_point_method, "offset_method": tri.get("offset_method") or "triangle_homography", "distance_method": tri.get("distance_method") or "pnp_triangle", "tri_markers": tri.get("markers", []), "tri_homography": tri.get("homography"), } # 三角形失败，等圆形结果（已并行跑完，几乎无额外等待） t_cir.join() logger.info(f"[TRI] end(fallback) {datetime.now()}") return _build_circle_result( circle_result.get('data') or (frame, None, None, None, None, None) ) def process_shot(adc_val): """ 处理射箭事件（逻辑控制部分） :param adc_val: ADC触发值 :return: None """ logger = logger_manager.logger try: network_manager.safe_enqueue({"shoot_event": "start"}, msg_type=2, high=True) frame = camera_manager.read_frame() # 调用算法分析 analysis_result = analyze_shot(frame) if not analysis_result.get("success"): reason = analysis_result.get("reason", "unknown") if logger: logger.warning(f"[MAIN] 射箭分析失败: {reason}") time.sleep_ms(100) return # 提取分析结果 result_img = analysis_result["result_img"] center = analysis_result["center"] radius = analysis_result["radius"] method = analysis_result["method"] ellipse_params = analysis_result["ellipse_params"] dx = analysis_result["dx"] dy = analysis_result["dy"] distance_m = analysis_result["distance_m"] laser_point = analysis_result["laser_point"] laser_point_method = analysis_result["laser_point_method"] offset_method = analysis_result.get("offset_method", "yellow_circle") distance_method = analysis_result.get("distance_method", "yellow_radius") tri_markers = analysis_result.get("tri_markers", []) tri_homography = analysis_result.get("tri_homography") x, y = laser_point # 三角形路径成功时 center/radius 为空是正常的；此时用 triangle 方法名用于保存文件名与上报字段 m if (not method) and tri_markers: method = offset_method or "triangle_homography" if config.SHOW_CAMERA_PHOTO_WHILE_SHOOTING: camera_manager.show(result_img) if dx is None and dy is None and logger: logger.warning("[MAIN] 未检测到偏移量（三角形与圆形均失败），但会保存图像") # 生成射箭ID from shot_id_generator import shot_id_generator shot_id = shot_id_generator.generate_id() if logger: logger.info(f"[MAIN] 射箭ID: {shot_id}") laser_distance_m = None laser_signal_quality = 0 # x,y 单位：物理厘米（compute_laser_position 与三角形单应性均输出物理 cm） # 未检测到靶心时 x/y 用 200.0（脱靶标志） srv_x = round(float(dx), 4) if dx is not None else 200.0 srv_y = round(float(dy), 4) if dy is not None else 200.0 # 构造上报数据 inner_data = { "shot_id": shot_id, "x": srv_x, "y": srv_y, "r": 20.0, # 保留字段（服务端当前忽略，物理外环半径 cm） "d": round((distance_m or 0.0) * 100), "d_laser": round((laser_distance_m or 0.0) * 100), "d_laser_quality": laser_signal_quality, "m": method if method else "no_target", "adc": adc_val, "laser_method": laser_point_method, "target_x": float(x), "target_y": float(y), "offset_method": offset_method, "distance_method": distance_method, } if ellipse_params: (ell_center, (width, height), angle) = ellipse_params inner_data["ellipse_major_axis"] = float(max(width, height)) inner_data["ellipse_minor_axis"] = float(min(width, height)) inner_data["ellipse_angle"] = float(angle) inner_data["ellipse_center_x"] = float(ell_center[0]) inner_data["ellipse_center_y"] = float(ell_center[1]) else: inner_data["ellipse_major_axis"] = None inner_data["ellipse_minor_axis"] = None inner_data["ellipse_angle"] = None inner_data["ellipse_center_x"] = None inner_data["ellipse_center_y"] = None report_data = {"cmd": 1, "data": inner_data} network_manager.safe_enqueue(report_data, msg_type=2, high=True) # 数据上报后再画标注，不干扰检测阶段的原始画面 if result_img is not None: # 1. 若有三角形标记，先用 cv2 画轮廓 / 顶点 / ID，再反推靶心位置 if tri_markers: import cv2 as _cv2 import numpy as _np _img_cv = image.image2cv(result_img, False, False) # 三角形轮廓 + 直角顶点 + ID for _m in tri_markers: _corners = _np.array(_m["corners"], dtype=_np.int32) _cv2.polylines(_img_cv, [_corners], True, (0, 255, 0), 2) _cx, _cy = int(_m["center"][0]), int(_m["center"][1]) _cv2.circle(_img_cv, (_cx, _cy), 4, (0, 0, 255), -1) _cv2.putText(_img_cv, f"T{_m['id']}", (_cx - 18, _cy - 12), _cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 0), 1) # 靶心（H_inv @ [0,0]）：小红圆 _center_px = None if tri_homography is not None: try: _H_inv = _np.linalg.inv(tri_homography) _c_img = _cv2.perspectiveTransform( _np.array([[[0.0, 0.0]]], dtype=_np.float32), _H_inv)[0][0] _ocx, _ocy = int(_c_img[0]), int(_c_img[1]) _cv2.circle(_img_cv, (_ocx, _ocy), 5, (0, 0, 255), -1) # 实心 _cv2.circle(_img_cv, (_ocx, _ocy), 9, (0, 0, 255), 1) # 外框 _center_px = (_ocx, _ocy) logger.info(f"[算法] 靶心: {_center_px}") except Exception: pass # 叠加信息：落点-圆心距离 / 相机-靶距离等 try: import math as _math _lines = [] if dx is not None and dy is not None: _r_cm = _math.hypot(float(dx), float(dy)) _lines.append(f"offset=({float(dx):.2f},{float(dy):.2f})cm |r|={_r_cm:.2f}cm") if distance_m is not None: _lines.append(f"cam_dist={float(distance_m):.2f}m ({distance_method})") if method: _lines.append(f"method={method}") if _lines: _y0 = 22 for i, _t in enumerate(_lines): _cv2.putText( _img_cv, _t, (10, _y0 + i * 18), _cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, ) except Exception: pass result_img = image.cv2image(_img_cv, False, False) # 2. 激光十字线 _lc = image.Color(config.LASER_COLOR[0], config.LASER_COLOR[1], config.LASER_COLOR[2]) result_img.draw_line(int(x - config.LASER_LENGTH), int(y), int(x + config.LASER_LENGTH), int(y), _lc, config.LASER_THICKNESS) result_img.draw_line(int(x), int(y - config.LASER_LENGTH), int(x), int(y + config.LASER_LENGTH), _lc, config.LASER_THICKNESS) result_img.draw_circle(int(x), int(y), 1, _lc, config.LASER_THICKNESS) # 闪一下激光（射箭反馈） if config.FLASH_LASER_WHILE_SHOOTING: laser_manager.flash_laser(config.FLASH_LASER_DURATION_MS) # 保存图像（异步队列，与 main.py 一致） enqueue_save_shot( result_img, center, radius, method, ellipse_params, (x, y), distance_m, shot_id=shot_id, photo_dir=config.PHOTO_DIR if config.SAVE_IMAGE_ENABLED else None, ) if logger: if dx is not None and dy is not None: logger.info(f"射箭事件已加入发送队列（偏移=({dx:.2f},{dy:.2f})cm），ID: {shot_id}") else: logger.info(f"射箭事件已加入发送队列（未检测到偏移，已保存图像），ID: {shot_id}") time.sleep_ms(100) except Exception as e: if logger: logger.error(f"[MAIN] 图像处理异常: {e}") import traceback logger.error(traceback.format_exc()) time.sleep_ms(100)