926 lines
37 KiB
Python
926 lines
37 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
FemtoBolt深度相机管理器
|
||
负责FemtoBolt深度相机的连接、配置和深度图像数据采集
|
||
"""
|
||
|
||
import os
|
||
import sys
|
||
import threading
|
||
import time
|
||
import base64
|
||
import numpy as np
|
||
import cv2
|
||
from typing import Optional, Dict, Any, Tuple
|
||
import logging
|
||
from collections import deque
|
||
import gc
|
||
from matplotlib.colors import LinearSegmentedColormap
|
||
import matplotlib.pyplot as plt
|
||
import matplotlib
|
||
from scipy import ndimage
|
||
from scipy.interpolate import griddata
|
||
import io
|
||
import importlib
|
||
from typing import Optional
|
||
import subprocess
|
||
import tempfile
|
||
|
||
try:
|
||
from .base_device import BaseDevice
|
||
from .utils.socket_manager import SocketManager
|
||
from .utils.config_manager import ConfigManager
|
||
except ImportError:
|
||
from base_device import BaseDevice
|
||
from utils.socket_manager import SocketManager
|
||
from utils.config_manager import ConfigManager
|
||
|
||
|
||
class FemtoBoltManager(BaseDevice):
|
||
"""FemtoBolt深度相机管理器"""
|
||
|
||
def __init__(self, socketio, config_manager: Optional[ConfigManager] = None):
|
||
"""
|
||
初始化FemtoBolt管理器
|
||
|
||
Args:
|
||
socketio: SocketIO实例
|
||
config_manager: 配置管理器实例
|
||
"""
|
||
# 配置管理
|
||
self.config_manager = config_manager or ConfigManager()
|
||
self.config = self.config_manager.get_device_config('femtobolt')
|
||
|
||
# 调用父类初始化
|
||
super().__init__("femtobolt", self.config)
|
||
|
||
# 设置SocketIO实例
|
||
self.set_socketio(socketio)
|
||
|
||
# 设备信息字典
|
||
self.device_info = {}
|
||
|
||
# 设备ID
|
||
self.device_id = "femtobolt_001"
|
||
|
||
# 性能统计
|
||
self.performance_stats = {
|
||
'fps': 0.0,
|
||
'frame_count': 0,
|
||
'dropped_frames': 0,
|
||
'processing_time': 0.0
|
||
}
|
||
|
||
# FemtoBolt SDK相关
|
||
self.femtobolt = None
|
||
self.device_handle = None
|
||
self.sdk_initialized = False
|
||
# 新增：记录已使用的k4a.dll路径，供子进程探测使用
|
||
self.k4a_dll_path: Optional[str] = None
|
||
|
||
# 设备配置
|
||
self.algorithm_type = self.config.get('algorithm_type', 'opencv')
|
||
self.color_resolution = self.config.get('color_resolution', '1080P')
|
||
self.depth_mode = self.config.get('depth_mode', 'NFOV_2X2BINNED')
|
||
self.color_format = self.config.get('color_format', 'COLOR_BGRA32')
|
||
self.fps = self.config.get('camera_fps', 20)
|
||
self.depth_range_min = self.config.get('depth_range_min', 500)
|
||
self.depth_range_max = self.config.get('depth_range_max', 4500)
|
||
self.synchronized_images_only = self.config.get('synchronized_images_only', False)
|
||
|
||
# 数据处理
|
||
self.streaming_thread = None
|
||
self.depth_frame_cache = deque(maxlen=10)
|
||
self.color_frame_cache = deque(maxlen=10)
|
||
self.last_depth_frame = None
|
||
self.last_color_frame = None
|
||
self.frame_count = 0
|
||
|
||
# 图像处理参数
|
||
self.contrast_factor = 1.2
|
||
self.gamma_value = 0.8
|
||
self.use_pseudo_color = True
|
||
|
||
# 性能监控
|
||
self.fps_counter = 0
|
||
|
||
# 图像渲染缓存
|
||
self.background = None
|
||
self.output_buffer = None
|
||
self._depth_filtered = None # 用于复用深度图过滤结果
|
||
self._blur_buffer = None # 用于复用高斯模糊结果
|
||
self._current_gamma = None
|
||
self.fps_start_time = time.time()
|
||
self.actual_fps = 0
|
||
self.dropped_frames = 0
|
||
|
||
# 重连机制
|
||
self.max_reconnect_attempts = 3
|
||
self.reconnect_delay = 3.0
|
||
|
||
# 发送频率控制（内存优化）
|
||
self.send_fps = self.config.get('send_fps', 20) # 默认20FPS发送
|
||
self._min_send_interval = 1.0 / self.send_fps if self.send_fps > 0 else 0.05
|
||
self._last_send_time = 0
|
||
|
||
# 编码参数缓存（避免每帧创建数组）
|
||
self._encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), int(self.config.get('jpeg_quality', 60))]
|
||
|
||
# 预计算伽马LUT（避免每帧计算）
|
||
self._gamma_lut = None
|
||
self._current_gamma = None
|
||
self._update_gamma_lut()
|
||
|
||
# 预生成网格背景（避免每帧创建）
|
||
self._grid_bg = None
|
||
self._grid_size = (480, 640) # 默认尺寸
|
||
self.background = None # 用于缓存等高线渲染的背景
|
||
|
||
# 自定义彩虹色 colormap（参考testfemtobolt.py）
|
||
colors = ['fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue',
|
||
'fuchsia', 'red', 'yellow', 'lime', 'cyan', 'blue']
|
||
self.custom_cmap = LinearSegmentedColormap.from_list("custom_cmap", colors)
|
||
|
||
# 设置matplotlib为非交互模式
|
||
matplotlib.use('Agg')
|
||
|
||
# 创建matplotlib图形对象（复用以提高性能）
|
||
self.fig, self.ax = plt.subplots(figsize=(7, 7))
|
||
self.ax.set_aspect('equal')
|
||
plt.subplots_adjust(left=0, right=1, top=1, bottom=0)
|
||
self.logger.info(f"FemtoBolt设备配置完成 - 算法类型: {self.algorithm_type}, 深度模式: {self.depth_mode}, FPS: {self.fps}")
|
||
|
||
|
||
def _update_gamma_lut(self):
|
||
"""更新伽马校正查找表"""
|
||
if self._current_gamma != self.gamma_value:
|
||
self._gamma_lut = np.array([((i / 255.0) ** (1.0 / self.gamma_value)) * 255 for i in range(256)], dtype=np.uint8)
|
||
self._current_gamma = self.gamma_value
|
||
|
||
def _generate_contour_image_opencv(self, depth):
|
||
"""改进版 OpenCV 等高线渲染，梯度平滑、局部对比增强"""
|
||
try:
|
||
# 初始化 depth_filtered 缓冲区
|
||
if self._depth_filtered is None or self._depth_filtered.shape != depth.shape:
|
||
self._depth_filtered = np.zeros_like(depth, dtype=np.uint16)
|
||
|
||
np.copyto(self._depth_filtered, depth) # 直接覆盖，不生成新数组
|
||
depth_filtered = self._depth_filtered
|
||
depth_filtered[depth_filtered > self.depth_range_max] = 0
|
||
depth_filtered[depth_filtered < self.depth_range_min] = 0
|
||
height, width = depth_filtered.shape
|
||
|
||
# 背景缓存
|
||
if self.background is None or self.background.shape[:2] != (height, width):
|
||
background_gray = int(0.5 * 255 * 0.3 + 255 * (1 - 0.3))
|
||
self.background = np.ones((height, width, 3), dtype=np.uint8) * background_gray
|
||
grid_spacing = max(height // 20, width // 20, 10)
|
||
for x in range(0, width, grid_spacing):
|
||
cv2.line(self.background, (x, 0), (x, height-1), (255, 255, 255), 1)
|
||
for y in range(0, height, grid_spacing):
|
||
cv2.line(self.background, (0, y), (width-1, y), (255, 255, 255), 1)
|
||
|
||
# 初始化输出缓存和模糊缓存
|
||
self.output_buffer = np.empty_like(self.background)
|
||
self._blur_buffer = np.empty_like(self.background)
|
||
|
||
# 复用输出缓存，避免 copy()
|
||
np.copyto(self.output_buffer, self.background)
|
||
output = self.output_buffer
|
||
valid_mask = depth_filtered > 0
|
||
|
||
if np.any(valid_mask):
|
||
# 连续归一化深度值
|
||
norm_depth = np.zeros_like(depth_filtered, dtype=np.float32)
|
||
norm_depth[valid_mask] = (depth_filtered[valid_mask] - self.depth_range_min) / (self.depth_range_max - self.depth_range_min)
|
||
norm_depth = np.clip(norm_depth, 0, 1) ** 0.8 # Gamma增强
|
||
|
||
# 使用 colormap 映射
|
||
cmap_colors = (self.custom_cmap(norm_depth)[..., :3] * 255).astype(np.uint8)
|
||
output[valid_mask] = cmap_colors[valid_mask]
|
||
|
||
# Sobel 边界检测 + cv2.magnitude 替换 np.hypot
|
||
depth_uint8 = (norm_depth * 255).astype(np.uint8)
|
||
gx = cv2.Sobel(depth_uint8, cv2.CV_32F, 1, 0, ksize=3)
|
||
gy = cv2.Sobel(depth_uint8, cv2.CV_32F, 0, 1, ksize=3)
|
||
grad_mag = cv2.magnitude(gx, gy)
|
||
grad_mag = grad_mag.astype(np.uint8)
|
||
|
||
# 自适应局部对比度增强（向量化）
|
||
edge_mask = grad_mag > 30
|
||
output[edge_mask] = np.clip(output[edge_mask].astype(np.float32) * 1.5, 0, 255).astype(np.uint8)
|
||
|
||
# 高斯平滑，复用 dst 缓冲区
|
||
cv2.GaussianBlur(output, (3, 3), 0.3, dst=self._blur_buffer)
|
||
|
||
# 注意：这里不进行裁剪，而是返回完整图像
|
||
# 推迟裁剪到显示阶段，与 testfemtobolt.py 保持一致
|
||
# 原代码在这里进行了裁剪：
|
||
# target_width = height // 2
|
||
# if width > target_width:
|
||
# left = (width - target_width) // 2
|
||
# right = left + target_width
|
||
# output = output[:, left:right]
|
||
return self._blur_buffer
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"优化等高线生成失败: {e}")
|
||
return None
|
||
|
||
def _create_grid_background(self, height, width):
|
||
"""创建网格背景缓存"""
|
||
bg = np.ones((height, width, 3), dtype=np.uint8) * 128
|
||
# 绘制白色网格线
|
||
grid_spacing = 50
|
||
for x in range(0, width, grid_spacing):
|
||
cv2.line(bg, (x, 0), (x, height-1), (255, 255, 255), 1)
|
||
for y in range(0, height, grid_spacing):
|
||
cv2.line(bg, (0, y), (width-1, y), (255, 255, 255), 1)
|
||
|
||
self._grid_bg = bg
|
||
self._grid_size = (height, width)
|
||
|
||
def _generate_contour_image_plt(self, depth):
|
||
"""使用matplotlib生成等高线图像（完全采用display_x.py的逻辑）"""
|
||
try:
|
||
# 清除之前的绘图
|
||
self.ax.clear()
|
||
|
||
# 深度数据过滤（与display_x.py完全一致）
|
||
depth[depth > self.depth_range_max] = 0
|
||
depth[depth < self.depth_range_min] = 0
|
||
|
||
# 背景图（与display_x.py完全一致）
|
||
background = np.ones_like(depth) * 0.5 # 设定灰色背景
|
||
|
||
# 使用 np.ma.masked_equal() 来屏蔽深度图中的零值（与display_x.py完全一致）
|
||
depth = np.ma.masked_equal(depth, 0)
|
||
|
||
# 绘制背景（与display_x.py完全一致）
|
||
self.ax.imshow(background, origin='lower', cmap='gray', alpha=0.3)
|
||
|
||
# 绘制白色栅格线，并将其置于底层（网格密度加大一倍）
|
||
self.ax.grid(True, which='both', axis='both', color='white', linestyle='-', linewidth=0.5, zorder=0)
|
||
self.ax.minorticks_on()
|
||
self.ax.grid(True, which='minor', axis='both', color='white', linestyle='-', linewidth=0.3, zorder=0)
|
||
|
||
# 隐藏坐标轴
|
||
# self.ax.set_xticks([])
|
||
# self.ax.set_yticks([])
|
||
|
||
# 绘制等高线图并设置原点在上方（与display_x.py完全一致）
|
||
import time
|
||
start_time = time.perf_counter()
|
||
self.ax.contourf(depth, levels=100, cmap=self.custom_cmap, vmin=self.depth_range_min, vmax=self.depth_range_max, origin='upper', zorder=2)
|
||
contourf_time = time.perf_counter() - start_time
|
||
# self.logger.info(f"contourf绘制耗时: {contourf_time*1000:.2f}ms")
|
||
|
||
|
||
# 将matplotlib图形转换为numpy数组
|
||
buf = io.BytesIO()
|
||
savefig_start = time.perf_counter()
|
||
savefig_start = time.perf_counter()
|
||
self.fig.savefig(buf, format='png',bbox_inches='tight', pad_inches=0, dpi=75)
|
||
savefig_time = time.perf_counter() - savefig_start
|
||
# self.logger.info(f"savefig保存耗时: {savefig_time*1000:.2f}ms")
|
||
|
||
buf_start = time.perf_counter()
|
||
buf.seek(0)
|
||
|
||
# 读取PNG数据并转换为OpenCV格式
|
||
img_array = np.frombuffer(buf.getvalue(), dtype=np.uint8)
|
||
buf.close()
|
||
buf_time = time.perf_counter() - buf_start
|
||
# self.logger.info(f"缓冲区操作耗时: {buf_time*1000:.2f}ms")
|
||
|
||
# 解码PNG图像
|
||
decode_start = time.perf_counter()
|
||
img = cv2.imdecode(img_array, cv2.IMREAD_COLOR)
|
||
decode_time = time.perf_counter() - decode_start
|
||
# self.logger.info(f"PNG解码耗时: {decode_time*1000:.2f}ms")
|
||
# return img
|
||
if img is not None:
|
||
# 裁剪宽度（与原逻辑保持一致）
|
||
height, width = img.shape[:2]
|
||
target_width = round(height // 2)
|
||
if width > target_width:
|
||
left = (width - target_width) // 2
|
||
right = left + target_width
|
||
img = img[:, left:right]
|
||
return img
|
||
else:
|
||
self.logger.error("无法解码matplotlib生成的PNG图像")
|
||
return None
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"生成等高线图像失败: {e}")
|
||
return None
|
||
|
||
def initialize(self) -> bool:
|
||
"""
|
||
初始化FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 初始化是否成功
|
||
"""
|
||
try:
|
||
self.logger.info("正在初始化FemtoBolt设备...")
|
||
|
||
# 使用构造函数中已加载的配置，避免并发读取配置文件
|
||
self.logger.info(f"使用已加载配置: algorithm_type={self.algorithm_type}, fps={self.fps}, depth_mode={self.depth_mode}")
|
||
|
||
# 初始化SDK
|
||
if not self._initialize_sdk():
|
||
raise Exception("SDK初始化失败")
|
||
|
||
# 配置设备
|
||
if not self._configure_device():
|
||
raise Exception("设备配置失败")
|
||
|
||
# 启动设备
|
||
if not self._start_device():
|
||
raise Exception("设备启动失败")
|
||
|
||
# 使用set_connected方法启动连接监控线程
|
||
self.set_connected(True)
|
||
self.device_info.update({
|
||
'color_resolution': self.color_resolution,
|
||
'depth_mode': self.depth_mode,
|
||
'camera_fps': self.fps,
|
||
'depth_range': f"{self.depth_range_min}-{self.depth_range_max}mm"
|
||
})
|
||
|
||
self.logger.info("FemtoBolt初始化成功")
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt初始化失败: {e}")
|
||
# 使用set_connected方法停止连接监控线程
|
||
self.set_connected(False)
|
||
self._cleanup_device()
|
||
return False
|
||
|
||
def _initialize_sdk(self) -> bool:
|
||
"""
|
||
初始化FemtoBolt SDK (使用pykinect_azure)
|
||
|
||
Returns:
|
||
bool: SDK初始化是否成功
|
||
"""
|
||
try:
|
||
# 尝试导入pykinect_azure
|
||
real_pykinect = None
|
||
try:
|
||
import pykinect_azure as pykinect
|
||
real_pykinect = pykinect
|
||
self.logger.info("成功导入pykinect_azure库")
|
||
except ImportError as e:
|
||
self.logger.error(f"无法导入pykinect_azure库: {e}")
|
||
self.sdk_initialized = False
|
||
return False
|
||
|
||
# 查找并初始化SDK路径
|
||
sdk_initialized = False
|
||
if real_pykinect and hasattr(real_pykinect, 'initialize_libraries'):
|
||
sdk_paths = self._get_femtobolt_sdk_paths()
|
||
for sdk_path in sdk_paths:
|
||
if os.path.exists(sdk_path):
|
||
try:
|
||
real_pykinect.initialize_libraries(track_body=False, module_k4a_path=sdk_path)
|
||
self.logger.info(f'✓ 成功使用FemtoBolt SDK: {sdk_path}')
|
||
self.pykinect = real_pykinect
|
||
sdk_initialized = True
|
||
break
|
||
except Exception as e:
|
||
self.logger.warning(f'✗ FemtoBolt SDK路径失败: {sdk_path} - {e}')
|
||
continue
|
||
|
||
if not sdk_initialized:
|
||
self.logger.error('未找到真实SDK，初始化失败')
|
||
self.sdk_initialized = False
|
||
return False
|
||
|
||
self.sdk_initialized = True
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"SDK初始化失败: {e}")
|
||
return False
|
||
|
||
def _get_femtobolt_sdk_paths(self) -> list:
|
||
import platform
|
||
sdk_paths = []
|
||
if platform.system() == "Windows":
|
||
# 优先使用Orbbec SDK K4A Wrapper（与azure_kinect_image_example.py一致）
|
||
base_dir = os.path.dirname(os.path.abspath(__file__))
|
||
dll_path = os.path.join(base_dir,"..", "dll","femtobolt", "k4a.dll")
|
||
self.logger.info(f"FemtoBolt SDK路径: {dll_path}")
|
||
sdk_paths.append(dll_path)
|
||
return sdk_paths
|
||
|
||
|
||
|
||
def _configure_device(self) -> bool:
|
||
"""
|
||
配置FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 配置是否成功
|
||
"""
|
||
try:
|
||
if not self.pykinect:
|
||
return False
|
||
|
||
# 配置FemtoBolt设备参数
|
||
self.femtobolt_config = self.pykinect.default_configuration
|
||
self.femtobolt_config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED
|
||
self.femtobolt_config.color_format = self.pykinect.K4A_IMAGE_FORMAT_COLOR_BGRA32
|
||
self.femtobolt_config.color_resolution = self.pykinect.K4A_COLOR_RESOLUTION_1080P
|
||
self.femtobolt_config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15
|
||
self.femtobolt_config.synchronized_images_only = False
|
||
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt设备配置失败: {e}")
|
||
return False
|
||
|
||
|
||
def _start_device(self) -> bool:
|
||
"""
|
||
启动FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 启动是否成功
|
||
"""
|
||
try:
|
||
if not self.pykinect:
|
||
return False
|
||
|
||
# 通过探测后再真正启动
|
||
# 在真正调用 start_device 之前，先通过 k4a.dll 查询已安装设备数量，0 则跳过
|
||
try:
|
||
import os, ctypes
|
||
k4a_path = getattr(self, "k4a_dll_path", None)
|
||
if not k4a_path:
|
||
base_dir = os.path.dirname(os.path.abspath(__file__))
|
||
k4a_path = os.path.normpath(os.path.join(base_dir, "..", "dll", "femtobolt", "k4a.dll"))
|
||
k4a = ctypes.CDLL(k4a_path)
|
||
try:
|
||
# 有些环境需要显式声明返回类型
|
||
k4a.k4a_device_get_installed_count.restype = ctypes.c_uint32
|
||
except Exception:
|
||
pass
|
||
device_count = int(k4a.k4a_device_get_installed_count())
|
||
except Exception as e:
|
||
self.logger.warning(f"获取FemtoBolt设备数量失败，跳过启动: {e}")
|
||
return False
|
||
|
||
if device_count <= 0:
|
||
self.logger.warning("未检测到FemtoBolt深度相机，跳过启动")
|
||
return False
|
||
else:
|
||
self.logger.info(f"检测到 FemtoBolt 设备数量: {device_count}")
|
||
|
||
self.device_handle = self.pykinect.start_device(config=self.femtobolt_config)
|
||
if self.device_handle:
|
||
self.logger.info('✓ FemtoBolt深度相机初始化成功!')
|
||
else:
|
||
self.logger.warning('FemtoBolt设备启动返回None，设备可能未连接')
|
||
return False
|
||
|
||
# # 等待设备稳定
|
||
# time.sleep(1.0)
|
||
|
||
# # 测试捕获（可选，失败不抛异常，只作为稳定性判断）
|
||
# try:
|
||
# if not self._test_capture():
|
||
# self.logger.warning('FemtoBolt设备捕获测试失败')
|
||
# return False
|
||
# except Exception:
|
||
# return False
|
||
|
||
self.logger.info('FemtoBolt设备启动成功')
|
||
return True
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt设备启动失败: {e}")
|
||
return False
|
||
|
||
def _test_capture(self) -> bool:
|
||
"""
|
||
测试设备捕获
|
||
|
||
Returns:
|
||
bool: 测试是否成功
|
||
"""
|
||
try:
|
||
for i in range(3):
|
||
capture = self.device_handle.update()
|
||
if capture:
|
||
ret, depth_image = capture.get_depth_image()
|
||
if ret and depth_image is not None:
|
||
self.logger.info(f"FemtoBolt捕获测试成功 - 深度图像大小: {depth_image.shape}")
|
||
return True
|
||
time.sleep(0.1)
|
||
|
||
self.logger.error("FemtoBolt捕获测试失败")
|
||
return False
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt捕获测试异常: {e}")
|
||
return False
|
||
|
||
def calibrate(self) -> bool:
|
||
"""
|
||
校准FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 校准是否成功
|
||
"""
|
||
try:
|
||
self.logger.info("开始FemtoBolt校准...")
|
||
|
||
if not self.is_connected:
|
||
if not self.initialize():
|
||
return False
|
||
|
||
# 对于FemtoBolt，校准主要是验证设备工作状态
|
||
# 捕获几帧来确保设备稳定
|
||
for i in range(10):
|
||
capture = self.device_handle.get_capture()
|
||
if capture:
|
||
depth_image = capture.get_depth_image()
|
||
if depth_image is not None:
|
||
# 检查深度图像质量
|
||
valid_pixels = np.sum((depth_image >= self.depth_range_min) &
|
||
(depth_image <= self.depth_range_max))
|
||
total_pixels = depth_image.size
|
||
valid_ratio = valid_pixels / total_pixels
|
||
|
||
if valid_ratio > 0.1: # 至少10%的像素有效
|
||
self.logger.info(f"校准帧 {i+1}: 有效像素比例 {valid_ratio:.2%}")
|
||
else:
|
||
self.logger.warning(f"校准帧 {i+1}: 有效像素比例过低 {valid_ratio:.2%}")
|
||
|
||
capture.release()
|
||
else:
|
||
self.logger.warning(f"校时时无法获取第{i+1}帧")
|
||
|
||
time.sleep(0.1)
|
||
|
||
self.logger.info("FemtoBolt校准完成")
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt校准失败: {e}")
|
||
return False
|
||
|
||
def start_streaming(self) -> bool:
|
||
"""
|
||
开始数据流推送
|
||
|
||
Returns:
|
||
bool: 启动是否成功
|
||
"""
|
||
if self.is_streaming:
|
||
self.logger.warning("FemtoBolt流已在运行")
|
||
return True
|
||
|
||
if not self.is_connected:
|
||
if not self.initialize():
|
||
return False
|
||
|
||
try:
|
||
self.is_streaming = True
|
||
self.streaming_thread = threading.Thread(
|
||
target=self._streaming_worker,
|
||
name="FemtoBolt-Stream",
|
||
daemon=True
|
||
)
|
||
self.streaming_thread.start()
|
||
|
||
self.logger.info("FemtoBolt流启动成功")
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"启动FemtoBolt流失败: {e}")
|
||
self.is_streaming = False
|
||
return False
|
||
|
||
def stop_streaming(self) -> bool:
|
||
"""
|
||
停止数据流推送
|
||
|
||
Returns:
|
||
bool: 停止是否成功
|
||
"""
|
||
try:
|
||
self.is_streaming = False
|
||
|
||
if self.streaming_thread and self.streaming_thread.is_alive():
|
||
self.streaming_thread.join(timeout=5.0)
|
||
|
||
self.logger.info("FemtoBolt流已停止")
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"停止FemtoBolt流失败: {e}")
|
||
return False
|
||
|
||
def _streaming_worker(self):
|
||
"""
|
||
流处理工作线程
|
||
"""
|
||
self.logger.info("FemtoBolt流工作线程启动")
|
||
|
||
frame_count = 0
|
||
|
||
try:
|
||
while self.is_streaming:
|
||
# 发送频率限制
|
||
now = time.time()
|
||
if now - self._last_send_time < self._min_send_interval:
|
||
time.sleep(0.001)
|
||
continue
|
||
|
||
if self.device_handle and self._socketio:
|
||
try:
|
||
capture = self.device_handle.update()
|
||
if capture is not None:
|
||
try:
|
||
ret, depth_image = capture.get_depth_image()
|
||
if ret and depth_image is not None:
|
||
|
||
# 更新心跳时间，防止连接监控线程判定为超时
|
||
self.update_heartbeat()
|
||
|
||
# 根据配置选择不同的等高线生成方法
|
||
if self.algorithm_type == 'plt':
|
||
depth_colored_final = self._generate_contour_image_plt(depth_image)
|
||
elif self.algorithm_type == 'opencv':
|
||
depth_colored_final = self._generate_contour_image_opencv(depth_image)
|
||
|
||
if depth_colored_final is None:
|
||
# 如果等高线生成失败，跳过这一帧
|
||
continue
|
||
|
||
# 裁剪处理（推迟到显示阶段）
|
||
h, w = depth_colored_final.shape[:2]
|
||
target_width = h // 2
|
||
display_image = depth_colored_final
|
||
if w > target_width:
|
||
left = (w - target_width) // 2
|
||
right = left + target_width
|
||
display_image = depth_colored_final[:, left:right]
|
||
|
||
# 推送SocketIO
|
||
success, buffer = cv2.imencode('.jpg', display_image, self._encode_param)
|
||
if success and self._socketio:
|
||
jpg_as_text = base64.b64encode(memoryview(buffer).tobytes()).decode('utf-8')
|
||
self._socketio.emit('femtobolt_frame', {
|
||
'depth_image': jpg_as_text,
|
||
'frame_count': frame_count,
|
||
'timestamp': now,
|
||
'fps': self.actual_fps,
|
||
'device_id': self.device_id,
|
||
'depth_range': {
|
||
'min': self.depth_range_min,
|
||
'max': self.depth_range_max
|
||
}
|
||
}, namespace='/devices')
|
||
frame_count += 1
|
||
self._last_send_time = now
|
||
|
||
# 更新统计
|
||
self._update_statistics()
|
||
else:
|
||
time.sleep(0.005)
|
||
except Exception as e:
|
||
# 捕获处理过程中出现异常，记录并继续
|
||
self.logger.error(f"FemtoBolt捕获处理错误: {e}")
|
||
finally:
|
||
# 无论处理成功与否，都应释放capture以回收内存:contentReference[oaicite:3]{index=3}
|
||
try:
|
||
if hasattr(capture, 'release'):
|
||
capture.release()
|
||
except Exception:
|
||
pass
|
||
else:
|
||
time.sleep(0.001)
|
||
|
||
except Exception as e:
|
||
self.logger.error(f'FemtoBolt帧推送失败: {e}')
|
||
time.sleep(0.05)
|
||
|
||
# 降低空转CPU
|
||
time.sleep(0.001)
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt流处理异常: {e}")
|
||
finally:
|
||
self.is_streaming = False
|
||
self.logger.info("FemtoBolt流工作线程结束")
|
||
|
||
|
||
def _update_statistics(self):
|
||
"""
|
||
更新性能统计
|
||
"""
|
||
self.frame_count += 1
|
||
self.fps_counter += 1
|
||
|
||
# 每秒计算一次实际FPS
|
||
current_time = time.time()
|
||
if current_time - self.fps_start_time >= 1.0:
|
||
self.actual_fps = self.fps_counter / (current_time - self.fps_start_time)
|
||
self.fps_counter = 0
|
||
self.fps_start_time = current_time
|
||
|
||
# 更新性能统计
|
||
self.performance_stats.update({
|
||
'frames_processed': self.frame_count,
|
||
'actual_fps': round(self.actual_fps, 2),
|
||
'dropped_frames': self.dropped_frames
|
||
})
|
||
|
||
def _reconnect(self) -> bool:
|
||
"""
|
||
重新连接FemtoBolt设备
|
||
|
||
Returns:
|
||
bool: 重连是否成功
|
||
"""
|
||
try:
|
||
self._cleanup_device()
|
||
time.sleep(2.0) # 等待设备释放
|
||
return self.initialize()
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"FemtoBolt重连失败: {e}")
|
||
return False
|
||
|
||
def get_status(self) -> Dict[str, Any]:
|
||
"""
|
||
获取设备状态
|
||
|
||
Returns:
|
||
Dict[str, Any]: 设备状态信息
|
||
"""
|
||
status = super().get_status()
|
||
status.update({
|
||
'color_resolution': self.color_resolution,
|
||
'depth_mode': self.depth_mode,
|
||
'target_fps': self.fps,
|
||
'actual_fps': self.actual_fps,
|
||
'frame_count': self.frame_count,
|
||
'dropped_frames': self.dropped_frames,
|
||
'depth_range': f"{self.depth_range_min}-{self.depth_range_max}mm",
|
||
'has_depth_frame': self.last_depth_frame is not None,
|
||
'has_color_frame': self.last_color_frame is not None
|
||
})
|
||
return status
|
||
|
||
|
||
|
||
def _cleanup_device(self):
|
||
"""
|
||
清理设备资源
|
||
"""
|
||
try:
|
||
if self.device_handle:
|
||
# 尝试停止设备（如果有stop方法）
|
||
if hasattr(self.device_handle, 'stop'):
|
||
try:
|
||
self.device_handle.stop()
|
||
self.logger.info("FemtoBolt设备已停止")
|
||
except Exception as e:
|
||
self.logger.warning(f"停止FemtoBolt设备时出现警告: {e}")
|
||
|
||
# 尝试关闭设备（如果有close方法）
|
||
if hasattr(self.device_handle, 'close'):
|
||
try:
|
||
self.device_handle.close()
|
||
self.logger.info("FemtoBolt设备连接已关闭")
|
||
except Exception as e:
|
||
self.logger.warning(f"关闭FemtoBolt设备时出现警告: {e}")
|
||
|
||
self.device_handle = None
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"清理FemtoBolt设备失败: {e}")
|
||
|
||
def disconnect(self):
|
||
"""
|
||
断开FemtoBolt设备连接
|
||
"""
|
||
try:
|
||
self.stop_streaming()
|
||
self._cleanup_device()
|
||
self.is_connected = False
|
||
self.logger.info("FemtoBolt设备已断开连接")
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"断开FemtoBolt设备连接失败: {e}")
|
||
|
||
def reload_config(self) -> bool:
|
||
"""
|
||
重新加载设备配置
|
||
|
||
Returns:
|
||
bool: 重新加载是否成功
|
||
"""
|
||
try:
|
||
self.logger.info("正在重新加载FemtoBolt配置...")
|
||
|
||
|
||
# 获取最新配置
|
||
self.config = self.config_manager.get_device_config('femtobolt')
|
||
|
||
# 更新配置属性
|
||
self.algorithm_type = self.config.get('algorithm_type', 'opencv')
|
||
self.color_resolution = self.config.get('color_resolution', '1080P')
|
||
self.depth_mode = self.config.get('depth_mode', 'NFOV_2X2BINNED')
|
||
self.color_format = self.config.get('color_format', 'COLOR_BGRA32')
|
||
self.fps = self.config.get('camera_fps', 20)
|
||
self.depth_range_min = self.config.get('depth_range_min', 500)
|
||
self.depth_range_max = self.config.get('depth_range_max', 4500)
|
||
self.synchronized_images_only = self.config.get('synchronized_images_only', False)
|
||
|
||
# 更新图像处理参数
|
||
self.contrast_factor = self.config.get('contrast_factor', 1.2)
|
||
self.gamma_value = self.config.get('gamma_value', 0.8)
|
||
self.use_pseudo_color = self.config.get('use_pseudo_color', True)
|
||
|
||
# 更新缓存队列大小
|
||
cache_size = self.config.get('frame_cache_size', 10)
|
||
if cache_size != self.depth_frame_cache.maxlen:
|
||
self.depth_frame_cache = deque(maxlen=cache_size)
|
||
self.color_frame_cache = deque(maxlen=cache_size)
|
||
|
||
# 更新gamma查找表
|
||
self._update_gamma_lut()
|
||
|
||
self.logger.info(f"FemtoBolt配置重新加载成功 - 算法: {self.algorithm_type}, 分辨率: {self.color_resolution}, FPS: {self.fps}")
|
||
return True
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"重新加载FemtoBolt配置失败: {e}")
|
||
return False
|
||
|
||
def check_hardware_connection(self) -> bool:
|
||
"""
|
||
检查FemtoBolt设备硬件连接状态
|
||
|
||
Returns:
|
||
bool: 设备是否物理连接
|
||
"""
|
||
try:
|
||
if not self.sdk_initialized or not self.device_handle:
|
||
return False
|
||
|
||
# 尝试获取设备状态来验证连接
|
||
if hasattr(self.femtobolt, 'device_get_capture'):
|
||
try:
|
||
# 尝试获取一帧数据来验证设备连接
|
||
capture = self.femtobolt.device_get_capture(self.device_handle, timeout_in_ms=1000)
|
||
if capture:
|
||
self.femtobolt.capture_release(capture)
|
||
return True
|
||
except Exception:
|
||
pass
|
||
return False
|
||
except Exception as e:
|
||
self.logger.debug(f"检查FemtoBolt硬件连接时发生异常: {e}")
|
||
return False
|
||
|
||
def cleanup(self):
|
||
"""
|
||
清理资源
|
||
"""
|
||
try:
|
||
# 清理监控线程
|
||
self._cleanup_monitoring()
|
||
|
||
self.stop_streaming()
|
||
self._cleanup_device()
|
||
|
||
# 清理matplotlib图形对象
|
||
if hasattr(self, 'fig') and self.fig is not None:
|
||
plt.close(self.fig)
|
||
self.fig = None
|
||
self.ax = None
|
||
|
||
self.depth_frame_cache.clear()
|
||
self.color_frame_cache.clear()
|
||
self.last_depth_frame = None
|
||
self.last_color_frame = None
|
||
|
||
super().cleanup()
|
||
self.logger.info("FemtoBolt资源清理完成")
|
||
|
||
except Exception as e:
|
||
self.logger.error(f"清理FemtoBolt资源失败: {e}")
|