深度相机未连接启动报错问题修复,imu和足部相机优化
This commit is contained in:
parent
84e18180a4
commit
3b5fce9be5
@ -253,12 +253,18 @@ class DeviceManager:
|
|||||||
|
|
||||||
# 启动FemtoBolt设备
|
# 启动FemtoBolt设备
|
||||||
# logger.info(f'尝试启动FemtoBolt设备...,参数详情是{self.femtobolt_config}')
|
# logger.info(f'尝试启动FemtoBolt设备...,参数详情是{self.femtobolt_config}')
|
||||||
|
|
||||||
|
|
||||||
self.femtobolt_camera = pykinect.start_device(config=self.femtobolt_config)
|
self.femtobolt_camera = pykinect.start_device(config=self.femtobolt_config)
|
||||||
if self.femtobolt_camera:
|
if self.femtobolt_camera:
|
||||||
self.device_status['femtobolt'] = True
|
self.device_status['femtobolt'] = True
|
||||||
logger.info('✓ FemtoBolt深度相机初始化成功!')
|
logger.info('✓ FemtoBolt深度相机初始化成功!')
|
||||||
else:
|
else:
|
||||||
raise Exception('设备启动返回None')
|
# 设备启动失败,但不抛出异常,允许应用继续运行
|
||||||
|
logger.warning('FemtoBolt设备启动返回None,可能设备未连接或被占用')
|
||||||
|
self.femtobolt_camera = None
|
||||||
|
self.device_status['femtobolt'] = False
|
||||||
|
return # 直接返回,不抛出异常
|
||||||
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
logger.warning(f'FemtoBolt深度相机初始化失败: {e}')
|
logger.warning(f'FemtoBolt深度相机初始化失败: {e}')
|
||||||
|
@ -119,14 +119,16 @@ class CameraManager(BaseDevice):
|
|||||||
self.logger.warning(f"后端 {backend} 打开相机失败: {e}")
|
self.logger.warning(f"后端 {backend} 打开相机失败: {e}")
|
||||||
continue
|
continue
|
||||||
else:
|
else:
|
||||||
raise Exception("所有后端都无法打开相机")
|
self.logger.warning("所有后端都无法打开相机,相机设备不可用")
|
||||||
|
return False
|
||||||
|
|
||||||
# 设置相机属性
|
# 设置相机属性
|
||||||
self._configure_camera()
|
self._configure_camera()
|
||||||
|
|
||||||
# 验证相机是否正常工作
|
# 验证相机是否正常工作
|
||||||
if not self._test_camera():
|
if not self._test_camera():
|
||||||
raise Exception("相机测试失败")
|
self.logger.warning("相机测试失败,相机设备不可用")
|
||||||
|
return False
|
||||||
|
|
||||||
self.is_connected = True
|
self.is_connected = True
|
||||||
self._device_info.update({
|
self._device_info.update({
|
||||||
@ -140,7 +142,17 @@ class CameraManager(BaseDevice):
|
|||||||
return True
|
return True
|
||||||
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.logger.error(f"相机初始化失败: {e}")
|
error_msg = f"相机初始化失败: {e}"
|
||||||
|
|
||||||
|
# 提供更详细的错误信息和解决建议
|
||||||
|
if "所有后端都无法打开相机" in str(e):
|
||||||
|
error_msg += f"\n可能原因:\n1. 相机设备索引 {self.device_index} 不存在或被其他程序占用\n2. 相机驱动程序未正确安装\n3. 相机硬件连接问题\n建议:检查相机连接状态,尝试更换设备索引或重启相机设备"
|
||||||
|
elif "Camera index out of range" in str(e):
|
||||||
|
error_msg += f"\n相机设备索引 {self.device_index} 超出范围,请检查可用的相机设备"
|
||||||
|
elif "backend" in str(e).lower():
|
||||||
|
error_msg += "\n相机后端初始化失败,可能是驱动程序问题"
|
||||||
|
|
||||||
|
self.logger.error(error_msg)
|
||||||
self.is_connected = False
|
self.is_connected = False
|
||||||
if self.cap:
|
if self.cap:
|
||||||
try:
|
try:
|
||||||
|
@ -96,7 +96,7 @@ class DeviceCoordinator:
|
|||||||
|
|
||||||
# 初始化设备
|
# 初始化设备
|
||||||
if not self._initialize_devices():
|
if not self._initialize_devices():
|
||||||
raise Exception("设备初始化失败")
|
self.logger.warning("设备初始化失败,将以降级模式继续运行")
|
||||||
|
|
||||||
# 启动监控线程
|
# 启动监控线程
|
||||||
self._start_monitor()
|
self._start_monitor()
|
||||||
@ -170,7 +170,8 @@ class DeviceCoordinator:
|
|||||||
|
|
||||||
# 至少需要一个设备初始化成功
|
# 至少需要一个设备初始化成功
|
||||||
if success_count == 0:
|
if success_count == 0:
|
||||||
raise Exception("没有设备初始化成功")
|
self.logger.warning("没有设备初始化成功,但系统将继续运行")
|
||||||
|
return False
|
||||||
|
|
||||||
self.logger.info(f"设备初始化完成,成功: {success_count}/{len(futures)}")
|
self.logger.info(f"设备初始化完成,成功: {success_count}/{len(futures)}")
|
||||||
return True
|
return True
|
||||||
|
@ -22,6 +22,10 @@ import matplotlib
|
|||||||
from scipy import ndimage
|
from scipy import ndimage
|
||||||
from scipy.interpolate import griddata
|
from scipy.interpolate import griddata
|
||||||
import io
|
import io
|
||||||
|
import importlib
|
||||||
|
from typing import Optional
|
||||||
|
import subprocess
|
||||||
|
import tempfile
|
||||||
|
|
||||||
try:
|
try:
|
||||||
from .base_device import BaseDevice
|
from .base_device import BaseDevice
|
||||||
@ -72,6 +76,8 @@ class FemtoBoltManager(BaseDevice):
|
|||||||
self.femtobolt = None
|
self.femtobolt = None
|
||||||
self.device_handle = None
|
self.device_handle = None
|
||||||
self.sdk_initialized = False
|
self.sdk_initialized = False
|
||||||
|
# 新增:记录已使用的k4a.dll路径,供子进程探测使用
|
||||||
|
self.k4a_dll_path: Optional[str] = None
|
||||||
|
|
||||||
# 设备配置
|
# 设备配置
|
||||||
self.algorithm_type = self.config.get('algorithm_type', 'opencv')
|
self.algorithm_type = self.config.get('algorithm_type', 'opencv')
|
||||||
@ -440,6 +446,7 @@ class FemtoBoltManager(BaseDevice):
|
|||||||
self.logger.error(f"FemtoBolt设备配置失败: {e}")
|
self.logger.error(f"FemtoBolt设备配置失败: {e}")
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
|
||||||
def _start_device(self) -> bool:
|
def _start_device(self) -> bool:
|
||||||
"""
|
"""
|
||||||
启动FemtoBolt设备
|
启动FemtoBolt设备
|
||||||
@ -448,26 +455,54 @@ class FemtoBoltManager(BaseDevice):
|
|||||||
bool: 启动是否成功
|
bool: 启动是否成功
|
||||||
"""
|
"""
|
||||||
try:
|
try:
|
||||||
# 启动FemtoBolt设备
|
if not self.pykinect:
|
||||||
self.logger.info(f'尝试启动FemtoBolt设备...')
|
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)
|
self.device_handle = self.pykinect.start_device(config=self.femtobolt_config)
|
||||||
if self.device_handle:
|
if self.device_handle:
|
||||||
self.logger.info('✓ FemtoBolt深度相机初始化成功!')
|
self.logger.info('✓ FemtoBolt深度相机初始化成功!')
|
||||||
else:
|
else:
|
||||||
raise Exception('设备启动返回None')
|
self.logger.warning('FemtoBolt设备启动返回None,设备可能未连接')
|
||||||
|
return False
|
||||||
|
|
||||||
# 等待设备稳定
|
# # 等待设备稳定
|
||||||
time.sleep(1.0)
|
# time.sleep(1.0)
|
||||||
|
|
||||||
# 测试捕获
|
# # 测试捕获(可选,失败不抛异常,只作为稳定性判断)
|
||||||
if not self._test_capture():
|
# try:
|
||||||
raise Exception("设备捕获测试失败")
|
# if not self._test_capture():
|
||||||
|
# self.logger.warning('FemtoBolt设备捕获测试失败')
|
||||||
|
# return False
|
||||||
|
# except Exception:
|
||||||
|
# return False
|
||||||
|
|
||||||
self.logger.info("FemtoBolt设备启动成功")
|
self.logger.info('FemtoBolt设备启动成功')
|
||||||
return True
|
return True
|
||||||
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.logger.error(f"FemtoBolt设备启动失败: {e}")
|
self.logger.error(f"FemtoBolt设备启动失败: {e}")
|
||||||
return False
|
return False
|
||||||
|
@ -77,7 +77,7 @@ class RealIMUDevice:
|
|||||||
head_pose = raw_data['head_pose'].copy()
|
head_pose = raw_data['head_pose'].copy()
|
||||||
angle=head_pose['rotation'] - self.head_pose_offset['rotation']
|
angle=head_pose['rotation'] - self.head_pose_offset['rotation']
|
||||||
# 减去基准值(零点偏移)
|
# 减去基准值(零点偏移)
|
||||||
head_pose['rotation'] = ((angle + 180) % 360) - 180
|
head_pose['rotation'] = round(((angle + 180) % 360) - 180, 1)
|
||||||
head_pose['tilt'] = head_pose['tilt'] - self.head_pose_offset['tilt']
|
head_pose['tilt'] = head_pose['tilt'] - self.head_pose_offset['tilt']
|
||||||
head_pose['pitch'] = head_pose['pitch'] - self.head_pose_offset['pitch']
|
head_pose['pitch'] = head_pose['pitch'] - self.head_pose_offset['pitch']
|
||||||
|
|
||||||
@ -103,9 +103,9 @@ class RealIMUDevice:
|
|||||||
pitchl, rxl, yawl, temp = vals # 注意这里 vals 已经是有符号整数
|
pitchl, rxl, yawl, temp = vals # 注意这里 vals 已经是有符号整数
|
||||||
# 使用第一段代码里的比例系数
|
# 使用第一段代码里的比例系数
|
||||||
k_angle = 180.0
|
k_angle = 180.0
|
||||||
roll = -round(rxl / 32768.0 * k_angle,2)
|
roll = -round(rxl / 32768.0 * k_angle,1)
|
||||||
pitch = -round(pitchl / 32768.0 * k_angle,2)
|
pitch = -round(pitchl / 32768.0 * k_angle,1)
|
||||||
yaw = -round(yawl / 32768.0 * k_angle,2)
|
yaw = -round(yawl / 32768.0 * k_angle,1)
|
||||||
temp = temp / 100.0
|
temp = temp / 100.0
|
||||||
self.last_data = {
|
self.last_data = {
|
||||||
'roll': roll,
|
'roll': roll,
|
||||||
@ -285,9 +285,9 @@ class BleIMUDevice:
|
|||||||
head_pose = raw_data['head_pose'].copy()
|
head_pose = raw_data['head_pose'].copy()
|
||||||
# 与串口IMU保持一致:对 rotation 做归一化到 [-180, 180)
|
# 与串口IMU保持一致:对 rotation 做归一化到 [-180, 180)
|
||||||
angle = head_pose['rotation'] - self.head_pose_offset['rotation']
|
angle = head_pose['rotation'] - self.head_pose_offset['rotation']
|
||||||
head_pose['rotation'] = ((angle + 180) % 360) - 180
|
head_pose['rotation'] = round(((angle + 180) % 360) - 180,1)
|
||||||
head_pose['tilt'] = head_pose['tilt'] - self.head_pose_offset['tilt']
|
head_pose['tilt'] = round(head_pose['tilt'] - self.head_pose_offset['tilt'],1)
|
||||||
head_pose['pitch'] = head_pose['pitch'] - self.head_pose_offset['pitch']
|
head_pose['pitch'] = round(head_pose['pitch'] - self.head_pose_offset['pitch'],1)
|
||||||
calibrated_data['head_pose'] = head_pose
|
calibrated_data['head_pose'] = head_pose
|
||||||
return calibrated_data
|
return calibrated_data
|
||||||
|
|
||||||
@ -483,9 +483,9 @@ class BleIMUDevice:
|
|||||||
angleZ = float(np.short((np.short(buf[L+1]) << 8) | buf[L]) * scaleAngle); L += 2
|
angleZ = float(np.short((np.short(buf[L+1]) << 8) | buf[L]) * scaleAngle); L += 2
|
||||||
with self._lock:
|
with self._lock:
|
||||||
# 映射:roll=X, pitch=Y, yaw=Z
|
# 映射:roll=X, pitch=Y, yaw=Z
|
||||||
self.last_data['roll'] = angleX*-1
|
self.last_data['roll'] = round(angleX*-1, 1)
|
||||||
self.last_data['pitch'] = angleY*-1
|
self.last_data['pitch'] = round(angleY*-1, 1)
|
||||||
self.last_data['yaw'] = angleZ*-1
|
self.last_data['yaw'] = round(angleZ*-1, 1)
|
||||||
if tmp_temperature is not None:
|
if tmp_temperature is not None:
|
||||||
self.last_data['temperature'] = tmp_temperature
|
self.last_data['temperature'] = tmp_temperature
|
||||||
except Exception:
|
except Exception:
|
||||||
|
@ -206,7 +206,7 @@ class ConfigManager:
|
|||||||
'baudrate': self.config.getint('DEVICES', 'imu_baudrate', fallback=9600),
|
'baudrate': self.config.getint('DEVICES', 'imu_baudrate', fallback=9600),
|
||||||
'timeout': self.config.getfloat('DEVICES', 'imu_timeout', fallback=1.0),
|
'timeout': self.config.getfloat('DEVICES', 'imu_timeout', fallback=1.0),
|
||||||
'calibration_samples': self.config.getint('DEVICES', 'imu_calibration_samples', fallback=100),
|
'calibration_samples': self.config.getint('DEVICES', 'imu_calibration_samples', fallback=100),
|
||||||
'mac_address': self.config.get('DEVICES', 'imu_mac_address', fallback=''),
|
'mac_address': self.config.get('DEVICES', 'imu_mac_address', fallback='ef:3c:1a:0a:fe:02'),
|
||||||
}
|
}
|
||||||
|
|
||||||
def _get_pressure_config(self) -> Dict[str, Any]:
|
def _get_pressure_config(self) -> Dict[str, Any]:
|
||||||
|
@ -1567,7 +1567,7 @@ class AppServer:
|
|||||||
except Exception as e:
|
except Exception as e:
|
||||||
self.logger.error(f'启动设备数据推送失败: {e}')
|
self.logger.error(f'启动设备数据推送失败: {e}')
|
||||||
self.is_pushing_data = False
|
self.is_pushing_data = False
|
||||||
raise
|
# 不抛出异常,允许应用继续运行
|
||||||
|
|
||||||
def stop_device_push_data(self):
|
def stop_device_push_data(self):
|
||||||
"""停止设备数据推送"""
|
"""停止设备数据推送"""
|
||||||
|
@ -30,7 +30,7 @@ class CameraViewer:
|
|||||||
|
|
||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
# 修改这里的数字可以切换不同摄像头设备
|
# 修改这里的数字可以切换不同摄像头设备
|
||||||
viewer = CameraViewer(device_index=3)
|
viewer = CameraViewer(device_index=1)
|
||||||
viewer.start_stream()
|
viewer.start_stream()
|
||||||
|
|
||||||
# import ctypes
|
# import ctypes
|
||||||
|
@ -2,6 +2,7 @@ import os
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
import cv2
|
import cv2
|
||||||
from matplotlib.colors import LinearSegmentedColormap
|
from matplotlib.colors import LinearSegmentedColormap
|
||||||
|
import ctypes
|
||||||
|
|
||||||
|
|
||||||
class FemtoBoltViewer:
|
class FemtoBoltViewer:
|
||||||
@ -42,12 +43,42 @@ class FemtoBoltViewer:
|
|||||||
print(f"加载 SDK 失败: {e}")
|
print(f"加载 SDK 失败: {e}")
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
# def _configure_device(self):
|
||||||
|
# self.config = self.pykinect.default_configuration
|
||||||
|
# self.config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED
|
||||||
|
# self.config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15
|
||||||
|
# self.config.synchronized_images_only = False
|
||||||
|
# self.device_handle = self.pykinect.start_device(config=self.config)
|
||||||
|
def get_device_count():
|
||||||
|
base_dir = os.path.dirname(os.path.abspath(__file__))
|
||||||
|
dll_path = os.path.join(base_dir, "..", "dll", "femtobolt", "k4a.dll")
|
||||||
|
k4a = ctypes.CDLL(dll_path)
|
||||||
|
count = k4a.k4a_device_get_installed_count()
|
||||||
|
return count
|
||||||
def _configure_device(self):
|
def _configure_device(self):
|
||||||
|
try:
|
||||||
|
base_dir = os.path.dirname(os.path.abspath(__file__))
|
||||||
|
dll_path = os.path.join(base_dir, "..", "dll", "femtobolt", "k4a.dll")
|
||||||
|
k4a = ctypes.CDLL(dll_path)
|
||||||
|
device_count = k4a.k4a_device_get_installed_count()
|
||||||
|
# device_count = get_device_count()
|
||||||
|
if device_count == 0:
|
||||||
|
print("未检测到任何深度相机,请检查连接。",device_count)
|
||||||
|
return False
|
||||||
|
|
||||||
self.config = self.pykinect.default_configuration
|
self.config = self.pykinect.default_configuration
|
||||||
self.config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED
|
self.config.depth_mode = self.pykinect.K4A_DEPTH_MODE_NFOV_UNBINNED
|
||||||
self.config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15
|
self.config.camera_fps = self.pykinect.K4A_FRAMES_PER_SECOND_15
|
||||||
self.config.synchronized_images_only = False
|
self.config.synchronized_images_only = False
|
||||||
|
|
||||||
self.device_handle = self.pykinect.start_device(config=self.config)
|
self.device_handle = self.pykinect.start_device(config=self.config)
|
||||||
|
print("设备启动成功")
|
||||||
|
return True
|
||||||
|
except Exception as e:
|
||||||
|
print(f"设备初始化失败: {e}")
|
||||||
|
return False
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
def _generate_contour_image(self, depth):
|
def _generate_contour_image(self, depth):
|
||||||
"""改进版 OpenCV 等高线渲染,梯度平滑、局部对比增强"""
|
"""改进版 OpenCV 等高线渲染,梯度平滑、局部对比增强"""
|
||||||
@ -114,8 +145,12 @@ class FemtoBoltViewer:
|
|||||||
if not self._load_sdk():
|
if not self._load_sdk():
|
||||||
print("SDK 加载失败,程序退出")
|
print("SDK 加载失败,程序退出")
|
||||||
return
|
return
|
||||||
|
# import pykinect_azure as pykinect
|
||||||
self._configure_device()
|
# print(dir(pykinect.k4a.device))
|
||||||
|
# self._configure_device()
|
||||||
|
if not self._configure_device():
|
||||||
|
print("设备配置失败,程序退出")
|
||||||
|
return
|
||||||
print("FemtoBolt 深度相机启动成功,按 Ctrl+C 或 ESC 退出", self.config)
|
print("FemtoBolt 深度相机启动成功,按 Ctrl+C 或 ESC 退出", self.config)
|
||||||
|
|
||||||
try:
|
try:
|
||||||
|
Loading…
Reference in New Issue
Block a user