1183 lines
54 KiB
Python
1183 lines
54 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
激光管理器模块
|
||
提供激光控制、校准等功能
|
||
"""
|
||
import json
|
||
import os
|
||
import binascii
|
||
from maix import time, camera
|
||
import threading
|
||
import config
|
||
from logger_manager import logger_manager
|
||
import vision
|
||
|
||
|
||
class LaserManager:
|
||
"""激光控制管理器（单例）"""
|
||
_instance = None
|
||
|
||
def __new__(cls):
|
||
if cls._instance is None:
|
||
cls._instance = super(LaserManager, cls).__new__(cls)
|
||
cls._instance._initialized = False
|
||
return cls._instance
|
||
|
||
def __init__(self):
|
||
if self._initialized:
|
||
return
|
||
|
||
# 私有状态
|
||
self._calibration_active = False
|
||
self._calibration_result = None
|
||
self._calibration_lock = threading.Lock()
|
||
self._laser_point = None
|
||
self._laser_turned_on = False
|
||
self._last_frame_with_ellipse = None # 保存绘制了椭圆的图像（用于调试/显示）
|
||
self._initialized = True
|
||
|
||
# ==================== 状态访问（只读属性）====================
|
||
|
||
@property
|
||
def logger(self):
|
||
"""获取 logger 对象"""
|
||
return logger_manager.logger
|
||
|
||
@property
|
||
def calibration_active(self):
|
||
"""是否正在校准"""
|
||
return self._calibration_active
|
||
|
||
@property
|
||
def laser_point(self):
|
||
"""当前激光点（如果启用硬编码，则返回硬编码值）"""
|
||
if config.HARDCODE_LASER_POINT:
|
||
return config.HARDCODE_LASER_POINT_VALUE
|
||
return self._laser_point
|
||
|
||
def get_last_frame_with_ellipse(self):
|
||
"""
|
||
获取最后一次查找激光点时绘制了椭圆的图像（如果启用椭圆绘制）
|
||
|
||
Returns:
|
||
MaixPy 图像对象，如果没有则返回 None
|
||
"""
|
||
return self._last_frame_with_ellipse
|
||
|
||
# ==================== 业务方法 ====================
|
||
|
||
def load_laser_point(self):
|
||
"""从配置文件加载激光中心点，失败则使用默认值
|
||
如果启用硬编码模式，则直接使用硬编码值
|
||
"""
|
||
if config.HARDCODE_LASER_POINT:
|
||
# 硬编码模式：直接使用硬编码值
|
||
self._laser_point = config.HARDCODE_LASER_POINT_VALUE
|
||
self.logger.info(f"[LASER] 使用硬编码激光点: {self._laser_point}")
|
||
return self._laser_point
|
||
|
||
# 正常模式：从配置文件加载
|
||
try:
|
||
if "laser_config.json" in os.listdir("/root"):
|
||
with open(config.CONFIG_FILE, "r") as f:
|
||
data = json.load(f)
|
||
if isinstance(data, list) and len(data) == 2:
|
||
self._laser_point = (int(data[0]), int(data[1]))
|
||
self.logger.debug(f"[INFO] 加载激光点: {self._laser_point}")
|
||
return self._laser_point
|
||
else:
|
||
raise ValueError
|
||
else:
|
||
self._laser_point = config.DEFAULT_LASER_POINT
|
||
except:
|
||
self._laser_point = config.DEFAULT_LASER_POINT
|
||
|
||
return self._laser_point
|
||
|
||
def save_laser_point(self, point):
|
||
"""保存激光中心点到配置文件
|
||
如果启用硬编码模式，则不保存（直接返回 True）
|
||
"""
|
||
if config.HARDCODE_LASER_POINT:
|
||
# 硬编码模式：不保存到文件，但更新内存中的值（虽然不会被使用）
|
||
self._laser_point = point
|
||
self.logger.info(f"[LASER] 硬编码模式已启用，跳过保存激光点: {point}")
|
||
return True
|
||
|
||
# 正常模式：保存到配置文件
|
||
try:
|
||
with open(config.CONFIG_FILE, "w") as f:
|
||
json.dump([point[0], point[1]], f)
|
||
self._laser_point = point
|
||
return True
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] 保存激光点失败: {e}")
|
||
return False
|
||
|
||
def turn_on_laser(self):
|
||
"""发送指令开启激光，并读取回包（部分模块支持）"""
|
||
from hardware import hardware_manager
|
||
|
||
if hardware_manager.distance_serial is None:
|
||
self.logger.error("[LASER] distance_serial 未初始化")
|
||
return None
|
||
|
||
# 打印调试信息
|
||
self.logger.info(f"[LASER] 发送开启命令: {config.LASER_ON_CMD.hex()}")
|
||
|
||
# 清空接收缓冲区
|
||
try:
|
||
hardware_manager.distance_serial.read(-1) # 清空缓冲区
|
||
except:
|
||
pass
|
||
|
||
# 发送命令
|
||
written = hardware_manager.distance_serial.write(config.LASER_ON_CMD)
|
||
self.logger.info(f"[LASER] 写入字节数: {written}")
|
||
|
||
# return None
|
||
|
||
# TODO: 暂时去掉这个等待
|
||
# 读取回包
|
||
resp = hardware_manager.distance_serial.read(len=20,timeout=10)
|
||
if resp:
|
||
self.logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||
if resp == config.LASER_ON_CMD:
|
||
self.logger.info("✅ 激光开启指令已确认")
|
||
else:
|
||
self.logger.warning("🔇 无回包（可能正常或模块不支持回包）")
|
||
return resp
|
||
|
||
def turn_off_laser(self):
|
||
"""发送指令关闭激光"""
|
||
from hardware import hardware_manager
|
||
|
||
if hardware_manager.distance_serial is None:
|
||
self.logger.error("[LASER] distance_serial 未初始化")
|
||
return None
|
||
|
||
# 打印调试信息
|
||
self.logger.info(f"[LASER] 发送关闭命令: {config.LASER_OFF_CMD.hex()}")
|
||
|
||
# 清空接收缓冲区
|
||
try:
|
||
hardware_manager.distance_serial.read(-1)
|
||
except:
|
||
pass
|
||
|
||
# 发送命令
|
||
written = hardware_manager.distance_serial.write(config.LASER_OFF_CMD)
|
||
self.logger.info(f"[LASER] 写入字节数: {written}")
|
||
|
||
|
||
# 读取回包
|
||
resp = hardware_manager.distance_serial.read(20)
|
||
if resp:
|
||
self.logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||
else:
|
||
self.logger.warning("🔇 无回包")
|
||
return resp
|
||
# 不用读回包
|
||
# return None
|
||
|
||
def flash_laser(self, duration_ms=1000):
|
||
"""闪一下激光（用于射箭反馈）"""
|
||
try:
|
||
self.turn_on_laser()
|
||
time.sleep_ms(duration_ms)
|
||
self.turn_off_laser()
|
||
except Exception as e:
|
||
self.logger.error(f"闪激光失败: {e}")
|
||
|
||
# def find_red_laser(self, frame, threshold=150):
|
||
# """在图像中查找最亮的红色激光点（基于 RGB 阈值）"""
|
||
# w, h = frame.width(), frame.height()
|
||
# img_bytes = frame.to_bytes()
|
||
# max_sum = 0
|
||
# best_pos = None
|
||
# for y in range(0, h, 2):
|
||
# for x in range(0, w, 2):
|
||
# idx = (y * w + x) * 3
|
||
# r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
|
||
# if r > threshold and r > g * 2 and r > b * 2:
|
||
# rgb_sum = r + g + b
|
||
# if rgb_sum > max_sum:
|
||
# max_sum = rgb_sum
|
||
# best_pos = (x, y)
|
||
# return best_pos
|
||
|
||
# def find_red_laser(self, frame, threshold=150, search_radius=50):
|
||
# """
|
||
# 在图像中心附近查找最亮的红色激光点（基于 RGB 阈值）
|
||
|
||
# Args:
|
||
# frame: 图像帧
|
||
# threshold: 红色通道阈值（默认150）
|
||
# search_radius: 搜索半径（像素），从图像中心开始搜索（默认150）
|
||
|
||
# Returns:
|
||
# (x, y) 坐标，如果未找到则返回 None
|
||
# """
|
||
# w, h = frame.width(), frame.height()
|
||
# center_x, center_y = w // 2, h // 2
|
||
|
||
# # 只在中心区域搜索
|
||
# x_min = max(0, center_x - search_radius)
|
||
# x_max = min(w, center_x + search_radius)
|
||
# y_min = max(0, center_y - search_radius)
|
||
# y_max = min(h, center_y + search_radius)
|
||
|
||
# img_bytes = frame.to_bytes()
|
||
# max_score = 0
|
||
# best_pos = None
|
||
|
||
# for y in range(y_min, y_max, 2):
|
||
# for x in range(x_min, x_max, 2):
|
||
# idx = (y * w + x) * 3
|
||
# r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
|
||
|
||
# # 判断是否为红色或过曝的红色（发白）
|
||
# is_red = False
|
||
# is_overexposed_red = False
|
||
|
||
# # 情况1：正常红色（r 明显大于 g 和 b）
|
||
# if r > threshold and r > g * 2 and r > b * 2:
|
||
# is_red = True
|
||
|
||
# # 情况2：过曝的红色（发白，r, g, b 都接近255，但 r 仍然最大）
|
||
# # 过曝时，r, g, b 都接近 255，但 r 应该仍然是最高的
|
||
# elif r > 200 and g > 200 and b > 200: # 接近白色
|
||
# if r >= g and r >= b and (r - g) > 10 and (r - b) > 10: # r 仍然明显最大
|
||
# is_overexposed_red = True
|
||
|
||
# if is_red or is_overexposed_red:
|
||
# # 计算得分：RGB 总和 + 距离中心权重（越靠近中心得分越高）
|
||
# rgb_sum = r + g + b
|
||
# # 计算到中心的距离
|
||
# dx = x - center_x
|
||
# dy = y - center_y
|
||
# distance_from_center = (dx * dx + dy * dy) ** 0.5
|
||
# # 距离权重：距离越近，权重越高（最大权重为 1.0，距离为 0 时）
|
||
# # 当距离为 search_radius 时，权重为 0.5
|
||
# distance_weight = 1.0 - (distance_from_center / search_radius) * 0.5
|
||
# distance_weight = max(0.5, distance_weight) # 最小权重 0.5
|
||
|
||
# # 综合得分：RGB 总和 * 距离权重
|
||
# score = rgb_sum * distance_weight
|
||
|
||
# if score > max_score:
|
||
# max_score = score
|
||
# best_pos = (x, y)
|
||
# print("best_pos:", best_pos)
|
||
# return best_pos
|
||
def _is_point_in_ellipse(self, point, ellipse_params):
|
||
"""
|
||
判断点是否在椭圆内
|
||
|
||
Args:
|
||
point: 点坐标 (x, y)
|
||
ellipse_params: 椭圆参数 ((center_x, center_y), (width, height), angle)
|
||
|
||
Returns:
|
||
bool: 如果点在椭圆内返回 True，否则返回 False
|
||
"""
|
||
if ellipse_params is None:
|
||
return True # 如果没有椭圆参数，不进行限制
|
||
|
||
import math
|
||
|
||
(cx, cy), (width, height), angle = ellipse_params
|
||
px, py = point
|
||
|
||
# 椭圆半长轴和半短轴
|
||
a = width / 2.0 # 半长轴
|
||
b = height / 2.0 # 半短轴
|
||
|
||
# 将点坐标平移到椭圆中心
|
||
dx = px - cx
|
||
dy = py - cy
|
||
|
||
# 旋转坐标系，使椭圆的长轴与x轴对齐
|
||
# angle 是度，需要转换为弧度
|
||
angle_rad = math.radians(angle)
|
||
cos_a = math.cos(angle_rad)
|
||
sin_a = math.sin(angle_rad)
|
||
|
||
# 旋转后的坐标
|
||
x_rot = dx * cos_a + dy * sin_a
|
||
y_rot = -dx * sin_a + dy * cos_a
|
||
|
||
# 检查点是否在椭圆内：((x_rot/a)^2 + (y_rot/b)^2) <= 1
|
||
ellipse_value = (x_rot / a) ** 2 + (y_rot / b) ** 2
|
||
|
||
return ellipse_value <= 1.0
|
||
|
||
def find_red_laser_with_ellipse(self, frame, threshold=None, search_radius=None, ellipse_params=None):
|
||
"""
|
||
使用椭圆拟合方法查找激光点中心（更准确）
|
||
先找到所有红色像素，然后拟合椭圆找到中心
|
||
|
||
Args:
|
||
frame: 图像帧
|
||
threshold: 红色通道阈值（如果为None，使用config.LASER_DETECTION_THRESHOLD）
|
||
search_radius: 搜索半径（像素），从图像中心开始搜索（如果为None，使用config.LASER_SEARCH_RADIUS）
|
||
ellipse_params: 椭圆参数 ((center_x, center_y), (width, height), angle)，用于限制激光点必须在椭圆内
|
||
|
||
Returns:
|
||
(x, y) 坐标，如果未找到则返回 None
|
||
注意：如果启用 LASER_DRAW_ELLIPSE，会在原始 frame 上绘制椭圆（会修改输入图像）
|
||
"""
|
||
import cv2
|
||
import numpy as np
|
||
from maix import image
|
||
self.logger.debug(f"find_red_laser_with_ellipse start: {time.ticks_ms()}")
|
||
# 使用配置项
|
||
if threshold is None:
|
||
threshold = config.LASER_DETECTION_THRESHOLD
|
||
if search_radius is None:
|
||
search_radius = config.LASER_SEARCH_RADIUS
|
||
|
||
red_ratio = config.LASER_RED_RATIO
|
||
overexposed_threshold = config.LASER_OVEREXPOSED_THRESHOLD
|
||
overexposed_diff = config.LASER_OVEREXPOSED_DIFF
|
||
|
||
w, h = frame.width(), frame.height()
|
||
center_x, center_y = w // 2, h // 2
|
||
|
||
# 转换为 OpenCV 格式
|
||
img_cv = image.image2cv(frame, False, False)
|
||
|
||
# 只在中心区域搜索
|
||
x_min = max(0, center_x - search_radius)
|
||
x_max = min(w, center_x + search_radius)
|
||
y_min = max(0, center_y - search_radius)
|
||
y_max = min(h, center_y + search_radius)
|
||
|
||
# 提取ROI区域（只处理搜索区域，而不是整个图像）
|
||
roi = img_cv[y_min:y_max, x_min:x_max]
|
||
if roi.size == 0:
|
||
self.logger.debug("[LASER] ROI区域为空")
|
||
return None
|
||
|
||
# 分离RGB通道（向量化操作，比循环快得多）
|
||
r_channel = roi[:, :, 0].astype(np.int32) # 转换为int32避免溢出
|
||
g_channel = roi[:, :, 1].astype(np.int32)
|
||
b_channel = roi[:, :, 2].astype(np.int32)
|
||
|
||
# 情况1：正常红色判断（向量化）
|
||
# r > threshold and r > g * red_ratio and r > b * red_ratio
|
||
mask_red = (r_channel > threshold) & \
|
||
(r_channel > (g_channel * red_ratio)) & \
|
||
(r_channel > (b_channel * red_ratio))
|
||
|
||
# 情况2：过曝的红色判断（向量化）
|
||
# r > overexposed_threshold and g > overexposed_threshold and b > overexposed_threshold
|
||
# and r >= g and r >= b and (r - g) > overexposed_diff and (r - b) > overexposed_diff
|
||
mask_overexposed = (r_channel > overexposed_threshold) & \
|
||
(g_channel > overexposed_threshold) & \
|
||
(b_channel > overexposed_threshold) & \
|
||
(r_channel >= g_channel) & \
|
||
(r_channel >= b_channel) & \
|
||
((r_channel - g_channel) > overexposed_diff) & \
|
||
((r_channel - b_channel) > overexposed_diff)
|
||
|
||
# 合并两种情况的掩码
|
||
mask_combined = mask_red | mask_overexposed
|
||
|
||
# 转换为uint8格式
|
||
mask_roi = mask_combined.astype(np.uint8) * 255
|
||
|
||
self.logger.debug(f"ellipse fitting start: {time.ticks_ms()}")
|
||
# 查找轮廓（只在搜索区域内）
|
||
contours, _ = cv2.findContours(mask_roi, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
self.logger.debug(f"ellipse fitting end: {time.ticks_ms()}")
|
||
if not contours:
|
||
self.logger.debug("[LASER] 未找到红色像素区域")
|
||
return None
|
||
self.logger.debug(f"ellipse filtering start: {time.ticks_ms()}")
|
||
# 找到最大的轮廓（应该是激光点）
|
||
largest_contour = max(contours, key=cv2.contourArea)
|
||
|
||
# 检查轮廓面积（太小可能是噪声）
|
||
area = cv2.contourArea(largest_contour)
|
||
min_area = config.LASER_MIN_AREA
|
||
if area < min_area:
|
||
self.logger.debug(f"[LASER] 红色区域太小（面积={area:.1f}），可能是噪声（最小={min_area}）")
|
||
return None
|
||
|
||
# 使用椭圆拟合找到中心
|
||
laser_center = None
|
||
outer_ellipse_params = None # 外层红色椭圆参数
|
||
inner_ellipse_params = None # 内层亮度椭圆参数
|
||
|
||
if len(largest_contour) >= 5:
|
||
# 椭圆拟合需要至少5个点
|
||
# 注意：需要将轮廓坐标转换回全图坐标
|
||
contour_global = largest_contour.copy()
|
||
for i in range(len(contour_global)):
|
||
contour_global[i][0][0] += x_min
|
||
contour_global[i][0][1] += y_min
|
||
|
||
try:
|
||
# 第一步：拟合外层红色椭圆
|
||
(x_outer, y_outer), (width_outer, height_outer), angle_outer = cv2.fitEllipse(contour_global)
|
||
outer_ellipse_params = ((x_outer, y_outer), (width_outer, height_outer), angle_outer)
|
||
|
||
self.logger.debug(f"[LASER] 外层红色椭圆拟合成功: 中心=({x_outer:.1f}, {y_outer:.1f}), 尺寸=({width_outer:.1f}, {height_outer:.1f}), 角度={angle_outer:.1f}°, 面积={area:.1f}")
|
||
|
||
# 第二步：在外层椭圆区域内，找亮度最高的像素
|
||
# 创建外层椭圆的掩码
|
||
outer_ellipse_mask = np.zeros((h, w), dtype=np.uint8)
|
||
cv2.ellipse(outer_ellipse_mask,
|
||
(int(x_outer), int(y_outer)),
|
||
(int(width_outer/2), int(height_outer/2)),
|
||
angle_outer,
|
||
0, 360,
|
||
255, -1) # 填充椭圆区域
|
||
|
||
# 在外层椭圆区域内，计算每个像素的亮度（RGB总和）
|
||
brightness = (img_cv[:, :, 0].astype(np.int32) +
|
||
img_cv[:, :, 1].astype(np.int32) +
|
||
img_cv[:, :, 2].astype(np.int32))
|
||
|
||
# 只考虑外层椭圆区域内的像素
|
||
brightness_masked = np.where(outer_ellipse_mask > 0, brightness, 0)
|
||
|
||
# 找到亮度阈值（使用区域内亮度的较高百分位，比如80%）
|
||
brightness_values = brightness_masked[brightness_masked > 0]
|
||
if len(brightness_values) > 0:
|
||
brightness_threshold = np.percentile(brightness_values, 90) # 取90%分位数
|
||
|
||
# 创建亮度掩码（只保留高亮度像素）
|
||
brightness_mask = (brightness_masked >= brightness_threshold).astype(np.uint8) * 255
|
||
|
||
# 查找亮度区域的轮廓
|
||
brightness_contours, _ = cv2.findContours(brightness_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||
|
||
# 在 brightness_contours 处理部分，添加 else 分支处理 brightness_area < 1 的情况
|
||
if brightness_contours:
|
||
# 找到最大的亮度轮廓
|
||
largest_brightness_contour = max(brightness_contours, key=cv2.contourArea)
|
||
brightness_area = cv2.contourArea(largest_brightness_contour)
|
||
|
||
if brightness_area >= 3 and len(largest_brightness_contour) >= 5:
|
||
# 第三步：拟合内层亮度椭圆
|
||
try:
|
||
(x_inner, y_inner), (width_inner, height_inner), angle_inner = cv2.fitEllipse(largest_brightness_contour)
|
||
inner_ellipse_params = ((x_inner, y_inner), (width_inner, height_inner), angle_inner)
|
||
laser_center = (float(x_inner), float(y_inner))
|
||
self.logger.debug(f"[LASER] 内层亮度椭圆拟合成功: 中心=({x_inner:.1f}, {y_inner:.1f}), 尺寸=({width_inner:.1f}, {height_inner:.1f}), 角度={angle_inner:.1f}°, 面积={brightness_area:.1f}")
|
||
except Exception as e:
|
||
# 内层椭圆拟合失败，使用质心
|
||
M = cv2.moments(largest_brightness_contour)
|
||
if M["m00"] != 0:
|
||
cx = M["m10"] / M["m00"]
|
||
cy = M["m01"] / M["m00"]
|
||
laser_center = (float(cx), float(cy))
|
||
self.logger.debug(f"[LASER] 内层亮度椭圆拟合失败，使用质心: {laser_center}, 错误: {e}")
|
||
else:
|
||
# 质心计算失败，使用外层椭圆中心
|
||
laser_center = (float(x_outer), float(y_outer))
|
||
self.logger.debug(f"[LASER] 内层区域质心计算失败，使用外层椭圆中心: {laser_center}")
|
||
elif brightness_area >= 1:
|
||
# 面积太小，使用质心
|
||
M = cv2.moments(largest_brightness_contour)
|
||
if M["m00"] != 0:
|
||
cx = M["m10"] / M["m00"]
|
||
cy = M["m01"] / M["m00"]
|
||
laser_center = (float(cx), float(cy))
|
||
self.logger.debug(f"[LASER] 内层区域质心计算成功: {laser_center}")
|
||
else:
|
||
# 质心计算失败，使用外层椭圆中心
|
||
laser_center = (float(x_outer), float(y_outer))
|
||
self.logger.debug(f"[LASER] 内层区域质心计算失败，使用外层椭圆中心: {laser_center}")
|
||
else:
|
||
# brightness_area < 1，面积太小，直接使用外层椭圆中心
|
||
laser_center = (float(x_outer), float(y_outer))
|
||
self.logger.debug(f"[LASER] 内层亮度区域面积太小（{brightness_area:.1f}），使用外层椭圆中心: {laser_center}")
|
||
else:
|
||
# 没有找到亮度轮廓，使用外层椭圆中心
|
||
laser_center = (float(x_outer), float(y_outer))
|
||
self.logger.debug(f"[LASER] 外层椭圆区域内无有效亮度值，使用外层椭圆中心: {laser_center}")
|
||
else:
|
||
# 没有亮度值，使用外层椭圆中心
|
||
laser_center = (float(x_outer), float(y_outer))
|
||
self.logger.debug(f"[LASER] 外层椭圆区域内无有效亮度值，使用外层椭圆中心: {laser_center}")
|
||
|
||
# 如果启用绘制椭圆，在图像上绘制
|
||
if config.LASER_DRAW_ELLIPSE:
|
||
import math
|
||
# 绘制外层红色椭圆（绿色）
|
||
cx_outer, cy_outer = int(x_outer), int(y_outer)
|
||
cv2.ellipse(img_cv,
|
||
(cx_outer, cy_outer),
|
||
(int(width_outer/2), int(height_outer/2)),
|
||
angle_outer,
|
||
0, 360,
|
||
(0, 255, 0), # 绿色 (RGB格式)
|
||
2)
|
||
|
||
# 如果找到内层椭圆，绘制内层亮度椭圆（黄色）和中心点（红色）
|
||
if inner_ellipse_params is not None:
|
||
(x_inner, y_inner), (width_inner, height_inner), angle_inner = inner_ellipse_params
|
||
cx_inner, cy_inner = int(x_inner), int(y_inner)
|
||
# 绘制内层椭圆（黄色）
|
||
cv2.ellipse(img_cv,
|
||
(cx_inner, cy_inner),
|
||
(int(width_inner/2), int(height_inner/2)),
|
||
angle_inner,
|
||
0, 360,
|
||
(255, 255, 0), # 黄色 (RGB格式)
|
||
2)
|
||
# 绘制内层椭圆中心点（红色，较大）
|
||
cv2.circle(img_cv, (cx_inner, cy_inner), 5, (255, 0, 0), -1)
|
||
else:
|
||
# 只绘制外层椭圆中心点（红色）
|
||
cv2.circle(img_cv, (cx_outer, cy_outer), 3, (255, 0, 0), -1)
|
||
|
||
|
||
# 将绘制后的图像转换回 MaixPy 格式并保存到实例变量
|
||
from maix import image
|
||
self._last_frame_with_ellipse = image.cv2image(img_cv, False, False)
|
||
|
||
if inner_ellipse_params:
|
||
self.logger.debug(f"[LASER] 已绘制双层椭圆: 外层(绿色)中心=({cx_outer}, {cy_outer}), 内层(黄色)中心=({cx_inner}, {cy_inner})")
|
||
else:
|
||
self.logger.debug(f"[LASER] 已绘制外层椭圆: 中心=({cx_outer}, {cy_outer})")
|
||
except Exception as e:
|
||
laser_ellipse_params = None
|
||
# 椭圆拟合失败，使用质心
|
||
M = cv2.moments(contour_global)
|
||
if M["m00"] != 0:
|
||
cx = M["m10"] / M["m00"]
|
||
cy = M["m01"] / M["m00"]
|
||
laser_center = (float(cx), float(cy))
|
||
self.logger.debug(f"[LASER] 椭圆拟合失败，使用质心: {laser_center}, 错误: {e}")
|
||
self.logger.debug(f"ellipse filtering start: {time.ticks_ms()}")
|
||
else:
|
||
# 点太少，使用质心
|
||
contour_global = largest_contour.copy()
|
||
for i in range(len(contour_global)):
|
||
contour_global[i][0][0] += x_min
|
||
contour_global[i][0][1] += y_min
|
||
|
||
M = cv2.moments(contour_global)
|
||
if M["m00"] != 0:
|
||
cx = M["m10"] / M["m00"]
|
||
cy = M["m01"] / M["m00"]
|
||
laser_center = (float(cx), float(cy))
|
||
self.logger.debug(f"[LASER] 点太少（{len(largest_contour)}个），使用质心: {laser_center}")
|
||
|
||
if laser_center is None:
|
||
# 清除之前保存的椭圆图像
|
||
self._last_frame_with_ellipse = None
|
||
return None
|
||
|
||
# 检查距离中心是否太远
|
||
final_x, final_y = laser_center
|
||
dx_final = final_x - center_x
|
||
dy_final = final_y - center_y
|
||
distance_from_center_final = (dx_final * dx_final + dy_final * dy_final) ** 0.5
|
||
|
||
max_distance = config.LASER_MAX_DISTANCE_FROM_CENTER
|
||
if distance_from_center_final > max_distance:
|
||
self.logger.warning(f"[LASER] 激光点距离中心太远: 位置={laser_center}, "
|
||
f"距离中心={distance_from_center_final:.1f}像素, "
|
||
f"最大允许距离={max_distance}像素")
|
||
return None
|
||
|
||
# 检查是否在黄心椭圆范围内（如果启用）
|
||
if config.LASER_REQUIRE_IN_ELLIPSE and ellipse_params is not None:
|
||
if not self._is_point_in_ellipse(laser_center, ellipse_params):
|
||
(ell_center, (ell_width, ell_height), ell_angle) = ellipse_params
|
||
self.logger.warning(f"[LASER] 激光点不在黄心椭圆内: 位置={laser_center}, "
|
||
f"椭圆中心={ell_center}, 椭圆尺寸=({ell_width:.1f}, {ell_height:.1f})")
|
||
return None
|
||
|
||
ellipse_info = ""
|
||
if config.LASER_REQUIRE_IN_ELLIPSE and ellipse_params is not None:
|
||
ellipse_info = f", 椭圆内检查: 通过"
|
||
elif not config.LASER_REQUIRE_IN_ELLIPSE:
|
||
ellipse_info = f", 椭圆检查: 已禁用"
|
||
self.logger.debug(f"[LASER] 找到激光点（椭圆拟合）: 位置={laser_center}, "
|
||
f"距离中心={distance_from_center_final:.1f}像素{ellipse_info}")
|
||
if config.LASER_DRAW_ELLIPSE and self._last_frame_with_ellipse is not None:
|
||
self.logger.debug(f"[LASER] 已保存绘制了椭圆的图像，可通过 get_last_frame_with_ellipse() 获取")
|
||
|
||
return laser_center
|
||
|
||
def _find_red_laser_brightest(self, frame, threshold=None, search_radius=None, ellipse_params=None):
|
||
"""
|
||
在图像中心附近查找最亮的红色激光点（基于 RGB 阈值）
|
||
使用两阶段搜索：先粗搜索找到候选区域，再精细搜索找到最亮点
|
||
如果启用 LASER_REQUIRE_IN_ELLIPSE，只有激光点落在黄心椭圆范围内才算有效
|
||
|
||
Args:
|
||
frame: 图像帧
|
||
threshold: 红色通道阈值（如果为None，使用config.LASER_DETECTION_THRESHOLD）
|
||
search_radius: 搜索半径（像素），从图像中心开始搜索（如果为None，使用config.LASER_SEARCH_RADIUS）
|
||
ellipse_params: 椭圆参数 ((center_x, center_y), (width, height), angle)，用于限制激光点必须在椭圆内
|
||
如果 config.LASER_REQUIRE_IN_ELLIPSE 为 False，则忽略此参数
|
||
|
||
Returns:
|
||
(x, y) 坐标，如果未找到或不在椭圆内（如果启用检查）则返回 None
|
||
"""
|
||
# 使用配置项，如果参数未提供则使用默认配置
|
||
if threshold is None:
|
||
threshold = config.LASER_DETECTION_THRESHOLD
|
||
if search_radius is None:
|
||
search_radius = config.LASER_SEARCH_RADIUS
|
||
|
||
red_ratio = config.LASER_RED_RATIO
|
||
overexposed_threshold = config.LASER_OVEREXPOSED_THRESHOLD
|
||
overexposed_diff = config.LASER_OVEREXPOSED_DIFF
|
||
|
||
w, h = frame.width(), frame.height()
|
||
center_x, center_y = w // 2, h // 2
|
||
|
||
# 只在中心区域搜索
|
||
x_min = max(0, center_x - search_radius)
|
||
x_max = min(w, center_x + search_radius)
|
||
y_min = max(0, center_y - search_radius)
|
||
y_max = min(h, center_y + search_radius)
|
||
|
||
img_bytes = frame.to_bytes()
|
||
max_score = 0
|
||
candidate_pos = None
|
||
|
||
# 用于调试：记录最接近但未满足条件的点
|
||
best_near_red = None
|
||
best_near_red_score = 0
|
||
best_near_red_rgb = None
|
||
|
||
# 第一阶段：粗搜索（每2像素采样），找到候选点
|
||
for y in range(y_min, y_max, 2):
|
||
for x in range(x_min, x_max, 2):
|
||
idx = (y * w + x) * 3
|
||
r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
|
||
|
||
# 判断是否为红色或过曝的红色（发白）
|
||
is_red = False
|
||
is_overexposed_red = False
|
||
|
||
# 情况1：正常红色（使用配置的倍数要求）
|
||
if r > threshold and r > g * red_ratio and r > b * red_ratio:
|
||
is_red = True
|
||
|
||
# 情况2：过曝的红色（发白，r, g, b 都接近255，但 r 仍然最大）
|
||
elif r > overexposed_threshold and g > overexposed_threshold and b > overexposed_threshold:
|
||
if r >= g and r >= b and (r - g) > overexposed_diff and (r - b) > overexposed_diff:
|
||
is_overexposed_red = True
|
||
|
||
if is_red or is_overexposed_red:
|
||
# 计算得分：RGB 总和 + 距离中心权重
|
||
rgb_sum = r + g + b
|
||
dx = x - center_x
|
||
dy = y - center_y
|
||
distance_from_center = (dx * dx + dy * dy) ** 0.5
|
||
distance_weight = 1.0 - (distance_from_center / search_radius) * 0.5
|
||
distance_weight = max(0.5, distance_weight)
|
||
|
||
score = rgb_sum * distance_weight
|
||
|
||
if score > max_score:
|
||
max_score = score
|
||
candidate_pos = (x, y)
|
||
else:
|
||
# 记录最接近但未满足条件的点（用于调试）
|
||
if r > threshold * 0.8: # 至少接近阈值
|
||
rgb_sum = r + g + b
|
||
# 计算接近度分数
|
||
ratio_score = min(r / (g + 1), r / (b + 1)) # 避免除零
|
||
near_score = rgb_sum * ratio_score
|
||
if near_score > best_near_red_score:
|
||
best_near_red_score = near_score
|
||
best_near_red = (x, y)
|
||
best_near_red_rgb = (r, g, b)
|
||
|
||
# 如果没有找到候选点，输出调试信息
|
||
if candidate_pos is None:
|
||
if best_near_red:
|
||
self.logger.debug(f"[LASER] 未找到激光点，最接近的点: 位置={best_near_red}, RGB={best_near_red_rgb}, "
|
||
f"阈值={threshold}, 倍数要求={red_ratio}, r/g={best_near_red_rgb[0]/(best_near_red_rgb[1]+1):.2f}, "
|
||
f"r/b={best_near_red_rgb[0]/(best_near_red_rgb[2]+1):.2f}")
|
||
else:
|
||
self.logger.debug(f"[LASER] 未找到激光点，搜索区域: ({x_min}, {y_min}) 到 ({x_max}, {y_max}), "
|
||
f"阈值={threshold}, 倍数要求={red_ratio}")
|
||
return None
|
||
|
||
# 第二阶段：在候选点周围进行精细搜索（1像素间隔）
|
||
# 在候选点周围 5x5 或 7x7 区域内找最亮的点
|
||
refine_radius = 3 # 精细搜索半径（像素）
|
||
cx, cy = candidate_pos
|
||
|
||
x_min_fine = max(0, cx - refine_radius)
|
||
x_max_fine = min(w, cx + refine_radius + 1)
|
||
y_min_fine = max(0, cy - refine_radius)
|
||
y_max_fine = min(h, cy + refine_radius + 1)
|
||
|
||
max_brightness = 0
|
||
best_pos = candidate_pos
|
||
best_rgb = None
|
||
|
||
# 精细搜索：1像素间隔，只考虑亮度（RGB总和）
|
||
for y in range(y_min_fine, y_max_fine, 1):
|
||
for x in range(x_min_fine, x_max_fine, 1):
|
||
idx = (y * w + x) * 3
|
||
r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2]
|
||
|
||
# 判断是否为红色或过曝的红色（使用配置的倍数要求）
|
||
is_red = False
|
||
is_overexposed_red = False
|
||
|
||
if r > threshold and r > g * red_ratio and r > b * red_ratio:
|
||
is_red = True
|
||
elif r > overexposed_threshold and g > overexposed_threshold and b > overexposed_threshold:
|
||
if r >= g and r >= b and (r - g) > overexposed_diff and (r - b) > overexposed_diff:
|
||
is_overexposed_red = True
|
||
|
||
if is_red or is_overexposed_red:
|
||
rgb_sum = r + g + b
|
||
# 精细搜索阶段只考虑亮度，不考虑距离权重
|
||
if rgb_sum > max_brightness:
|
||
max_brightness = rgb_sum
|
||
best_pos = (x, y)
|
||
best_rgb = (r, g, b)
|
||
|
||
# 检查找到的激光点是否满足条件
|
||
if best_pos:
|
||
final_x, final_y = best_pos
|
||
dx_final = final_x - center_x
|
||
dy_final = final_y - center_y
|
||
distance_from_center_final = (dx_final * dx_final + dy_final * dy_final) ** 0.5
|
||
|
||
# 检查1：距离中心是否太远
|
||
max_distance = config.LASER_MAX_DISTANCE_FROM_CENTER
|
||
if distance_from_center_final > max_distance:
|
||
# 距离中心太远，拒绝这个结果
|
||
self.logger.warning(f"[LASER] 找到的激光点距离中心太远: 位置={best_pos}, "
|
||
f"距离中心={distance_from_center_final:.1f}像素, "
|
||
f"最大允许距离={max_distance}像素, 拒绝此结果")
|
||
return None
|
||
|
||
# 检查2：是否在黄心椭圆范围内（仅在启用时检查）
|
||
if config.LASER_REQUIRE_IN_ELLIPSE and ellipse_params is not None:
|
||
if not self._is_point_in_ellipse(best_pos, ellipse_params):
|
||
# 不在椭圆内，拒绝这个结果
|
||
(ell_center, (ell_width, ell_height), ell_angle) = ellipse_params
|
||
self.logger.warning(f"[LASER] 找到的激光点不在黄心椭圆内: 位置={best_pos}, "
|
||
f"椭圆中心={ell_center}, 椭圆尺寸=({ell_width:.1f}, {ell_height:.1f}), "
|
||
f"椭圆角度={ell_angle:.1f}°, 拒绝此结果")
|
||
return None
|
||
|
||
# 输出成功找到激光点的日志
|
||
ellipse_info = ""
|
||
if config.LASER_REQUIRE_IN_ELLIPSE and ellipse_params is not None:
|
||
ellipse_info = f", 椭圆内检查: 通过"
|
||
elif not config.LASER_REQUIRE_IN_ELLIPSE:
|
||
ellipse_info = f", 椭圆检查: 已禁用"
|
||
self.logger.debug(f"[LASER] 找到激光点: 位置={best_pos}, RGB={best_rgb}, "
|
||
f"亮度={max_brightness}, 距离中心={distance_from_center_final:.1f}像素{ellipse_info}, "
|
||
f"阈值={threshold}, 倍数要求={red_ratio}")
|
||
|
||
return best_pos
|
||
|
||
def find_red_laser(self, frame, threshold=None, search_radius=None, ellipse_params=None):
|
||
"""
|
||
查找激光点（支持两种方法：椭圆拟合或最亮点）
|
||
根据 config.LASER_USE_ELLIPSE_FITTING 配置选择使用哪种方法
|
||
|
||
Args:
|
||
frame: 图像帧
|
||
threshold: 红色通道阈值（如果为None，使用config.LASER_DETECTION_THRESHOLD）
|
||
search_radius: 搜索半径（像素），从图像中心开始搜索（如果为None，使用config.LASER_SEARCH_RADIUS）
|
||
ellipse_params: 椭圆参数 ((center_x, center_y), (width, height), angle)，用于限制激光点必须在椭圆内
|
||
|
||
Returns:
|
||
(x, y) 坐标，如果未找到则返回 None
|
||
"""
|
||
if config.LASER_USE_ELLIPSE_FITTING:
|
||
return self.find_red_laser_with_ellipse(frame, threshold, search_radius, ellipse_params)
|
||
else:
|
||
# 使用原来的最亮点方法
|
||
return self._find_red_laser_brightest(frame, threshold, search_radius, ellipse_params)
|
||
|
||
def calibrate_laser_position(self, timeout_ms=8000, check_sharpness=True):
|
||
"""
|
||
执行激光校准：循环拍照 → 检测靶心 → 检查激光点清晰度 → 找红点 → 保存坐标
|
||
只有检测到靶心时才读取激光点
|
||
|
||
Args:
|
||
timeout_ms: 超时时间（毫秒），默认8000ms
|
||
check_sharpness: 是否检查激光点清晰度，默认True
|
||
|
||
Returns:
|
||
(x, y) 坐标，如果超时或失败则返回 None
|
||
"""
|
||
from camera_manager import camera_manager
|
||
# from vision import check_laser_point_sharpness, save_calibration_image, detect_circle_v3
|
||
import vision
|
||
from maix import time
|
||
|
||
start = time.ticks_ms()
|
||
|
||
# 注意：使用 abs(time.ticks_diff(start, time.ticks_ms())) 避免负数问题
|
||
while self._calibration_active and abs(time.ticks_diff(start, time.ticks_ms())) < timeout_ms:
|
||
try:
|
||
# 使用全局 camera_manager，线程安全读取
|
||
frame = camera_manager.read_frame()
|
||
|
||
# 先检测靶心（仅在需要椭圆检查时）
|
||
ellipse_params_temp = None
|
||
center_temp = None
|
||
radius_temp = None
|
||
if config.LASER_REQUIRE_IN_ELLIPSE:
|
||
result_img_temp, center_temp, radius_temp, method_temp, best_radius1_temp, ellipse_params_temp = vision.detect_circle_v3(frame, None)
|
||
|
||
# 只有检测到靶心时才继续处理激光点
|
||
if center_temp is None or radius_temp is None:
|
||
self.logger.debug(f"[LASER] 未检测到靶心，跳过")
|
||
time.sleep_ms(60)
|
||
continue
|
||
|
||
# 检测到靶心，继续处理激光点
|
||
# 检查激光点清晰度（可选）
|
||
sharpness_score = None # 初始化清晰度分数
|
||
if check_sharpness:
|
||
try:
|
||
# 使用 check_laser_point_sharpness 检测激光点清晰度
|
||
# 该函数会自动查找激光点并检测其清晰度
|
||
# 仅在启用椭圆检查时传入椭圆参数
|
||
is_sharp, sharpness_score, laser_pos = vision.check_laser_point_sharpness(
|
||
frame,
|
||
laser_point=None, # 自动查找激光点
|
||
roi_size=30,
|
||
threshold=config.IMAGE_SHARPNESS_THRESHOLD,
|
||
ellipse_params=ellipse_params_temp if config.LASER_REQUIRE_IN_ELLIPSE else None
|
||
)
|
||
|
||
if laser_pos is None:
|
||
# 未找到激光点
|
||
self.logger.debug(f"[LASER] 未找到激光点，跳过")
|
||
time.sleep_ms(60)
|
||
continue
|
||
|
||
if not is_sharp:
|
||
# 激光点模糊
|
||
self.logger.debug(f"[LASER] 激光点模糊（清晰度: {sharpness_score:.2f}），跳过")
|
||
time.sleep_ms(60)
|
||
continue
|
||
|
||
# 激光点清晰，使用找到的激光点位置
|
||
pos = laser_pos
|
||
|
||
except Exception as e:
|
||
self.logger.warning(f"[LASER] 激光点清晰度检测失败: {e}，继续处理")
|
||
# 检测失败时，回退到原来的方法：直接查找激光点
|
||
# 仅在启用椭圆检查时传入椭圆参数
|
||
pos = self.find_red_laser(frame, ellipse_params=ellipse_params_temp if config.LASER_REQUIRE_IN_ELLIPSE else None)
|
||
if pos is None:
|
||
time.sleep_ms(60)
|
||
continue
|
||
else:
|
||
# 不检查清晰度，直接查找激光点
|
||
# 仅在启用椭圆检查时传入椭圆参数
|
||
pos = self.find_red_laser(frame, ellipse_params=ellipse_params_temp if config.LASER_REQUIRE_IN_ELLIPSE else None)
|
||
if pos is None:
|
||
time.sleep_ms(60)
|
||
continue
|
||
|
||
# 找到清晰的激光点，保存校准图像
|
||
if pos:
|
||
# 保存校准图像（带标注）
|
||
try:
|
||
# 如果使用椭圆拟合且启用了椭圆绘制，使用绘制了椭圆的图像
|
||
frame_to_save = frame
|
||
if config.LASER_USE_ELLIPSE_FITTING and config.LASER_DRAW_ELLIPSE:
|
||
frame_with_ellipse = self.get_last_frame_with_ellipse()
|
||
if frame_with_ellipse is not None:
|
||
frame_to_save = frame_with_ellipse
|
||
|
||
vision.save_calibration_image(frame_to_save, pos)
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] 保存校准图像失败: {e}")
|
||
|
||
# 设置结果、停止校准、保存坐标
|
||
self.set_calibration_result(pos)
|
||
self.stop_calibration()
|
||
self.save_laser_point(pos)
|
||
|
||
if sharpness_score is not None:
|
||
self.logger.info(f"✅ 校准成功: {pos} (清晰度: {sharpness_score:.2f}, 靶心: {center_temp}, 半径: {radius_temp})")
|
||
else:
|
||
self.logger.info(f"✅ 校准成功: {pos} (靶心: {center_temp}, 半径: {radius_temp})")
|
||
return pos
|
||
|
||
# 未找到激光点，继续循环
|
||
time.sleep_ms(60)
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] 校准过程异常: {e}")
|
||
import traceback
|
||
self.logger.error(traceback.format_exc())
|
||
time.sleep_ms(200)
|
||
|
||
# 超时或校准被停止
|
||
if self._calibration_active:
|
||
self.logger.warning(f"[LASER] 校准超时（{timeout_ms}ms）")
|
||
else:
|
||
self.logger.info("[LASER] 校准已停止")
|
||
|
||
return None
|
||
|
||
def start_calibration(self):
|
||
"""开始校准（公共方法）"""
|
||
with self._calibration_lock:
|
||
if self._calibration_active:
|
||
return False
|
||
self._calibration_active = True
|
||
self._calibration_result = None
|
||
return True
|
||
|
||
def stop_calibration(self):
|
||
"""停止校准（公共方法）"""
|
||
with self._calibration_lock:
|
||
self._calibration_active = False
|
||
|
||
def set_calibration_result(self, result):
|
||
"""设置校准结果（内部方法）"""
|
||
with self._calibration_lock:
|
||
self._calibration_result = result
|
||
|
||
def get_calibration_result(self):
|
||
"""获取并清除校准结果（内部方法）"""
|
||
with self._calibration_lock:
|
||
result = self._calibration_result
|
||
self._calibration_result = None
|
||
return result
|
||
|
||
def parse_bcd_distance(self, bcd_bytes: bytes) -> float:
|
||
"""将 4 字节 BCD 码转换为距离（米）"""
|
||
if len(bcd_bytes) != 4:
|
||
return 0.0
|
||
try:
|
||
hex_string = binascii.hexlify(bcd_bytes).decode()
|
||
distance_int = int(hex_string)
|
||
return distance_int / 1000.0
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] BCD 解析失败: {e}")
|
||
return 0.0
|
||
|
||
def read_distance_from_laser_sensor(self):
|
||
"""发送测距指令并返回距离（米）和信号质量
|
||
返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0)
|
||
"""
|
||
from hardware import hardware_manager
|
||
if hardware_manager.distance_serial is None:
|
||
self.logger.error("[LASER] distance_serial 未初始化")
|
||
return (0.0, 0)
|
||
|
||
try:
|
||
# 清空缓冲区
|
||
try:
|
||
hardware_manager.distance_serial.read(-1)
|
||
except:
|
||
pass
|
||
# 打开激光
|
||
|
||
self.turn_on_laser()
|
||
self._laser_turned_on = True
|
||
# time.sleep_ms(500) # 需要一定时间让激光稳定
|
||
# 发送测距查询命令
|
||
hardware_manager.distance_serial.write(config.DISTANCE_QUERY_CMD)
|
||
# time.sleep_ms(500) # 测试结果：这里的等待没有用！
|
||
self.turn_off_laser()
|
||
self._laser_turned_on = False
|
||
|
||
# 这里的等待才是有效的！大概350ms能读到数据
|
||
# 循环读取响应，最多等待500ms
|
||
start_time = time.ticks_ms()
|
||
max_wait_ms = 500
|
||
response = None
|
||
|
||
while True:
|
||
# 检查是否超时
|
||
# 注意：使用 time.ticks_diff(start_time, time.ticks_ms()) 避免负数问题
|
||
elapsed_ms = abs(time.ticks_diff(start_time, time.ticks_ms()))
|
||
if elapsed_ms >= max_wait_ms:
|
||
self.logger.warning(f"[LASER] 读取超时 ({elapsed_ms}ms)，未收到完整响应")
|
||
return (0.0, 0)
|
||
|
||
# 尝试读取数据
|
||
response = hardware_manager.distance_serial.read(config.DISTANCE_RESPONSE_LEN)
|
||
|
||
# 如果读到完整数据，立即返回
|
||
if response and len(response) == config.DISTANCE_RESPONSE_LEN:
|
||
elapsed_ms = abs(time.ticks_diff(start_time, time.ticks_ms()))
|
||
self.logger.debug(f"[LASER] 收到响应 ({elapsed_ms}ms)")
|
||
break
|
||
|
||
# 如果还没超时，短暂等待后继续尝试
|
||
time.sleep_ms(10) # 每次循环等待10ms，避免CPU占用过高
|
||
|
||
# 验证响应格式
|
||
if response and len(response) == config.DISTANCE_RESPONSE_LEN:
|
||
if response[3] != 0x20:
|
||
if response[0] == 0xEE:
|
||
err_code = (response[7] << 8) | response[8]
|
||
self.logger.warning(f"[LASER] 模块错误代码: {hex(err_code)}")
|
||
return (0.0, 0)
|
||
|
||
# 解析BCD码距离
|
||
bcd_bytes = response[6:10]
|
||
distance_value_m = self.parse_bcd_distance(bcd_bytes)
|
||
signal_quality = (response[10] << 8) | response[11]
|
||
|
||
self.logger.debug(f"[LASER] 测距成功: {distance_value_m:.3f} m, 信号质量: {signal_quality}")
|
||
return (distance_value_m, signal_quality)
|
||
|
||
self.logger.warning(f"[LASER] 无效响应: {response.hex() if response else 'None'}")
|
||
return (0.0, 0)
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] 读取激光测距失败: {e}")
|
||
return (0.0, 0)
|
||
|
||
def calculate_laser_point_from_distance(self, distance_m):
|
||
"""
|
||
根据目标距离动态计算激光点在图像中的坐标
|
||
激光在摄像头下方，所以需要将图像中心的 y 值加上偏移
|
||
|
||
Args:
|
||
distance_m: 目标距离（米），例如到靶心的距离
|
||
|
||
Returns:
|
||
(x, y): 激光点在图像中的坐标
|
||
"""
|
||
# from vision import estimate_pixel
|
||
|
||
# 图像中心坐标
|
||
center_x = config.IMAGE_CENTER_X
|
||
center_y = config.IMAGE_CENTER_Y
|
||
|
||
# 计算激光在摄像头下方的像素偏移（y 方向）
|
||
# 激光在摄像头下方，所以 y 值要增加（向下为正）
|
||
pixel_offset_y = estimate_pixel(config.LASER_CAMERA_OFFSET_CM, distance_m)
|
||
|
||
# 激光点坐标：x 保持中心，y 加上偏移，
|
||
laser_x = center_x
|
||
laser_y = center_y + int(pixel_offset_y)
|
||
|
||
self.logger.debug(f"[LASER] 根据距离 {distance_m:.2f}m 计算激光点: ({laser_x}, {laser_y}), 像素偏移: {pixel_offset_y:.2f}")
|
||
|
||
return (laser_x, laser_y)
|
||
|
||
def has_calibrated_point(self):
|
||
"""检查是否真正校准过（配置文件存在且不是默认值）"""
|
||
if config.HARDCODE_LASER_POINT:
|
||
return False # 硬编码模式下不算校准
|
||
|
||
# 检查配置文件是否存在
|
||
if "laser_config.json" not in os.listdir("/root"):
|
||
return False
|
||
|
||
# 检查当前值是否是默认值
|
||
if self._laser_point == config.DEFAULT_LASER_POINT:
|
||
return False
|
||
|
||
return self._laser_point is not None
|
||
|
||
def compute_laser_position(self, circle_center, laser_point, radius, method):
|
||
"""计算激光相对于靶心的偏移量（单位：厘米）
|
||
|
||
Args:
|
||
circle_center: 靶心中心坐标 (x, y)
|
||
laser_point: 激光点坐标 (x, y)
|
||
radius: 靶心半径（像素）
|
||
method: 检测方法（"模糊" 或其他）
|
||
|
||
Returns:
|
||
(dx, dy): 激光相对于靶心的偏移量（厘米），如果输入无效则返回 (None, None)
|
||
"""
|
||
if not all([circle_center, radius, method]):
|
||
return None, None
|
||
|
||
cx, cy = circle_center
|
||
lx, ly = laser_point
|
||
# r = 22.16 * 5
|
||
r = radius * 5
|
||
self.logger.debug(f"compute_laser_position: circle_center: {circle_center} laser_point: {laser_point} radius: {radius} method: {method} r: {r}")
|
||
target_x = (lx-cx)/r*100
|
||
target_y = (ly-cy)/r*100
|
||
self.logger.info(f"lx：{lx} ly: {ly} cx: {cx} cy: {cy} result_x: {target_x} result_y: {-target_y} real_r_x: {lx-cx} real_r_y: {-1*(ly-cy)}")
|
||
return (target_x, -target_y)
|
||
|
||
# # 根据检测方法动态调整靶心物理半径（简化模型）
|
||
# circle_r = (radius / 4.0) * 20.0 if method == "模糊" else (68 / 16.0) * 20.0
|
||
# dx = lx - cx
|
||
# dy = ly - cy
|
||
# return dx / (circle_r / 100.0), -dy / (circle_r / 100.0)
|
||
|
||
def quick_measure_distance(self):
|
||
"""
|
||
快速激光测距：打开激光 → 测距 → 关闭激光
|
||
激光开启时间最小化（约500-600ms），尽量不让用户觉察到
|
||
返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0)
|
||
"""
|
||
self._laser_turned_on = False
|
||
|
||
try:
|
||
|
||
|
||
# 等待激光稳定（最小延迟）
|
||
# time.sleep_ms(50)
|
||
|
||
# 读取距离和信号质量
|
||
result = self.read_distance_from_laser_sensor()
|
||
return result
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] 快速测距异常: {e}")
|
||
return (0.0, 0)
|
||
finally:
|
||
# 确保激光关闭
|
||
if self._laser_turned_on:
|
||
try:
|
||
self.turn_off_laser()
|
||
except Exception as e:
|
||
self.logger.error(f"[LASER] 关闭激光失败: {e}")
|
||
|
||
|
||
# 创建全局单例实例
|
||
laser_manager = LaserManager()
|
||
|
||
# ==================== 向后兼容的函数接口 ====================
|
||
|
||
def load_laser_point():
|
||
"""加载激光点（向后兼容接口）"""
|
||
return laser_manager.load_laser_point()
|
||
|
||
def save_laser_point(point):
|
||
"""保存激光点（向后兼容接口）"""
|
||
return laser_manager.save_laser_point(point)
|
||
|
||
def turn_on_laser():
|
||
"""开启激光（向后兼容接口）"""
|
||
return laser_manager.turn_on_laser()
|
||
|
||
def turn_off_laser():
|
||
"""关闭激光（向后兼容接口）"""
|
||
return laser_manager.turn_off_laser()
|
||
|
||
def flash_laser(duration_ms=1000):
|
||
"""闪激光（向后兼容接口）"""
|
||
return laser_manager.flash_laser(duration_ms)
|
||
|
||
def find_red_laser(frame, threshold=150):
|
||
"""查找红色激光点（向后兼容接口）"""
|
||
return laser_manager.find_red_laser(frame, threshold)
|
||
|
||
def calibrate_laser_position():
|
||
"""校准激光位置（向后兼容接口）"""
|
||
return laser_manager.calibrate_laser_position()
|
||
|