refind logger
This commit is contained in:
gcw_4spBpAfv
257
laser_manager.py
257
laser_manager.py
@@ -39,6 +39,11 @@ class LaserManager:
|
||||
|
||||
# ==================== 状态访问（只读属性）====================
|
||||
|
||||
@property
|
||||
def logger(self):
|
||||
"""获取 logger 对象"""
|
||||
return logger_manager.logger
|
||||
|
||||
@property
|
||||
def calibration_active(self):
|
||||
"""是否正在校准"""
|
||||
@@ -69,9 +74,7 @@ class LaserManager:
|
||||
if config.HARDCODE_LASER_POINT:
|
||||
# 硬编码模式：直接使用硬编码值
|
||||
self._laser_point = config.HARDCODE_LASER_POINT_VALUE
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.info(f"[LASER] 使用硬编码激光点: {self._laser_point}")
|
||||
self.logger.info(f"[LASER] 使用硬编码激光点: {self._laser_point}")
|
||||
return self._laser_point
|
||||
|
||||
# 正常模式：从配置文件加载
|
||||
@@ -81,9 +84,7 @@ class LaserManager:
|
||||
data = json.load(f)
|
||||
if isinstance(data, list) and len(data) == 2:
|
||||
self._laser_point = (int(data[0]), int(data[1]))
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.debug(f"[INFO] 加载激光点: {self._laser_point}")
|
||||
self.logger.debug(f"[INFO] 加载激光点: {self._laser_point}")
|
||||
return self._laser_point
|
||||
else:
|
||||
raise ValueError
|
||||
@@ -101,9 +102,7 @@ class LaserManager:
|
||||
if config.HARDCODE_LASER_POINT:
|
||||
# 硬编码模式：不保存到文件，但更新内存中的值（虽然不会被使用）
|
||||
self._laser_point = point
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.info(f"[LASER] 硬编码模式已启用，跳过保存激光点: {point}")
|
||||
self.logger.info(f"[LASER] 硬编码模式已启用，跳过保存激光点: {point}")
|
||||
return True
|
||||
|
||||
# 正常模式：保存到配置文件
|
||||
@@ -113,24 +112,19 @@ class LaserManager:
|
||||
self._laser_point = point
|
||||
return True
|
||||
except Exception as e:
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.error(f"[LASER] 保存激光点失败: {e}")
|
||||
self.logger.error(f"[LASER] 保存激光点失败: {e}")
|
||||
return False
|
||||
|
||||
def turn_on_laser(self):
|
||||
"""发送指令开启激光，并读取回包（部分模块支持）"""
|
||||
from hardware import hardware_manager
|
||||
logger = logger_manager.logger
|
||||
|
||||
if hardware_manager.distance_serial is None:
|
||||
if logger:
|
||||
logger.error("[LASER] distance_serial 未初始化")
|
||||
self.logger.error("[LASER] distance_serial 未初始化")
|
||||
return None
|
||||
|
||||
# 打印调试信息
|
||||
if logger:
|
||||
logger.info(f"[LASER] 发送开启命令: {config.LASER_ON_CMD.hex()}")
|
||||
self.logger.info(f"[LASER] 发送开启命令: {config.LASER_ON_CMD.hex()}")
|
||||
|
||||
# 清空接收缓冲区
|
||||
try:
|
||||
@@ -140,8 +134,7 @@ class LaserManager:
|
||||
|
||||
# 发送命令
|
||||
written = hardware_manager.distance_serial.write(config.LASER_ON_CMD)
|
||||
if logger:
|
||||
logger.info(f"[LASER] 写入字节数: {written}")
|
||||
self.logger.info(f"[LASER] 写入字节数: {written}")
|
||||
|
||||
# return None
|
||||
|
||||
@@ -149,29 +142,23 @@ class LaserManager:
|
||||
# 读取回包
|
||||
resp = hardware_manager.distance_serial.read(len=20,timeout=10)
|
||||
if resp:
|
||||
if logger:
|
||||
logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||||
self.logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||||
if resp == config.LASER_ON_CMD:
|
||||
if logger:
|
||||
logger.info("✅ 激光开启指令已确认")
|
||||
self.logger.info("✅ 激光开启指令已确认")
|
||||
else:
|
||||
if logger:
|
||||
logger.warning("🔇 无回包（可能正常或模块不支持回包）")
|
||||
self.logger.warning("🔇 无回包（可能正常或模块不支持回包）")
|
||||
return resp
|
||||
|
||||
def turn_off_laser(self):
|
||||
"""发送指令关闭激光"""
|
||||
from hardware import hardware_manager
|
||||
logger = logger_manager.logger
|
||||
|
||||
if hardware_manager.distance_serial is None:
|
||||
if logger:
|
||||
logger.error("[LASER] distance_serial 未初始化")
|
||||
self.logger.error("[LASER] distance_serial 未初始化")
|
||||
return None
|
||||
|
||||
# 打印调试信息
|
||||
if logger:
|
||||
logger.info(f"[LASER] 发送关闭命令: {config.LASER_OFF_CMD.hex()}")
|
||||
self.logger.info(f"[LASER] 发送关闭命令: {config.LASER_OFF_CMD.hex()}")
|
||||
|
||||
# 清空接收缓冲区
|
||||
try:
|
||||
@@ -181,18 +168,15 @@ class LaserManager:
|
||||
|
||||
# 发送命令
|
||||
written = hardware_manager.distance_serial.write(config.LASER_OFF_CMD)
|
||||
if logger:
|
||||
logger.info(f"[LASER] 写入字节数: {written}")
|
||||
self.logger.info(f"[LASER] 写入字节数: {written}")
|
||||
|
||||
|
||||
# 读取回包
|
||||
resp = hardware_manager.distance_serial.read(20)
|
||||
if resp:
|
||||
if logger:
|
||||
logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||||
self.logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||||
else:
|
||||
if logger:
|
||||
logger.warning("🔇 无回包")
|
||||
self.logger.warning("🔇 无回包")
|
||||
return resp
|
||||
# 不用读回包
|
||||
# return None
|
||||
@@ -204,9 +188,7 @@ class LaserManager:
|
||||
time.sleep_ms(duration_ms)
|
||||
self.turn_off_laser()
|
||||
except Exception as e:
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.error(f"闪激光失败: {e}")
|
||||
self.logger.error(f"闪激光失败: {e}")
|
||||
|
||||
# def find_red_laser(self, frame, threshold=150):
|
||||
# """在图像中查找最亮的红色激光点（基于 RGB 阈值）"""
|
||||
@@ -349,7 +331,7 @@ class LaserManager:
|
||||
import cv2
|
||||
import numpy as np
|
||||
from maix import image
|
||||
logger_manager.logger.debug(f"find_red_laser_with_ellipse start: {time.ticks_ms()}")
|
||||
self.logger.debug(f"find_red_laser_with_ellipse start: {time.ticks_ms()}")
|
||||
# 使用配置项
|
||||
if threshold is None:
|
||||
threshold = config.LASER_DETECTION_THRESHOLD
|
||||
@@ -360,7 +342,6 @@ class LaserManager:
|
||||
overexposed_threshold = config.LASER_OVEREXPOSED_THRESHOLD
|
||||
overexposed_diff = config.LASER_OVEREXPOSED_DIFF
|
||||
|
||||
logger = logger_manager.logger
|
||||
w, h = frame.width(), frame.height()
|
||||
center_x, center_y = w // 2, h // 2
|
||||
|
||||
@@ -376,8 +357,7 @@ class LaserManager:
|
||||
# 提取ROI区域（只处理搜索区域，而不是整个图像）
|
||||
roi = img_cv[y_min:y_max, x_min:x_max]
|
||||
if roi.size == 0:
|
||||
if logger:
|
||||
logger.debug("[LASER] ROI区域为空")
|
||||
self.logger.debug("[LASER] ROI区域为空")
|
||||
return None
|
||||
|
||||
# 分离RGB通道（向量化操作，比循环快得多）
|
||||
@@ -408,15 +388,14 @@ class LaserManager:
|
||||
# 转换为uint8格式
|
||||
mask_roi = mask_combined.astype(np.uint8) * 255
|
||||
|
||||
logger_manager.logger.debug(f"ellipse fitting start: {time.ticks_ms()}")
|
||||
self.logger.debug(f"ellipse fitting start: {time.ticks_ms()}")
|
||||
# 查找轮廓（只在搜索区域内）
|
||||
contours, _ = cv2.findContours(mask_roi, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
|
||||
logger_manager.logger.debug(f"ellipse fitting end: {time.ticks_ms()}")
|
||||
self.logger.debug(f"ellipse fitting end: {time.ticks_ms()}")
|
||||
if not contours:
|
||||
if logger:
|
||||
logger.debug("[LASER] 未找到红色像素区域")
|
||||
self.logger.debug("[LASER] 未找到红色像素区域")
|
||||
return None
|
||||
logger_manager.logger.debug(f"ellipse filtering start: {time.ticks_ms()}")
|
||||
self.logger.debug(f"ellipse filtering start: {time.ticks_ms()}")
|
||||
# 找到最大的轮廓（应该是激光点）
|
||||
largest_contour = max(contours, key=cv2.contourArea)
|
||||
|
||||
@@ -424,8 +403,7 @@ class LaserManager:
|
||||
area = cv2.contourArea(largest_contour)
|
||||
min_area = config.LASER_MIN_AREA
|
||||
if area < min_area:
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 红色区域太小（面积={area:.1f}），可能是噪声（最小={min_area}）")
|
||||
self.logger.debug(f"[LASER] 红色区域太小（面积={area:.1f}），可能是噪声（最小={min_area}）")
|
||||
return None
|
||||
|
||||
# 使用椭圆拟合找到中心
|
||||
@@ -446,8 +424,7 @@ class LaserManager:
|
||||
(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)
|
||||
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 外层红色椭圆拟合成功: 中心=({x_outer:.1f}, {y_outer:.1f}), 尺寸=({width_outer:.1f}, {height_outer:.1f}), 角度={angle_outer:.1f}°, 面积={area:.1f}")
|
||||
self.logger.debug(f"[LASER] 外层红色椭圆拟合成功: 中心=({x_outer:.1f}, {y_outer:.1f}), 尺寸=({width_outer:.1f}, {height_outer:.1f}), 角度={angle_outer:.1f}°, 面积={area:.1f}")
|
||||
|
||||
# 第二步：在外层椭圆区域内，找亮度最高的像素
|
||||
# 创建外层椭圆的掩码
|
||||
@@ -490,7 +467,7 @@ class LaserManager:
|
||||
(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))
|
||||
logger.debug(f"[LASER] 内层亮度椭圆拟合成功: 中心=({x_inner:.1f}, {y_inner:.1f}), 尺寸=({width_inner:.1f}, {height_inner:.1f}), 角度={angle_inner:.1f}°, 面积={brightness_area:.1f}")
|
||||
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)
|
||||
@@ -498,12 +475,11 @@ class LaserManager:
|
||||
cx = M["m10"] / M["m00"]
|
||||
cy = M["m01"] / M["m00"]
|
||||
laser_center = (float(cx), float(cy))
|
||||
logger.debug(f"[LASER] 内层亮度椭圆拟合失败，使用质心: {laser_center}, 错误: {e}")
|
||||
self.logger.debug(f"[LASER] 内层亮度椭圆拟合失败，使用质心: {laser_center}, 错误: {e}")
|
||||
else:
|
||||
# 质心计算失败，使用外层椭圆中心
|
||||
laser_center = (float(x_outer), float(y_outer))
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 内层区域质心计算失败，使用外层椭圆中心: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 内层区域质心计算失败，使用外层椭圆中心: {laser_center}")
|
||||
elif brightness_area >= 1:
|
||||
# 面积太小，使用质心
|
||||
M = cv2.moments(largest_brightness_contour)
|
||||
@@ -511,26 +487,23 @@ class LaserManager:
|
||||
cx = M["m10"] / M["m00"]
|
||||
cy = M["m01"] / M["m00"]
|
||||
laser_center = (float(cx), float(cy))
|
||||
logger.debug(f"[LASER] 内层区域质心计算成功: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 内层区域质心计算成功: {laser_center}")
|
||||
else:
|
||||
# 质心计算失败，使用外层椭圆中心
|
||||
laser_center = (float(x_outer), float(y_outer))
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 内层区域质心计算失败，使用外层椭圆中心: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 内层区域质心计算失败，使用外层椭圆中心: {laser_center}")
|
||||
else:
|
||||
# brightness_area < 1，面积太小，直接使用外层椭圆中心
|
||||
laser_center = (float(x_outer), float(y_outer))
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 内层亮度区域面积太小（{brightness_area:.1f}），使用外层椭圆中心: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 内层亮度区域面积太小（{brightness_area:.1f}），使用外层椭圆中心: {laser_center}")
|
||||
else:
|
||||
# 没有找到亮度轮廓，使用外层椭圆中心
|
||||
laser_center = (float(x_outer), float(y_outer))
|
||||
logger.debug(f"[LASER] 外层椭圆区域内无有效亮度值，使用外层椭圆中心: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 外层椭圆区域内无有效亮度值，使用外层椭圆中心: {laser_center}")
|
||||
else:
|
||||
# 没有亮度值，使用外层椭圆中心
|
||||
laser_center = (float(x_outer), float(y_outer))
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 外层椭圆区域内无有效亮度值，使用外层椭圆中心: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 外层椭圆区域内无有效亮度值，使用外层椭圆中心: {laser_center}")
|
||||
|
||||
# 如果启用绘制椭圆，在图像上绘制
|
||||
if config.LASER_DRAW_ELLIPSE:
|
||||
@@ -568,11 +541,10 @@ class LaserManager:
|
||||
from maix import image
|
||||
self._last_frame_with_ellipse = image.cv2image(img_cv, False, False)
|
||||
|
||||
if logger:
|
||||
if inner_ellipse_params:
|
||||
logger.debug(f"[LASER] 已绘制双层椭圆: 外层(绿色)中心=({cx_outer}, {cy_outer}), 内层(黄色)中心=({cx_inner}, {cy_inner})")
|
||||
else:
|
||||
logger.debug(f"[LASER] 已绘制外层椭圆: 中心=({cx_outer}, {cy_outer})")
|
||||
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
|
||||
# 椭圆拟合失败，使用质心
|
||||
@@ -581,8 +553,8 @@ class LaserManager:
|
||||
cx = M["m10"] / M["m00"]
|
||||
cy = M["m01"] / M["m00"]
|
||||
laser_center = (float(cx), float(cy))
|
||||
logger.debug(f"[LASER] 椭圆拟合失败，使用质心: {laser_center}, 错误: {e}")
|
||||
logger_manager.logger.debug(f"ellipse filtering start: {time.ticks_ms()}")
|
||||
self.logger.debug(f"[LASER] 椭圆拟合失败，使用质心: {laser_center}, 错误: {e}")
|
||||
self.logger.debug(f"ellipse filtering start: {time.ticks_ms()}")
|
||||
else:
|
||||
# 点太少，使用质心
|
||||
contour_global = largest_contour.copy()
|
||||
@@ -595,8 +567,7 @@ class LaserManager:
|
||||
cx = M["m10"] / M["m00"]
|
||||
cy = M["m01"] / M["m00"]
|
||||
laser_center = (float(cx), float(cy))
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 点太少（{len(largest_contour)}个），使用质心: {laser_center}")
|
||||
self.logger.debug(f"[LASER] 点太少（{len(largest_contour)}个），使用质心: {laser_center}")
|
||||
|
||||
if laser_center is None:
|
||||
# 清除之前保存的椭圆图像
|
||||
@@ -611,8 +582,7 @@ class LaserManager:
|
||||
|
||||
max_distance = config.LASER_MAX_DISTANCE_FROM_CENTER
|
||||
if distance_from_center_final > max_distance:
|
||||
if logger:
|
||||
logger.warning(f"[LASER] 激光点距离中心太远: 位置={laser_center}, "
|
||||
self.logger.warning(f"[LASER] 激光点距离中心太远: 位置={laser_center}, "
|
||||
f"距离中心={distance_from_center_final:.1f}像素, "
|
||||
f"最大允许距离={max_distance}像素")
|
||||
return None
|
||||
@@ -620,22 +590,20 @@ class LaserManager:
|
||||
# 检查是否在黄心椭圆范围内（如果启用）
|
||||
if config.LASER_REQUIRE_IN_ELLIPSE and ellipse_params is not None:
|
||||
if not self._is_point_in_ellipse(laser_center, ellipse_params):
|
||||
if logger:
|
||||
(ell_center, (ell_width, ell_height), ell_angle) = ellipse_params
|
||||
logger.warning(f"[LASER] 激光点不在黄心椭圆内: 位置={laser_center}, "
|
||||
(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
|
||||
|
||||
if logger:
|
||||
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", 椭圆检查: 已禁用"
|
||||
logger.debug(f"[LASER] 找到激光点（椭圆拟合）: 位置={laser_center}, "
|
||||
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:
|
||||
logger.debug(f"[LASER] 已保存绘制了椭圆的图像，可通过 get_last_frame_with_ellipse() 获取")
|
||||
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
|
||||
|
||||
@@ -665,7 +633,6 @@ class LaserManager:
|
||||
overexposed_threshold = config.LASER_OVEREXPOSED_THRESHOLD
|
||||
overexposed_diff = config.LASER_OVEREXPOSED_DIFF
|
||||
|
||||
logger = logger_manager.logger
|
||||
w, h = frame.width(), frame.height()
|
||||
center_x, center_y = w // 2, h // 2
|
||||
|
||||
@@ -731,13 +698,12 @@ class LaserManager:
|
||||
|
||||
# 如果没有找到候选点，输出调试信息
|
||||
if candidate_pos is None:
|
||||
if logger:
|
||||
if best_near_red:
|
||||
logger.debug(f"[LASER] 未找到激光点，最接近的点: 位置={best_near_red}, RGB={best_near_red_rgb}, "
|
||||
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:
|
||||
logger.debug(f"[LASER] 未找到激光点，搜索区域: ({x_min}, {y_min}) 到 ({x_max}, {y_max}), "
|
||||
else:
|
||||
self.logger.debug(f"[LASER] 未找到激光点，搜索区域: ({x_min}, {y_min}) 到 ({x_max}, {y_max}), "
|
||||
f"阈值={threshold}, 倍数要求={red_ratio}")
|
||||
return None
|
||||
|
||||
@@ -790,8 +756,7 @@ class LaserManager:
|
||||
max_distance = config.LASER_MAX_DISTANCE_FROM_CENTER
|
||||
if distance_from_center_final > max_distance:
|
||||
# 距离中心太远，拒绝这个结果
|
||||
if logger:
|
||||
logger.warning(f"[LASER] 找到的激光点距离中心太远: 位置={best_pos}, "
|
||||
self.logger.warning(f"[LASER] 找到的激光点距离中心太远: 位置={best_pos}, "
|
||||
f"距离中心={distance_from_center_final:.1f}像素, "
|
||||
f"最大允许距离={max_distance}像素, 拒绝此结果")
|
||||
return None
|
||||
@@ -800,21 +765,19 @@ class LaserManager:
|
||||
if config.LASER_REQUIRE_IN_ELLIPSE and ellipse_params is not None:
|
||||
if not self._is_point_in_ellipse(best_pos, ellipse_params):
|
||||
# 不在椭圆内，拒绝这个结果
|
||||
if logger:
|
||||
(ell_center, (ell_width, ell_height), ell_angle) = ellipse_params
|
||||
logger.warning(f"[LASER] 找到的激光点不在黄心椭圆内: 位置={best_pos}, "
|
||||
(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
|
||||
|
||||
# 输出成功找到激光点的日志
|
||||
if logger:
|
||||
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", 椭圆检查: 已禁用"
|
||||
logger.debug(f"[LASER] 找到激光点: 位置={best_pos}, RGB={best_rgb}, "
|
||||
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}")
|
||||
|
||||
@@ -857,7 +820,6 @@ class LaserManager:
|
||||
import vision
|
||||
from maix import time
|
||||
|
||||
logger = logger_manager.logger
|
||||
start = time.ticks_ms()
|
||||
|
||||
# 注意：使用 abs(time.ticks_diff(start, time.ticks_ms())) 避免负数问题
|
||||
@@ -875,8 +837,7 @@ class LaserManager:
|
||||
|
||||
# 只有检测到靶心时才继续处理激光点
|
||||
if center_temp is None or radius_temp is None:
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 未检测到靶心，跳过")
|
||||
self.logger.debug(f"[LASER] 未检测到靶心，跳过")
|
||||
time.sleep_ms(60)
|
||||
continue
|
||||
|
||||
@@ -898,15 +859,13 @@ class LaserManager:
|
||||
|
||||
if laser_pos is None:
|
||||
# 未找到激光点
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 未找到激光点，跳过")
|
||||
self.logger.debug(f"[LASER] 未找到激光点，跳过")
|
||||
time.sleep_ms(60)
|
||||
continue
|
||||
|
||||
if not is_sharp:
|
||||
# 激光点模糊
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 激光点模糊（清晰度: {sharpness_score:.2f}），跳过")
|
||||
self.logger.debug(f"[LASER] 激光点模糊（清晰度: {sharpness_score:.2f}），跳过")
|
||||
time.sleep_ms(60)
|
||||
continue
|
||||
|
||||
@@ -914,8 +873,7 @@ class LaserManager:
|
||||
pos = laser_pos
|
||||
|
||||
except Exception as e:
|
||||
if logger:
|
||||
logger.warning(f"[LASER] 激光点清晰度检测失败: {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)
|
||||
@@ -943,37 +901,33 @@ class LaserManager:
|
||||
|
||||
vision.save_calibration_image(frame_to_save, pos)
|
||||
except Exception as e:
|
||||
if logger:
|
||||
logger.error(f"[LASER] 保存校准图像失败: {e}")
|
||||
self.logger.error(f"[LASER] 保存校准图像失败: {e}")
|
||||
|
||||
# 设置结果、停止校准、保存坐标
|
||||
self.set_calibration_result(pos)
|
||||
self.stop_calibration()
|
||||
self.save_laser_point(pos)
|
||||
|
||||
if logger:
|
||||
if sharpness_score is not None:
|
||||
logger.info(f"✅ 校准成功: {pos} (清晰度: {sharpness_score:.2f}, 靶心: {center_temp}, 半径: {radius_temp})")
|
||||
else:
|
||||
logger.info(f"✅ 校准成功: {pos} (靶心: {center_temp}, 半径: {radius_temp})")
|
||||
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:
|
||||
if logger:
|
||||
logger.error(f"[LASER] 校准过程异常: {e}")
|
||||
import traceback
|
||||
logger.error(traceback.format_exc())
|
||||
self.logger.error(f"[LASER] 校准过程异常: {e}")
|
||||
import traceback
|
||||
self.logger.error(traceback.format_exc())
|
||||
time.sleep_ms(200)
|
||||
|
||||
# 超时或校准被停止
|
||||
if logger:
|
||||
if self._calibration_active:
|
||||
logger.warning(f"[LASER] 校准超时（{timeout_ms}ms）")
|
||||
else:
|
||||
logger.info("[LASER] 校准已停止")
|
||||
if self._calibration_active:
|
||||
self.logger.warning(f"[LASER] 校准超时（{timeout_ms}ms）")
|
||||
else:
|
||||
self.logger.info("[LASER] 校准已停止")
|
||||
|
||||
return None
|
||||
|
||||
@@ -1012,9 +966,7 @@ class LaserManager:
|
||||
distance_int = int(hex_string)
|
||||
return distance_int / 1000.0
|
||||
except Exception as e:
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.error(f"[LASER] BCD 解析失败: {e}")
|
||||
self.logger.error(f"[LASER] BCD 解析失败: {e}")
|
||||
return 0.0
|
||||
|
||||
def read_distance_from_laser_sensor(self):
|
||||
@@ -1022,11 +974,8 @@ class LaserManager:
|
||||
返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0)
|
||||
"""
|
||||
from hardware import hardware_manager
|
||||
logger = logger_manager.logger
|
||||
|
||||
if hardware_manager.distance_serial is None:
|
||||
if logger:
|
||||
logger.error("[LASER] distance_serial 未初始化")
|
||||
self.logger.error("[LASER] distance_serial 未初始化")
|
||||
return (0.0, 0)
|
||||
|
||||
try:
|
||||
@@ -1057,8 +1006,7 @@ class LaserManager:
|
||||
# 注意：使用 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:
|
||||
if logger:
|
||||
logger.warning(f"[LASER] 读取超时 ({elapsed_ms}ms)，未收到完整响应")
|
||||
self.logger.warning(f"[LASER] 读取超时 ({elapsed_ms}ms)，未收到完整响应")
|
||||
return (0.0, 0)
|
||||
|
||||
# 尝试读取数据
|
||||
@@ -1067,8 +1015,7 @@ class LaserManager:
|
||||
# 如果读到完整数据，立即返回
|
||||
if response and len(response) == config.DISTANCE_RESPONSE_LEN:
|
||||
elapsed_ms = abs(time.ticks_diff(start_time, time.ticks_ms()))
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 收到响应 ({elapsed_ms}ms)")
|
||||
self.logger.debug(f"[LASER] 收到响应 ({elapsed_ms}ms)")
|
||||
break
|
||||
|
||||
# 如果还没超时，短暂等待后继续尝试
|
||||
@@ -1079,8 +1026,7 @@ class LaserManager:
|
||||
if response[3] != 0x20:
|
||||
if response[0] == 0xEE:
|
||||
err_code = (response[7] << 8) | response[8]
|
||||
if logger:
|
||||
logger.warning(f"[LASER] 模块错误代码: {hex(err_code)}")
|
||||
self.logger.warning(f"[LASER] 模块错误代码: {hex(err_code)}")
|
||||
return (0.0, 0)
|
||||
|
||||
# 解析BCD码距离
|
||||
@@ -1088,17 +1034,13 @@ class LaserManager:
|
||||
distance_value_m = self.parse_bcd_distance(bcd_bytes)
|
||||
signal_quality = (response[10] << 8) | response[11]
|
||||
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 测距成功: {distance_value_m:.3f} m, 信号质量: {signal_quality}")
|
||||
self.logger.debug(f"[LASER] 测距成功: {distance_value_m:.3f} m, 信号质量: {signal_quality}")
|
||||
return (distance_value_m, signal_quality)
|
||||
|
||||
if logger:
|
||||
logger.warning(f"[LASER] 无效响应: {response.hex() if response else 'None'}")
|
||||
self.logger.warning(f"[LASER] 无效响应: {response.hex() if response else 'None'}")
|
||||
return (0.0, 0)
|
||||
except Exception as e:
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.error(f"[LASER] 读取激光测距失败: {e}")
|
||||
self.logger.error(f"[LASER] 读取激光测距失败: {e}")
|
||||
return (0.0, 0)
|
||||
|
||||
def calculate_laser_point_from_distance(self, distance_m):
|
||||
@@ -1126,9 +1068,7 @@ class LaserManager:
|
||||
laser_x = center_x
|
||||
laser_y = center_y + int(pixel_offset_y)
|
||||
|
||||
logger = logger_manager.logger
|
||||
if logger:
|
||||
logger.debug(f"[LASER] 根据距离 {distance_m:.2f}m 计算激光点: ({laser_x}, {laser_y}), 像素偏移: {pixel_offset_y:.2f}")
|
||||
self.logger.debug(f"[LASER] 根据距离 {distance_m:.2f}m 计算激光点: ({laser_x}, {laser_y}), 像素偏移: {pixel_offset_y:.2f}")
|
||||
|
||||
return (laser_x, laser_y)
|
||||
|
||||
@@ -1166,10 +1106,10 @@ class LaserManager:
|
||||
lx, ly = laser_point
|
||||
# r = 22.16 * 5
|
||||
r = radius * 5
|
||||
logger_manager.logger.debug(f"compute_laser_position: circle_center: {circle_center} laser_point: {laser_point} radius: {radius} method: {method} r: {r}")
|
||||
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
|
||||
logger_manager.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)}")
|
||||
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)
|
||||
|
||||
# # 根据检测方法动态调整靶心物理半径（简化模型）
|
||||
@@ -1184,7 +1124,6 @@ class LaserManager:
|
||||
激光开启时间最小化（约500-600ms），尽量不让用户觉察到
|
||||
返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0)
|
||||
"""
|
||||
logger = logger_manager.logger
|
||||
self._laser_turned_on = False
|
||||
|
||||
try:
|
||||
@@ -1197,8 +1136,7 @@ class LaserManager:
|
||||
result = self.read_distance_from_laser_sensor()
|
||||
return result
|
||||
except Exception as e:
|
||||
if logger:
|
||||
logger.error(f"[LASER] 快速测距异常: {e}")
|
||||
self.logger.error(f"[LASER] 快速测距异常: {e}")
|
||||
return (0.0, 0)
|
||||
finally:
|
||||
# 确保激光关闭
|
||||
@@ -1206,8 +1144,7 @@ class LaserManager:
|
||||
try:
|
||||
self.turn_off_laser()
|
||||
except Exception as e:
|
||||
if logger:
|
||||
logger.error(f"[LASER] 关闭激光失败: {e}")
|
||||
self.logger.error(f"[LASER] 关闭激光失败: {e}")
|
||||
|
||||
|
||||
# 创建全局单例实例
|
||||
|
||||
Reference in New Issue
Block a user