561 lines
21 KiB
Python
561 lines
21 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
|
||
|
||
|
||
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._initialized = True
|
||
|
||
# ==================== 状态访问（只读属性）====================
|
||
|
||
@property
|
||
def calibration_active(self):
|
||
"""是否正在校准"""
|
||
return self._calibration_active
|
||
|
||
@property
|
||
def laser_point(self):
|
||
"""当前激光点"""
|
||
return self._laser_point
|
||
|
||
# ==================== 业务方法 ====================
|
||
|
||
def load_laser_point(self):
|
||
"""从配置文件加载激光中心点，失败则使用默认值"""
|
||
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]))
|
||
logger = logger_manager.logger
|
||
if logger:
|
||
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):
|
||
"""保存激光中心点到配置文件"""
|
||
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:
|
||
logger = logger_manager.logger
|
||
if logger:
|
||
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 未初始化")
|
||
return None
|
||
|
||
# 打印调试信息
|
||
if logger:
|
||
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)
|
||
if logger:
|
||
logger.info(f"[LASER] 写入字节数: {written}")
|
||
|
||
# return None
|
||
|
||
# TODO: 暂时去掉这个等待
|
||
# 读取回包
|
||
print("before read:", time.ticks_ms())
|
||
resp = hardware_manager.distance_serial.read(len=20,timeout=10)
|
||
print("after read:", time.ticks_ms())
|
||
if resp:
|
||
if logger:
|
||
logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||
if resp == config.LASER_ON_CMD:
|
||
if logger:
|
||
logger.info("✅ 激光开启指令已确认")
|
||
else:
|
||
if logger:
|
||
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 未初始化")
|
||
return None
|
||
|
||
# 打印调试信息
|
||
if logger:
|
||
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)
|
||
if logger:
|
||
logger.info(f"[LASER] 写入字节数: {written}")
|
||
|
||
|
||
# 读取回包
|
||
resp = hardware_manager.distance_serial.read(20)
|
||
if resp:
|
||
if logger:
|
||
logger.info(f"[LASER] 收到回包 ({len(resp)}字节): {resp.hex()}")
|
||
else:
|
||
if logger:
|
||
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:
|
||
logger = logger_manager.logger
|
||
if logger:
|
||
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 find_red_laser(self, frame, threshold=150, search_radius=150):
|
||
"""
|
||
在图像中心附近查找最亮的红色激光点（基于 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
|
||
candidate_pos = 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：正常红色（r 明显大于 g 和 b）
|
||
if r > threshold and r > g * 2 and r > b * 2:
|
||
is_red = True
|
||
|
||
# 情况2：过曝的红色（发白，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
|
||
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)
|
||
|
||
# 如果没有找到候选点，直接返回
|
||
if candidate_pos is None:
|
||
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
|
||
|
||
# 精细搜索：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 * 2 and r > b * 2:
|
||
is_red = True
|
||
elif r > 200 and g > 200 and b > 200:
|
||
if r >= g and r >= b and (r - g) > 10 and (r - b) > 10:
|
||
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)
|
||
|
||
return best_pos
|
||
|
||
def calibrate_laser_position(self):
|
||
"""执行一次激光校准：拍照 → 找红点 → 保存坐标"""
|
||
time.sleep_ms(80)
|
||
cam = camera.Camera(640, 480)
|
||
frame = cam.read()
|
||
pos = self.find_red_laser(frame)
|
||
if pos:
|
||
self.save_laser_point(pos)
|
||
return pos
|
||
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:
|
||
logger = logger_manager.logger
|
||
if logger:
|
||
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
|
||
logger = logger_manager.logger
|
||
|
||
if hardware_manager.distance_serial is None:
|
||
if logger:
|
||
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:
|
||
if logger:
|
||
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()))
|
||
if logger:
|
||
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]
|
||
if logger:
|
||
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]
|
||
|
||
if logger:
|
||
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'}")
|
||
return (0.0, 0)
|
||
except Exception as e:
|
||
logger = logger_manager.logger
|
||
if logger:
|
||
logger.error(f"[LASER] 读取激光测距失败: {e}")
|
||
return (0.0, 0)
|
||
|
||
def quick_measure_distance(self):
|
||
"""
|
||
快速激光测距：打开激光 → 测距 → 关闭激光
|
||
激光开启时间最小化（约500-600ms），尽量不让用户觉察到
|
||
返回: (distance_m, signal_quality) 元组，失败返回 (0.0, 0)
|
||
"""
|
||
logger = logger_manager.logger
|
||
self._laser_turned_on = False
|
||
|
||
try:
|
||
|
||
|
||
# 等待激光稳定（最小延迟）
|
||
# time.sleep_ms(50)
|
||
|
||
# 读取距离和信号质量
|
||
result = self.read_distance_from_laser_sensor()
|
||
return result
|
||
except Exception as e:
|
||
if logger:
|
||
logger.error(f"[LASER] 快速测距异常: {e}")
|
||
return (0.0, 0)
|
||
finally:
|
||
# 确保激光关闭
|
||
if self._laser_turned_on:
|
||
try:
|
||
self.turn_off_laser()
|
||
except Exception as e:
|
||
if logger:
|
||
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()
|
||
|