BodyBalanceEvaluation/backend/devices/femtobolt_manager.py

#!/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

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
        
        # 设备配置
        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("设备启动失败")
                
            self.is_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}")
            self.is_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:
            # 启动FemtoBolt设备
            self.logger.info(f'尝试启动FemtoBolt设备...')
            
            # 启动真实设备
            self.device_handle = self.pykinect.start_device(config=self.femtobolt_config)
            if self.device_handle:
                self.logger.info('✓ FemtoBolt深度相机初始化成功!')
            else:
                raise Exception('设备启动返回None')
                
            # 等待设备稳定
            time.sleep(1.0)
            
            # 测试捕获
            if not self._test_capture():
                raise Exception("设备捕获测试失败")
                
            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:
                                    
                                    # 根据配置选择不同的等高线生成方法
                                    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 cleanup(self):
        """
        清理资源
        """
        try:
            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}")