# -- coding: utf-8 -- import threading import msvcrt import ctypes import time import os import struct from thread_pool import submit_task from ctypes import * from datetime import datetime import Point as point from Mv3dRgbdImport.Mv3dRgbdDefine import * from Mv3dRgbdImport.Mv3dRgbdApi import * from Mv3dRgbdImport.Mv3dRgbdDefine import DeviceType_Ethernet, DeviceType_USB, DeviceType_Ethernet_Vir, DeviceType_USB_Vir, MV3D_RGBD_FLOAT_EXPOSURETIME, \ ParamType_Float, ParamType_Int, ParamType_Enum, CoordinateType_Depth, MV3D_RGBD_FLOAT_Z_UNIT, MV3D_RGBD_OK, \ FileType_BMP,FileType_TIFF,ImageType_Depth, ImageType_RGB8_Planar, ImageType_YUV420SP_NV12 ,ImageType_YUV420SP_NV21 , ImageType_YUV422, ImageType_Mono8 import config as configMap # 全局变量 SN_MAP = {} # {sn: camera_instance} import time import random from ctypes import * from logConfig import get_logger # 使用示例 logger = get_logger() def reconnect_device(sn, max_retries=5, base_delay=1.0, max_delay=10.0): """ 重新连接指定序列号的设备 Args: sn (str): 设备序列号 max_retries (int): 最大重试次数 base_delay (float): 初始延迟时间(秒) max_delay (float): 最大延迟时间(秒) Returns: bool: 重连是否成功 """ camera = SN_MAP.get(sn) # 如果设备已存在，先尝试关闭 try: camera.MV3D_RGBD_CloseDevice() except: pass # 忽略关闭时可能的错误 # 获取设备信息（需要重新枚举设备） nDeviceNum = c_uint(0) ret = Mv3dRgbd.MV3D_RGBD_GetDeviceNumber( DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir, byref(nDeviceNum) ) if ret != MV3D_RGBD_OK or nDeviceNum.value == 0: logger.error("Failed to get device number or no devices found.") return False stDeviceList = MV3D_RGBD_DEVICE_INFO_LIST() Mv3dRgbd.MV3D_RGBD_GetDeviceList( DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir, pointer(stDeviceList.DeviceInfo[0]), 20, byref(nDeviceNum) ) # 查找对应序列号的设备 target_device_info = None for i in range(nDeviceNum.value): device_sn = ''.join(chr(c) for c in stDeviceList.DeviceInfo[i].chSerialNumber).rstrip('\x00') if device_sn == sn: target_device_info = stDeviceList.DeviceInfo[i] break if not target_device_info: logger.error(f"Device with SN: {sn} not found during reconnection attempt") return False # 尝试重连 for attempt in range(max_retries): try: # 创建新的相机实例 new_camera = Mv3dRgbd() # 尝试打开设备 ret = new_camera.MV3D_RGBD_OpenDevice(pointer(target_device_info)) if ret == MV3D_RGBD_OK: # 更新全局映射 SN_MAP[sn] = new_camera logger.info(f"Successfully reconnected to device {sn}") return True else: logger.info(f"Failed to reconnect to device {sn}. Error code: {ret:#x}") except Exception as e: logger.info(f"Exception during reconnection attempt {attempt + 1}: {e}") # 如果不是最后一次尝试，等待后重试 if attempt < max_retries - 1: # 指数退避延迟 delay = min(base_delay * (2 ** attempt) + random.uniform(0, 1), max_delay) logger.info(f"Retrying reconnection in {delay:.2f} seconds...") time.sleep(delay) logger.info(f"Failed to reconnect to device {sn} after {max_retries} attempts") return False def retry_initialize_devices(max_retries=3, base_delay=2.0): """ 重试初始化设备 Args: max_retries (int): 最大重试次数 base_delay (float): 初始延迟时间(秒) Returns: bool: 初始化是否成功 """ for attempt in range(max_retries): try: # 清空现有设备映射 SN_MAP.clear() nDeviceNum = c_uint(0) ret = Mv3dRgbd.MV3D_RGBD_GetDeviceNumber( DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir, byref(nDeviceNum) ) if ret != MV3D_RGBD_OK or nDeviceNum.value == 0: logger.info("Failed to get device number or no devices found.") if attempt < max_retries - 1: delay = base_delay * (2 ** attempt) logger.info(f"Retrying device initialization in {delay} seconds...") time.sleep(delay) continue stDeviceList = MV3D_RGBD_DEVICE_INFO_LIST() Mv3dRgbd.MV3D_RGBD_GetDeviceList( DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir, pointer(stDeviceList.DeviceInfo[0]), 20, byref(nDeviceNum) ) success_count = 0 for i in range(nDeviceNum.value): serial_number = ''.join(chr(c) for c in stDeviceList.DeviceInfo[i].chSerialNumber).rstrip('\x00') logger.info(f"Found device [{i}]: Serial Number: {serial_number}") camera = Mv3dRgbd() # 打开设备 ret = camera.MV3D_RGBD_OpenDevice(pointer(stDeviceList.DeviceInfo[i])) if ret != MV3D_RGBD_OK: logger.info(f"Failed to open device with SN: {serial_number}. Error code: {ret:#x}") continue # 存入全局 map SN_MAP[serial_number] = camera logger.info(f"Successfully added device {serial_number} to SN_MAP") success_count += 1 if success_count > 0: logger.info(f"Successfully initialized {success_count} devices") return True else: logger.info("No devices were successfully initialized") except Exception as e: logger.info(f"Exception during device initialization attempt {attempt + 1}: {e}") if attempt < max_retries - 1: delay = base_delay * (2 ** attempt) logger.info(f"Retrying device initialization in {delay} seconds...") time.sleep(delay) logger.info(f"Failed to initialize devices after {max_retries} attempts") return False def enhanced_pic(sn, type): """ 增强版的图片采集函数，包含重连机制和点云数据重试机制 """ camera = SN_MAP.get(sn) if not camera: logger.info(f"No camera found for SN: {sn}, attempting reconnection...") if not reconnect_device(sn): logger.info(f"Failed to reconnect to device {sn}") return None camera = SN_MAP.get(sn) config = configMap.CAMERA_CONFIG_MAP.get(sn) if not config: logger.info(f"No config found for SN: {sn}") return None time_on = config.get("time_on", 0) logger.info(f"Delaying start by {time_on}ms...") time.sleep(time_on / 1000.0) saved_files = { "sn": sn, "depth": "", "color": "", "pcd": "", "point": [] } # 尝试开始取流，如果失败则尝试重连 ret = camera.MV3D_RGBD_Start() if ret != MV3D_RGBD_OK: logger.info(f"Failed to start grabbing. Error code: {ret:#x}") logger.info("Attempting reconnection...") if reconnect_device(sn): camera = SN_MAP.get(sn) ret = camera.MV3D_RGBD_Start() if ret != MV3D_RGBD_OK: logger.info(f"Failed to start grabbing after reconnection. Error code: {ret:#x}") return saved_files else: logger.info("Reconnection failed") return saved_files try: # 最多重试10次获取有效的点云数据 max_retries = 10 retry_count = 0 point_cloud_success = False # 用于存储最后一次成功的彩色图数据 last_color_image_info = None last_color_file_name = None while retry_count < max_retries and not point_cloud_success: stFrameData = MV3D_RGBD_FRAME_DATA() # 获取单帧数据 ret = camera.MV3D_RGBD_FetchFrame(pointer(stFrameData), 5000) if ret == MV3D_RGBD_OK: has_depth_data = False current_color_image_info = None current_color_file_name = None flag = True for i in range(stFrameData.nImageCount): image_info = stFrameData.stImageData[i] # 保存深度图 if image_info.enImageType == ImageType_Depth: has_depth_data = True stPointCloudImage = MV3D_RGBD_IMAGE_DATA() ret = camera.MV3D_RGBD_MapDepthToPointCloud(pointer(stFrameData.stImageData[i]), pointer(stPointCloudImage)) if MV3D_RGBD_OK != ret: logger.info(f"_MapDepthToPointCloud() Run failed... Retry: {retry_count + 1}/{max_retries}") else: logger.info( "_MapDepthToPointCloud() Run Succeed: framenum (%d) height(%d) width(%d) len (%d)!" % ( stPointCloudImage.nFrameNum, stPointCloudImage.nHeight, stPointCloudImage.nWidth, stPointCloudImage.nDataLen)) strMode = string_at(stPointCloudImage.pData, stPointCloudImage.nDataLen) sValue = struct.unpack('f' * int(stPointCloudImage.nHeight * stPointCloudImage.nWidth * 3), strMode) pcd_file, processed_points = point.sValue_to_pcd(sValue, stPointCloudImage, sn, type) if pcd_file and len(processed_points) > 0: saved_files["pcd"] = pcd_file saved_files["point"] = processed_points point_cloud_success = True # 成功获取点云数据 flag = False # 记录彩色图信息，但不立即保存 elif image_info.enImageType in ( ImageType_RGB8_Planar, ImageType_YUV420SP_NV12, ImageType_YUV420SP_NV21, ImageType_YUV422 ): file_name = configMap.save_path("color", sn+"_Color") if config.get("save_image"): submit_task(camera.MV3D_RGBD_SaveImage,pointer(image_info), FileType_BMP, file_name) saved_files["color"] = file_name + ".bmp" logger.info(f"Color image saved successfully: {saved_files['color']}") if flag:break # 如果没有深度数据或点云处理失败，增加重试次数 if not has_depth_data: logger.info(f"No depth data found in frame. Retry: {retry_count + 1}/{max_retries}") retry_count += 1 time.sleep(0.1) # 短暂等待后重试 elif not point_cloud_success: retry_count += 1 time.sleep(0.1) # 短暂等待后重试 else: logger.info(f"Failed to fetch frame. Error code: {ret:#x}. Retry: {retry_count + 1}/{max_retries}") retry_count += 1 time.sleep(0.1) # 短暂等待后重试 except Exception as e: logger.error(f"Error during frame capture for device {sn}: {e}") finally: camera.MV3D_RGBD_Stop() # logger.info(f"Single capture completed. Results: {saved_files}") return saved_files def initialize_devices(): nDeviceNum = ctypes.c_uint(0) ret = Mv3dRgbd.MV3D_RGBD_GetDeviceNumber( DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir, byref(nDeviceNum) ) if ret != MV3D_RGBD_OK or nDeviceNum.value == 0: logger.info("Failed to get device number or no devices found.") return stDeviceList = MV3D_RGBD_DEVICE_INFO_LIST() Mv3dRgbd.MV3D_RGBD_GetDeviceList( DeviceType_Ethernet | DeviceType_USB | DeviceType_Ethernet_Vir | DeviceType_USB_Vir, pointer(stDeviceList.DeviceInfo[0]), 20, byref(nDeviceNum) ) for i in range(nDeviceNum.value): serial_number = ''.join(chr(c) for c in stDeviceList.DeviceInfo[i].chSerialNumber).rstrip('\x00') logger.info(f"Found device [{i}]: Serial Number: {serial_number}") camera = Mv3dRgbd() # 打开设备 ret = camera.MV3D_RGBD_OpenDevice(pointer(stDeviceList.DeviceInfo[i])) if ret != MV3D_RGBD_OK: logger.info(f"Failed to open device with SN: {serial_number}. Error code: {ret:#x}") continue # 存入全局 map SN_MAP[serial_number] = camera logger.info(f"Successfully added device {serial_number} to SN_MAP") initialize_devices()