修改了imu连接、重启相关功能
This commit is contained in:
parent
0f0ca132d3
commit
6dbde36852
@ -52,6 +52,7 @@ synchronized_images_only = False
|
||||
[DEVICES]
|
||||
imu_enable = True
|
||||
imu_use_mock = False
|
||||
imu_ble_name = WT901BLE67
|
||||
imu_mac_address = FA:E8:88:06:FE:F3
|
||||
pressure_enable = False
|
||||
pressure_use_mock = False
|
||||
|
||||
@ -33,6 +33,7 @@ class BaseDevice(ABC):
|
||||
self.config = config
|
||||
self.is_connected = False
|
||||
self.is_streaming = False
|
||||
self._initializing = False
|
||||
self.socket_namespace = f"/{device_name}"
|
||||
self.logger = logging.getLogger(f"device.{device_name}")
|
||||
self._lock = threading.RLock() # 可重入锁
|
||||
@ -361,4 +362,4 @@ class BaseDevice(ABC):
|
||||
self._stop_connection_monitor()
|
||||
|
||||
def __repr__(self):
|
||||
return f"<{self.__class__.__name__}(name='{self.device_name}', connected={self.is_connected}, streaming={self.is_streaming})>"
|
||||
return f"<{self.__class__.__name__}(name='{self.device_name}', connected={self.is_connected}, streaming={self.is_streaming})>"
|
||||
|
||||
@ -812,17 +812,19 @@ class CameraManager(BaseDevice):
|
||||
Returns:
|
||||
Dict[str, Any]: 设备状态信息
|
||||
"""
|
||||
status = super().get_status()
|
||||
status.update({
|
||||
return {
|
||||
'device_type': 'camera',
|
||||
'is_connected': self.is_connected,
|
||||
'is_streaming': self.is_streaming,
|
||||
'device_index': self.device_index,
|
||||
'resolution': f"{self.width}x{self.height}",
|
||||
'target_fps': self.fps,
|
||||
'actual_fps': self.actual_fps,
|
||||
'frame_count': self.frame_count,
|
||||
'dropped_frames': self.dropped_frames,
|
||||
'has_frame': self.last_frame is not None
|
||||
})
|
||||
return status
|
||||
'has_frame': self.last_frame is not None,
|
||||
'device_info': self.get_device_info()
|
||||
}
|
||||
|
||||
def capture_image(self, save_path: Optional[str] = None) -> Optional[np.ndarray]:
|
||||
"""
|
||||
@ -994,4 +996,4 @@ class CameraManager(BaseDevice):
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -75,6 +75,8 @@ class DeviceCoordinator:
|
||||
'device_errors': defaultdict(int),
|
||||
'reconnect_attempts': defaultdict(int)
|
||||
}
|
||||
self._last_restart_ts = defaultdict(float)
|
||||
self._restart_in_progress = defaultdict(bool)
|
||||
|
||||
# 线程池
|
||||
self.executor = ThreadPoolExecutor(max_workers=8, thread_name_prefix="DeviceCoord")
|
||||
@ -117,12 +119,12 @@ class DeviceCoordinator:
|
||||
if not self._initialize_devices():
|
||||
self.logger.warning("设备初始化失败，将以降级模式继续运行")
|
||||
|
||||
# 启动监控线程
|
||||
self._start_monitor()
|
||||
|
||||
self.is_initialized = True
|
||||
self.stats['start_time'] = time.time()
|
||||
|
||||
# 启动监控线程
|
||||
self._start_monitor()
|
||||
|
||||
self.logger.info("设备协调器初始化成功")
|
||||
self._emit_event('coordinator_initialized', {'devices': list(self.devices.keys())})
|
||||
|
||||
@ -191,7 +193,8 @@ class DeviceCoordinator:
|
||||
success_count = 0
|
||||
for device_name, future in futures:
|
||||
try:
|
||||
result = future.result(timeout=30) # 30秒超时
|
||||
timeout_s = 45 if device_name == 'imu' else 30
|
||||
result = future.result(timeout=timeout_s)
|
||||
if result:
|
||||
success_count += 1
|
||||
self.logger.info(f"{device_name}设备初始化成功")
|
||||
@ -587,12 +590,22 @@ class DeviceCoordinator:
|
||||
was_streaming = False
|
||||
|
||||
try:
|
||||
if self._restart_in_progress[device_name]:
|
||||
self.logger.warning(f"{device_name} 设备正在重启中，跳过重复重启请求")
|
||||
return False
|
||||
self._restart_in_progress[device_name] = True
|
||||
self._last_restart_ts[device_name] = time.time()
|
||||
self.logger.info(f"开始彻底重启设备: {device_name}")
|
||||
|
||||
# 第一步：检查并停止数据流
|
||||
stop_start = time.time()
|
||||
if hasattr(device, 'is_streaming'):
|
||||
was_streaming = device.is_streaming
|
||||
try:
|
||||
if hasattr(device, 'get_status'):
|
||||
was_streaming = bool((device.get_status() or {}).get('is_streaming', False))
|
||||
elif hasattr(device, 'is_streaming'):
|
||||
was_streaming = bool(device.is_streaming)
|
||||
except Exception:
|
||||
was_streaming = False
|
||||
|
||||
if hasattr(device, 'stop_streaming') and was_streaming:
|
||||
self.logger.info(f"正在停止 {device_name} 设备推流...")
|
||||
@ -609,6 +622,11 @@ class DeviceCoordinator:
|
||||
# 第二步：断开连接并彻底清理资源
|
||||
cleanup_start = time.time()
|
||||
self.logger.info(f"正在彻底清理 {device_name} 设备...")
|
||||
try:
|
||||
if hasattr(device, '_init_abort'):
|
||||
device._init_abort.set()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# 断开连接但暂时不广播状态变化，避免重启过程中的状态冲突
|
||||
if hasattr(device, 'disconnect'):
|
||||
@ -646,7 +664,7 @@ class DeviceCoordinator:
|
||||
self.logger.info(f"{device_name} 设备实例已销毁")
|
||||
|
||||
# 短暂等待，确保资源完全释放
|
||||
time.sleep(0.2)
|
||||
time.sleep(1.5 if device_name == 'imu' else 0.2)
|
||||
destroy_time = (time.time() - destroy_start) * 1000
|
||||
|
||||
# 第四步：重新创建设备实例
|
||||
@ -754,8 +772,6 @@ class DeviceCoordinator:
|
||||
if not new_device.initialize():
|
||||
init_time = (time.time() - init_start) * 1000
|
||||
self.logger.error(f"{device_name} 设备初始化失败 (耗时: {init_time:.1f}ms)")
|
||||
# 初始化失败，从设备字典中移除
|
||||
self.devices.pop(device_name, None)
|
||||
return False
|
||||
|
||||
init_time = (time.time() - init_start) * 1000
|
||||
@ -797,6 +813,8 @@ class DeviceCoordinator:
|
||||
error_msg = f"彻底重启设备 {device_name} 异常: {e} (耗时: {total_time:.1f}ms)"
|
||||
self.logger.error(error_msg)
|
||||
return False
|
||||
finally:
|
||||
self._restart_in_progress[device_name] = False
|
||||
|
||||
def _start_monitor(self):
|
||||
"""
|
||||
@ -822,15 +840,18 @@ class DeviceCoordinator:
|
||||
while self.is_initialized:
|
||||
try:
|
||||
# 检查设备健康状态
|
||||
for device_name, device in self.devices.items():
|
||||
for device_name, device in list(self.devices.items()):
|
||||
try:
|
||||
if self._restart_in_progress.get(device_name, False) or getattr(device, '_initializing', False):
|
||||
continue
|
||||
status = device.get_status()
|
||||
if not status.get('is_connected', False):
|
||||
self.logger.warning(f"设备 {device_name} 连接丢失")
|
||||
self.stats['device_errors'][device_name] += 1
|
||||
|
||||
# 尝试重连
|
||||
if self.stats['device_errors'][device_name] <= 3:
|
||||
now = time.time()
|
||||
if now - self._last_restart_ts[device_name] >= 15.0:
|
||||
self._last_restart_ts[device_name] = now
|
||||
self.logger.info(f"尝试重连设备: {device_name}")
|
||||
if self.restart_device(device_name):
|
||||
self.stats['device_errors'][device_name] = 0
|
||||
@ -1098,7 +1119,7 @@ if __name__ == "__main__":
|
||||
|
||||
# 执行测试
|
||||
# 可选值: 'camera1', 'camera2', 'imu', 'pressure', 'femtobolt'
|
||||
success = test_restart_device('pressure')
|
||||
success = test_restart_device('imu')
|
||||
|
||||
if success:
|
||||
print("\n🎉 所有测试通过！")
|
||||
|
||||
@ -1,9 +1,8 @@
|
||||
# coding:UTF-8
|
||||
import threading
|
||||
import time
|
||||
import struct
|
||||
import bleak
|
||||
import asyncio
|
||||
import logging
|
||||
|
||||
|
||||
# 设备实例 Device instance
|
||||
@ -24,7 +23,8 @@ class DeviceModel:
|
||||
# endregion
|
||||
|
||||
def __init__(self, deviceName, BLEDevice, callback_method):
|
||||
print("Initialize device model")
|
||||
self.logger = logging.getLogger("device.imu.witmotion")
|
||||
self.logger.info("初始化IMU设备模型")
|
||||
# 设备名称（自定义） Device Name
|
||||
self.deviceName = deviceName
|
||||
self.BLEDevice = BLEDevice
|
||||
@ -57,10 +57,12 @@ class DeviceModel:
|
||||
|
||||
# 打开设备 open Device
|
||||
async def openDevice(self):
|
||||
print("Opening device......")
|
||||
# 获取设备的服务和特征 Obtain the services and characteristic of the device
|
||||
start_ts = time.perf_counter()
|
||||
self.logger.info("正在打开蓝牙IMU设备...")
|
||||
connect_start = time.perf_counter()
|
||||
async with bleak.BleakClient(self.BLEDevice, timeout=15) as client:
|
||||
self.client = client
|
||||
self.logger.info(f"蓝牙连接建立完成（耗时: {(time.perf_counter() - connect_start)*1000:.1f}ms）")
|
||||
self.isOpen = True
|
||||
# 设备UUID常量 Device UUID constant
|
||||
target_service_uuid = "0000ffe5-0000-1000-8000-00805f9a34fb"
|
||||
@ -68,11 +70,28 @@ class DeviceModel:
|
||||
target_characteristic_uuid_write = "0000ffe9-0000-1000-8000-00805f9a34fb"
|
||||
notify_characteristic = None
|
||||
|
||||
print("Matching services......")
|
||||
for service in client.services:
|
||||
self.logger.info("正在匹配服务...")
|
||||
services = None
|
||||
for _ in range(3):
|
||||
get_services = getattr(client, 'get_services', None)
|
||||
if callable(get_services):
|
||||
services = await get_services()
|
||||
else:
|
||||
backend = getattr(client, "_backend", None)
|
||||
backend_get_services = getattr(backend, "get_services", None)
|
||||
if callable(backend_get_services):
|
||||
services = await backend_get_services()
|
||||
else:
|
||||
services = getattr(client, 'services', None)
|
||||
if services:
|
||||
break
|
||||
await asyncio.sleep(0.2)
|
||||
if not services:
|
||||
services = []
|
||||
for service in services:
|
||||
if service.uuid == target_service_uuid:
|
||||
print(f"Service: {service}")
|
||||
print("Matching characteristic......")
|
||||
self.logger.info(f"匹配到服务: {service}")
|
||||
self.logger.info("正在匹配特征...")
|
||||
for characteristic in service.characteristics:
|
||||
if characteristic.uuid == target_characteristic_uuid_read:
|
||||
notify_characteristic = characteristic
|
||||
@ -81,16 +100,12 @@ class DeviceModel:
|
||||
if notify_characteristic:
|
||||
break
|
||||
|
||||
if self.writer_characteristic:
|
||||
# 读取磁场四元数 Reading magnetic field quaternions
|
||||
print("Reading magnetic field quaternions")
|
||||
time.sleep(3)
|
||||
asyncio.create_task(self.sendDataTh())
|
||||
|
||||
if notify_characteristic:
|
||||
print(f"Characteristic: {notify_characteristic}")
|
||||
self.logger.info(f"匹配到特征: {notify_characteristic}")
|
||||
# 设置通知以接收数据 Set up notifications to receive data
|
||||
await client.start_notify(notify_characteristic.uuid, self.onDataReceived)
|
||||
self.logger.info("开始接收姿态数据（XYZ欧拉角）")
|
||||
self.logger.info(f"设备打开完成（耗时: {(time.perf_counter() - start_ts)*1000:.1f}ms）")
|
||||
|
||||
# 保持连接打开 Keep connected and open
|
||||
try:
|
||||
@ -102,19 +117,13 @@ class DeviceModel:
|
||||
# 在退出时停止通知 Stop notification on exit
|
||||
await client.stop_notify(notify_characteristic.uuid)
|
||||
else:
|
||||
print("No matching services or characteristic found")
|
||||
self.logger.warning("未找到匹配的服务或特征")
|
||||
raise RuntimeError("未找到匹配的服务或特征")
|
||||
|
||||
# 关闭设备 close Device
|
||||
def closeDevice(self):
|
||||
self.isOpen = False
|
||||
print("The device is turned off")
|
||||
|
||||
async def sendDataTh(self):
|
||||
while self.isOpen:
|
||||
await self.readReg(0x3A)
|
||||
time.sleep(0.1)
|
||||
await self.readReg(0x51)
|
||||
time.sleep(0.1)
|
||||
self.logger.info("设备已关闭")
|
||||
|
||||
# region 数据解析 data analysis
|
||||
# 串口数据处理 Serial port data processing
|
||||
@ -125,7 +134,7 @@ class DeviceModel:
|
||||
if len(self.TempBytes) == 1 and self.TempBytes[0] != 0x55:
|
||||
del self.TempBytes[0]
|
||||
continue
|
||||
if len(self.TempBytes) == 2 and (self.TempBytes[1] != 0x61 and self.TempBytes[1] != 0x71):
|
||||
if len(self.TempBytes) == 2 and self.TempBytes[1] != 0x61:
|
||||
del self.TempBytes[0]
|
||||
continue
|
||||
if len(self.TempBytes) == 20:
|
||||
@ -134,47 +143,15 @@ class DeviceModel:
|
||||
|
||||
# 数据解析 data analysis
|
||||
def processData(self, Bytes):
|
||||
if Bytes[1] == 0x61:
|
||||
Ax = self.getSignInt16(Bytes[3] << 8 | Bytes[2]) / 32768 * 16
|
||||
Ay = self.getSignInt16(Bytes[5] << 8 | Bytes[4]) / 32768 * 16
|
||||
Az = self.getSignInt16(Bytes[7] << 8 | Bytes[6]) / 32768 * 16
|
||||
Gx = self.getSignInt16(Bytes[9] << 8 | Bytes[8]) / 32768 * 2000
|
||||
Gy = self.getSignInt16(Bytes[11] << 8 | Bytes[10]) / 32768 * 2000
|
||||
Gz = self.getSignInt16(Bytes[13] << 8 | Bytes[12]) / 32768 * 2000
|
||||
AngX = self.getSignInt16(Bytes[15] << 8 | Bytes[14]) / 32768 * 180
|
||||
AngY = self.getSignInt16(Bytes[17] << 8 | Bytes[16]) / 32768 * 180
|
||||
AngZ = self.getSignInt16(Bytes[19] << 8 | Bytes[18]) / 32768 * 180
|
||||
self.set("AccX", round(Ax, 3))
|
||||
self.set("AccY", round(Ay, 3))
|
||||
self.set("AccZ", round(Az, 3))
|
||||
self.set("AsX", round(Gx, 3))
|
||||
self.set("AsY", round(Gy, 3))
|
||||
self.set("AsZ", round(Gz, 3))
|
||||
self.set("AngX", round(AngX, 3))
|
||||
self.set("AngY", round(AngY, 3))
|
||||
self.set("AngZ", round(AngZ, 3))
|
||||
self.callback_method(self)
|
||||
else:
|
||||
# 磁场 magnetic field
|
||||
if Bytes[2] == 0x3A:
|
||||
Hx = self.getSignInt16(Bytes[5] << 8 | Bytes[4]) / 120
|
||||
Hy = self.getSignInt16(Bytes[7] << 8 | Bytes[6]) / 120
|
||||
Hz = self.getSignInt16(Bytes[9] << 8 | Bytes[8]) / 120
|
||||
self.set("HX", round(Hx, 3))
|
||||
self.set("HY", round(Hy, 3))
|
||||
self.set("HZ", round(Hz, 3))
|
||||
# 四元数 Quaternion
|
||||
elif Bytes[2] == 0x51:
|
||||
Q0 = self.getSignInt16(Bytes[5] << 8 | Bytes[4]) / 32768
|
||||
Q1 = self.getSignInt16(Bytes[7] << 8 | Bytes[6]) / 32768
|
||||
Q2 = self.getSignInt16(Bytes[9] << 8 | Bytes[8]) / 32768
|
||||
Q3 = self.getSignInt16(Bytes[11] << 8 | Bytes[10]) / 32768
|
||||
self.set("Q0", round(Q0, 5))
|
||||
self.set("Q1", round(Q1, 5))
|
||||
self.set("Q2", round(Q2, 5))
|
||||
self.set("Q3", round(Q3, 5))
|
||||
else:
|
||||
pass
|
||||
if Bytes[1] != 0x61:
|
||||
return
|
||||
AngX = self.getSignInt16(Bytes[15] << 8 | Bytes[14]) / 32768 * 180
|
||||
AngY = self.getSignInt16(Bytes[17] << 8 | Bytes[16]) / 32768 * 180
|
||||
AngZ = self.getSignInt16(Bytes[19] << 8 | Bytes[18]) / 32768 * 180
|
||||
self.set("AngX", round(AngX, 3))
|
||||
self.set("AngY", round(AngY, 3))
|
||||
self.set("AngZ", round(AngZ, 3))
|
||||
self.callback_method(self)
|
||||
|
||||
# 获得int16有符号数 Obtain int16 signed number
|
||||
@staticmethod
|
||||
@ -191,7 +168,7 @@ class DeviceModel:
|
||||
if self.client.is_connected and self.writer_characteristic is not None:
|
||||
await self.client.write_gatt_char(self.writer_characteristic.uuid, bytes(data))
|
||||
except Exception as ex:
|
||||
print(ex)
|
||||
self.logger.warning(f"发送数据失败: {ex}")
|
||||
|
||||
# 读取寄存器 read register
|
||||
async def readReg(self, regAddr):
|
||||
|
||||
@ -939,8 +939,10 @@ class FemtoBoltManager(BaseDevice):
|
||||
Returns:
|
||||
Dict[str, Any]: 设备状态信息
|
||||
"""
|
||||
status = super().get_status()
|
||||
status.update({
|
||||
return {
|
||||
'device_type': 'femtobolt',
|
||||
'is_connected': self.is_connected,
|
||||
'is_streaming': self.is_streaming,
|
||||
'color_resolution': self.color_resolution,
|
||||
'depth_mode': self.depth_mode,
|
||||
'target_fps': self.fps,
|
||||
@ -949,9 +951,9 @@ class FemtoBoltManager(BaseDevice):
|
||||
'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
|
||||
'has_color_frame': self.last_color_frame is not None,
|
||||
'device_info': self.get_device_info()
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -23,8 +23,9 @@ logger = logging.getLogger(__name__)
|
||||
|
||||
class BleIMUDevice:
|
||||
"""蓝牙IMU设备（WitMotion WT9011DCL-BT50），基于 device_model.py 官方接口"""
|
||||
def __init__(self, mac_address: str):
|
||||
def __init__(self, mac_address: str, ble_name: str = ""):
|
||||
self.mac_address = mac_address
|
||||
self.ble_name = ble_name
|
||||
self.loop = None
|
||||
self.loop_thread = None
|
||||
self.running = False
|
||||
@ -40,11 +41,14 @@ class BleIMUDevice:
|
||||
self._connected = False
|
||||
self._device_model = None
|
||||
self._open_task = None
|
||||
self._main_task = None
|
||||
self._last_update_ts = None
|
||||
try:
|
||||
from . import device_model as wit_device_model
|
||||
except Exception:
|
||||
import device_model as wit_device_model
|
||||
self._wit_device_model = wit_device_model
|
||||
logger.info(f"BLE IMU实例创建: mac={self.mac_address}")
|
||||
|
||||
def set_calibration(self, calibration: Dict[str, Any]):
|
||||
self.calibration_data = calibration
|
||||
@ -75,7 +79,21 @@ class BleIMUDevice:
|
||||
self.running = False
|
||||
try:
|
||||
if self.loop:
|
||||
asyncio.run_coroutine_threadsafe(self._disconnect(), self.loop)
|
||||
try:
|
||||
if self._main_task is not None and not self._main_task.done():
|
||||
self.loop.call_soon_threadsafe(self._main_task.cancel)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
fut = asyncio.run_coroutine_threadsafe(self._disconnect(), self.loop)
|
||||
fut.result(timeout=5.0)
|
||||
except Exception:
|
||||
pass
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if self.loop_thread and self.loop_thread.is_alive():
|
||||
self.loop_thread.join(timeout=6.0)
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
@ -96,18 +114,28 @@ class BleIMUDevice:
|
||||
self.loop = asyncio.new_event_loop()
|
||||
asyncio.set_event_loop(self.loop)
|
||||
try:
|
||||
self.loop.run_until_complete(self._connect_and_listen())
|
||||
self._main_task = self.loop.create_task(self._connect_and_listen())
|
||||
self.loop.run_until_complete(self._main_task)
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
except Exception as e:
|
||||
logger.error(f'BLE IMU事件循环异常: {e}', exc_info=True)
|
||||
finally:
|
||||
try:
|
||||
try:
|
||||
pending = asyncio.all_tasks(self.loop)
|
||||
for t in pending:
|
||||
t.cancel()
|
||||
if pending:
|
||||
self.loop.run_until_complete(asyncio.gather(*pending, return_exceptions=True))
|
||||
except Exception:
|
||||
pass
|
||||
if not self.loop.is_closed():
|
||||
self.loop.stop()
|
||||
self.loop.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._main_task = None
|
||||
self.loop = None
|
||||
|
||||
def _on_device_update(self, dm):
|
||||
try:
|
||||
@ -120,6 +148,8 @@ class BleIMUDevice:
|
||||
self.last_data['roll'] = float(roll)
|
||||
self.last_data['pitch'] = float(pitch)
|
||||
self.last_data['yaw'] = float(yaw)
|
||||
self._last_update_ts = time.time()
|
||||
self._connected = True
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
@ -130,6 +160,12 @@ class BleIMUDevice:
|
||||
self._device_model.closeDevice()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
client = getattr(self._device_model, "client", None)
|
||||
if client is not None and getattr(client, "is_connected", False):
|
||||
await client.disconnect()
|
||||
except Exception:
|
||||
pass
|
||||
if self._open_task is not None and not self._open_task.done():
|
||||
self._open_task.cancel()
|
||||
try:
|
||||
@ -141,6 +177,7 @@ class BleIMUDevice:
|
||||
finally:
|
||||
self._open_task = None
|
||||
self._device_model = None
|
||||
self._last_update_ts = None
|
||||
|
||||
async def _connect_and_listen(self):
|
||||
try:
|
||||
@ -150,39 +187,86 @@ class BleIMUDevice:
|
||||
self.running = False
|
||||
return
|
||||
|
||||
async def find_device() -> Optional[Any]:
|
||||
scan_timeout_s = 30.0
|
||||
if self.ble_name:
|
||||
find_by_name = getattr(BleakScanner, "find_device_by_name", None)
|
||||
if callable(find_by_name):
|
||||
try:
|
||||
device = await find_by_name(self.ble_name, timeout=scan_timeout_s)
|
||||
if device is not None:
|
||||
if self.mac_address and (getattr(device, "address", "") or "").lower() != self.mac_address.lower():
|
||||
return None
|
||||
return device
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
found = await BleakScanner.discover(timeout=scan_timeout_s)
|
||||
except Exception:
|
||||
found = []
|
||||
if self.ble_name:
|
||||
for d in found:
|
||||
if (getattr(d, "name", None) or "") != self.ble_name:
|
||||
continue
|
||||
if self.mac_address and (getattr(d, "address", "") or "").lower() != self.mac_address.lower():
|
||||
continue
|
||||
return d
|
||||
if self.mac_address:
|
||||
target = self.mac_address.lower()
|
||||
for d in found:
|
||||
addr = getattr(d, "address", "") or ""
|
||||
if addr.lower() == target:
|
||||
return d
|
||||
candidates = [d for d in found if (getattr(d, "name", "") or "").startswith("WT")]
|
||||
if len(candidates) == 1:
|
||||
return candidates[0]
|
||||
return None
|
||||
|
||||
while self.running:
|
||||
try:
|
||||
try:
|
||||
device = await BleakScanner.find_device_by_address(self.mac_address, timeout=20.0)
|
||||
except TypeError:
|
||||
device = await BleakScanner.find_device_by_address(self.mac_address, cb=dict(use_bdaddr=False))
|
||||
attempt_ts = time.perf_counter()
|
||||
logger.info(f"BLE IMU开始扫描并连接: name={self.ble_name}, mac={self.mac_address}")
|
||||
device = await find_device()
|
||||
|
||||
if device is None:
|
||||
logger.info(f"BLE IMU扫描未发现设备 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms)")
|
||||
await asyncio.sleep(2.0)
|
||||
continue
|
||||
|
||||
device_addr = getattr(device, "address", None)
|
||||
device_name = getattr(device, "name", None)
|
||||
logger.info(f"BLE IMU发现设备 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms, address={device_addr}, name={device_name})")
|
||||
self._connected = False
|
||||
self._last_update_ts = None
|
||||
self._device_model = self._wit_device_model.DeviceModel("WitMotionBle5.0", device, self._on_device_update)
|
||||
self._open_task = asyncio.create_task(self._device_model.openDevice())
|
||||
|
||||
connected = False
|
||||
for _ in range(50):
|
||||
if not self.running:
|
||||
ready = False
|
||||
ready_timeout_s = 20.0
|
||||
deadline = time.time() + ready_timeout_s
|
||||
while self.running and time.time() < deadline:
|
||||
if self._open_task is not None and self._open_task.done():
|
||||
try:
|
||||
exc = self._open_task.exception()
|
||||
except Exception:
|
||||
exc = None
|
||||
if exc:
|
||||
logger.warning(f"BLE IMU打开失败: {type(exc).__name__}: {repr(exc)}")
|
||||
break
|
||||
if self._open_task.done():
|
||||
if self.has_received_data:
|
||||
ready = True
|
||||
break
|
||||
client = getattr(self._device_model, "client", None)
|
||||
if client is not None and getattr(client, "is_connected", False):
|
||||
connected = True
|
||||
break
|
||||
await asyncio.sleep(0.2)
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
self._connected = connected
|
||||
if not connected:
|
||||
if not ready:
|
||||
logger.warning(f"BLE IMU未获取到姿态数据 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms)")
|
||||
await self._disconnect()
|
||||
self._connected = False
|
||||
await asyncio.sleep(2.0)
|
||||
continue
|
||||
|
||||
logger.info(f"BLE IMU连接并开始产出数据 (耗时: {(time.perf_counter()-attempt_ts)*1000:.1f}ms)")
|
||||
|
||||
while self.running and self._open_task is not None and not self._open_task.done():
|
||||
await asyncio.sleep(1.0)
|
||||
|
||||
@ -199,7 +283,15 @@ class BleIMUDevice:
|
||||
@property
|
||||
def connected(self) -> bool:
|
||||
return self._connected
|
||||
|
||||
|
||||
@property
|
||||
def has_received_data(self) -> bool:
|
||||
return self._last_update_ts is not None
|
||||
|
||||
@property
|
||||
def last_update_time(self):
|
||||
return self._last_update_ts
|
||||
|
||||
class MockIMUDevice:
|
||||
def __init__(self):
|
||||
self.running = False
|
||||
@ -215,6 +307,7 @@ class MockIMUDevice:
|
||||
'temperature': 25.0
|
||||
}
|
||||
self._phase = 0.0
|
||||
self._last_update_ts = None
|
||||
|
||||
def set_calibration(self, calibration: Dict[str, Any]):
|
||||
self.calibration_data = calibration
|
||||
@ -286,6 +379,7 @@ class MockIMUDevice:
|
||||
self.last_data['roll'] = round(roll, 1)
|
||||
# 温度模拟：以25℃为基准，叠加±0.5℃的轻微波动
|
||||
self.last_data['temperature'] = round(25.0 + math.sin(self._phase * 0.2) * 0.5, 2)
|
||||
self._last_update_ts = time.time()
|
||||
# 控制输出频率为约60Hz
|
||||
time.sleep(1.0 / 30.0)
|
||||
except Exception:
|
||||
@ -296,6 +390,14 @@ class MockIMUDevice:
|
||||
def connected(self) -> bool:
|
||||
return self._connected
|
||||
|
||||
@property
|
||||
def has_received_data(self) -> bool:
|
||||
return self._last_update_ts is not None
|
||||
|
||||
@property
|
||||
def last_update_time(self):
|
||||
return self._last_update_ts
|
||||
|
||||
class IMUManager(BaseDevice):
|
||||
"""IMU传感器管理器"""
|
||||
|
||||
@ -319,11 +421,12 @@ class IMUManager(BaseDevice):
|
||||
# 设备配置
|
||||
self.use_mock = bool(config.get('use_mock', False))
|
||||
self.mac_address = config.get('mac_address', '')
|
||||
self.ble_name = config.get('ble_name', '')
|
||||
# IMU设备实例
|
||||
self.imu_device = None
|
||||
self._init_abort = threading.Event()
|
||||
|
||||
# 推流相关
|
||||
self.imu_streaming = False
|
||||
self.imu_thread = None
|
||||
|
||||
# 统计信息
|
||||
@ -338,7 +441,7 @@ class IMUManager(BaseDevice):
|
||||
self.data_buffer = deque(maxlen=100)
|
||||
self.last_valid_data = None
|
||||
|
||||
self.logger.info(f"IMU管理器初始化完成 - use_mock: {self.use_mock}, MAC: {self.mac_address}")
|
||||
self.logger.info(f"IMU管理器初始化完成 - use_mock: {self.use_mock}, BLE_NAME: {self.ble_name}, MAC: {self.mac_address}")
|
||||
|
||||
def initialize(self) -> bool:
|
||||
"""
|
||||
@ -347,35 +450,62 @@ class IMUManager(BaseDevice):
|
||||
Returns:
|
||||
bool: 初始化是否成功
|
||||
"""
|
||||
self._initializing = True
|
||||
self._init_abort.clear()
|
||||
try:
|
||||
start_ts = time.perf_counter()
|
||||
self.logger.info(f"正在初始化IMU设备...")
|
||||
|
||||
# 使用构造函数中已加载的配置，避免并发读取配置文件
|
||||
self.logger.info(f"使用已加载配置: use_mock={self.use_mock}, mac={self.mac_address}")
|
||||
self.logger.info(f"使用已加载配置: use_mock={self.use_mock}, name={self.ble_name}, mac={self.mac_address}")
|
||||
|
||||
# 根据配置选择设备类型
|
||||
if not self.use_mock:
|
||||
if not self.mac_address:
|
||||
self.logger.error("IMU BLE设备未配置MAC地址")
|
||||
if not self.mac_address and not self.ble_name:
|
||||
self.logger.error("IMU BLE设备未配置蓝牙名称或MAC地址")
|
||||
self.is_connected = False
|
||||
return False
|
||||
self.logger.info(f"使用蓝牙IMU设备 - MAC: {self.mac_address}")
|
||||
self.imu_device = BleIMUDevice(self.mac_address)
|
||||
self.logger.info(f"使用蓝牙IMU设备 - NAME: {self.ble_name}, MAC: {self.mac_address}")
|
||||
try:
|
||||
if self.imu_device and hasattr(self.imu_device, 'stop'):
|
||||
self.imu_device.stop()
|
||||
except Exception:
|
||||
pass
|
||||
self.imu_device = BleIMUDevice(self.mac_address, self.ble_name)
|
||||
self.imu_device.start()
|
||||
# 使用set_connected方法来正确启动连接监控线程
|
||||
self.set_connected(True)
|
||||
connect_timeout_s = float(self.config.get('connect_timeout', 40.0))
|
||||
deadline = time.time() + max(0.1, connect_timeout_s)
|
||||
while time.time() < deadline:
|
||||
if self._init_abort.is_set():
|
||||
self.logger.warning("IMU初始化被中断")
|
||||
return False
|
||||
if bool(getattr(self.imu_device, 'connected', False)):
|
||||
break
|
||||
time.sleep(0.1)
|
||||
connected = bool(getattr(self.imu_device, 'connected', False))
|
||||
self._start_connection_monitor()
|
||||
self.set_connected(connected)
|
||||
if not connected:
|
||||
self.logger.error(f"IMU蓝牙连接超时（等待 {connect_timeout_s:.1f}s）")
|
||||
try:
|
||||
self.imu_device.stop()
|
||||
except Exception:
|
||||
pass
|
||||
self.imu_device = None
|
||||
self.set_connected(False)
|
||||
return False
|
||||
else:
|
||||
self.logger.info("使用模拟IMU设备")
|
||||
self.imu_device = MockIMUDevice()
|
||||
self.imu_device.start()
|
||||
# 使用set_connected方法来正确启动连接监控线程
|
||||
self._start_connection_monitor()
|
||||
self.set_connected(True)
|
||||
|
||||
self._device_info.update({
|
||||
'mac_address': self.mac_address,
|
||||
})
|
||||
|
||||
self.logger.info("IMU初始化成功")
|
||||
self.logger.info(f"IMU初始化完成（耗时: {(time.perf_counter() - start_ts)*1000:.1f}ms，当前连接状态: {self.is_connected}）")
|
||||
return True
|
||||
|
||||
except Exception as e:
|
||||
@ -383,6 +513,8 @@ class IMUManager(BaseDevice):
|
||||
self.is_connected = False
|
||||
self.imu_device = None
|
||||
return False
|
||||
finally:
|
||||
self._initializing = False
|
||||
|
||||
def _quick_calibrate_imu(self) -> Dict[str, Any]:
|
||||
"""
|
||||
@ -464,20 +596,39 @@ class IMUManager(BaseDevice):
|
||||
bool: 启动是否成功
|
||||
"""
|
||||
try:
|
||||
if self.is_streaming:
|
||||
self.logger.warning("IMU数据流已在运行")
|
||||
return True
|
||||
|
||||
if not self.is_connected or not self.imu_device:
|
||||
if not self.initialize():
|
||||
return False
|
||||
|
||||
if not self.is_connected:
|
||||
self.logger.error("IMU设备未连接")
|
||||
return False
|
||||
|
||||
if self.imu_streaming:
|
||||
self.logger.warning("IMU数据流已在运行")
|
||||
return True
|
||||
first_data_timeout_s = float(self.config.get('first_data_timeout', 30.0))
|
||||
deadline = time.time() + max(0.1, first_data_timeout_s)
|
||||
while time.time() < deadline:
|
||||
if not self.imu_device:
|
||||
break
|
||||
if hasattr(self.imu_device, 'connected') and not bool(getattr(self.imu_device, 'connected')):
|
||||
break
|
||||
if bool(getattr(self.imu_device, 'has_received_data', False)):
|
||||
break
|
||||
time.sleep(0.05)
|
||||
if not self.imu_device or not bool(getattr(self.imu_device, 'has_received_data', False)):
|
||||
self.logger.error(f"IMU未获取到有效数据（等待 {first_data_timeout_s:.1f}s）")
|
||||
return False
|
||||
|
||||
# 启动前进行快速校准
|
||||
if not self.is_calibrated:
|
||||
self.logger.info("启动前进行快速零点校准...")
|
||||
self._quick_calibrate_imu()
|
||||
|
||||
self.imu_streaming = True
|
||||
self.is_streaming = True
|
||||
self.update_heartbeat()
|
||||
self.imu_thread = threading.Thread(target=self._imu_streaming_thread, daemon=True)
|
||||
self.imu_thread.start()
|
||||
|
||||
@ -486,7 +637,7 @@ class IMUManager(BaseDevice):
|
||||
|
||||
except Exception as e:
|
||||
self.logger.error(f"IMU数据流启动失败: {e}")
|
||||
self.imu_streaming = False
|
||||
self.is_streaming = False
|
||||
return False
|
||||
|
||||
def stop_streaming(self) -> bool:
|
||||
@ -497,7 +648,7 @@ class IMUManager(BaseDevice):
|
||||
bool: 停止是否成功
|
||||
"""
|
||||
try:
|
||||
self.imu_streaming = False
|
||||
self.is_streaming = False
|
||||
|
||||
if self.imu_thread and self.imu_thread.is_alive():
|
||||
self.imu_thread.join(timeout=3.0)
|
||||
@ -522,19 +673,25 @@ class IMUManager(BaseDevice):
|
||||
"""
|
||||
self.logger.info("IMU数据流工作线程启动")
|
||||
|
||||
while self.imu_streaming:
|
||||
while self.is_streaming:
|
||||
try:
|
||||
if self.imu_device:
|
||||
# 读取IMU数据
|
||||
data = self.imu_device.read_data(apply_calibration=True)
|
||||
|
||||
if data:
|
||||
if data and isinstance(data, dict) and data.get('head_pose') is not None:
|
||||
# 发送数据到前端
|
||||
if self._socketio:
|
||||
self._socketio.emit('imu_data', data, namespace='/devices')
|
||||
|
||||
# 更新统计
|
||||
self.data_count += 1
|
||||
self.update_heartbeat()
|
||||
self.last_valid_data = data
|
||||
try:
|
||||
self.data_buffer.append(data)
|
||||
except Exception:
|
||||
pass
|
||||
else:
|
||||
self.error_count += 1
|
||||
|
||||
@ -556,19 +713,19 @@ class IMUManager(BaseDevice):
|
||||
Returns:
|
||||
Dict[str, Any]: 设备状态信息
|
||||
"""
|
||||
status = super().get_status()
|
||||
status.update({
|
||||
'is_streaming': self.imu_streaming,
|
||||
return {
|
||||
'device_type': 'mock' if self.use_mock else 'ble',
|
||||
'mac_address': self.mac_address,
|
||||
'is_connected': self.is_connected,
|
||||
'is_streaming': self.is_streaming,
|
||||
'is_calibrated': self.is_calibrated,
|
||||
'data_count': self.data_count,
|
||||
'error_count': self.error_count,
|
||||
'buffer_size': len(self.data_buffer),
|
||||
'has_data': self.last_valid_data is not None,
|
||||
'head_pose_offset': self.head_pose_offset,
|
||||
'device_type': 'mock' if self.use_mock else 'ble',
|
||||
'mac_address': self.mac_address
|
||||
})
|
||||
return status
|
||||
'device_info': self.get_device_info()
|
||||
}
|
||||
|
||||
def get_latest_data(self) -> Optional[Dict[str, float]]:
|
||||
"""
|
||||
@ -586,9 +743,15 @@ class IMUManager(BaseDevice):
|
||||
断开IMU设备连接
|
||||
"""
|
||||
try:
|
||||
self._init_abort.set()
|
||||
self.stop_streaming()
|
||||
|
||||
if self.imu_device:
|
||||
try:
|
||||
if hasattr(self.imu_device, 'stop'):
|
||||
self.imu_device.stop()
|
||||
except Exception:
|
||||
pass
|
||||
self.imu_device = None
|
||||
|
||||
self.is_connected = False
|
||||
@ -643,7 +806,18 @@ class IMUManager(BaseDevice):
|
||||
return False
|
||||
|
||||
if hasattr(self.imu_device, 'connected'):
|
||||
return bool(getattr(self.imu_device, 'connected'))
|
||||
connected = bool(getattr(self.imu_device, 'connected'))
|
||||
if connected:
|
||||
last_ts = getattr(self.imu_device, 'last_update_time', None)
|
||||
if last_ts:
|
||||
try:
|
||||
if time.time() - float(last_ts) <= float(self._connection_timeout):
|
||||
self.update_heartbeat()
|
||||
except Exception:
|
||||
self.update_heartbeat()
|
||||
else:
|
||||
self.update_heartbeat()
|
||||
return connected
|
||||
|
||||
if hasattr(self.imu_device, 'ser') and getattr(self.imu_device, 'ser', None):
|
||||
if not self.imu_device.ser.is_open:
|
||||
@ -653,6 +827,7 @@ class IMUManager(BaseDevice):
|
||||
self.imu_device.ser.timeout = 0.1
|
||||
self.imu_device.ser.read(1)
|
||||
self.imu_device.ser.timeout = original_timeout
|
||||
self.update_heartbeat()
|
||||
return True
|
||||
except Exception:
|
||||
return False
|
||||
@ -668,6 +843,7 @@ class IMUManager(BaseDevice):
|
||||
清理资源
|
||||
"""
|
||||
try:
|
||||
self._init_abort.set()
|
||||
# 停止连接监控
|
||||
self._cleanup_monitoring()
|
||||
|
||||
|
||||
@ -1,69 +1,84 @@
|
||||
import argparse
|
||||
import asyncio
|
||||
import time
|
||||
from statistics import mean
|
||||
|
||||
import bleak
|
||||
import device_model
|
||||
|
||||
# 扫描到的设备 Scanned devices
|
||||
devices = []
|
||||
# 蓝牙设备 BLEDevice
|
||||
BLEDevice = None
|
||||
|
||||
|
||||
# 扫描蓝牙设备并过滤名称
|
||||
# Scan Bluetooth devices and filter names
|
||||
async def scan():
|
||||
global devices
|
||||
global BLEDevice
|
||||
find = []
|
||||
print("Searching for Bluetooth devices......")
|
||||
async def find_device_by_address(address: str, timeout_s: float):
|
||||
try:
|
||||
devices = await bleak.BleakScanner.discover(timeout=20.0)
|
||||
print("Search ended")
|
||||
for d in devices:
|
||||
if d.name is not None and "WT" in d.name:
|
||||
find.append(d)
|
||||
print(d)
|
||||
if len(find) == 0:
|
||||
print("No devices found in this search!")
|
||||
return await bleak.BleakScanner.find_device_by_address(address, timeout=timeout_s)
|
||||
except TypeError:
|
||||
return await bleak.BleakScanner.find_device_by_address(address, cb=dict(use_bdaddr=False))
|
||||
|
||||
|
||||
async def find_device_by_name(name: str, timeout_s: float):
|
||||
scanner_fn = getattr(bleak.BleakScanner, "find_device_by_name", None)
|
||||
if callable(scanner_fn):
|
||||
return await scanner_fn(name, timeout=timeout_s)
|
||||
devices = await bleak.BleakScanner.discover(timeout=timeout_s)
|
||||
for d in devices:
|
||||
if (getattr(d, "name", None) or "") == name:
|
||||
return d
|
||||
return None
|
||||
|
||||
|
||||
async def run_trials(label: str, finder, runs: int, cooldown_s: float):
|
||||
ok_times = []
|
||||
fail = 0
|
||||
|
||||
for i in range(1, runs + 1):
|
||||
start = time.perf_counter()
|
||||
device = await finder()
|
||||
ms = (time.perf_counter() - start) * 1000
|
||||
if device is None:
|
||||
fail += 1
|
||||
print(f"[{label}] [{i:03d}] FAIL {ms:.1f}ms")
|
||||
else:
|
||||
user_input = input("Please enter the Mac address you want to connect to (e.g. DF:E9:1F:2C:BD:59)：")
|
||||
for d in devices:
|
||||
if d.address == user_input:
|
||||
BLEDevice = d
|
||||
break
|
||||
except Exception as ex:
|
||||
print("Bluetooth search failed to start")
|
||||
print(ex)
|
||||
addr = getattr(device, "address", None)
|
||||
name = getattr(device, "name", None)
|
||||
ok_times.append(ms)
|
||||
print(f"[{label}] [{i:03d}] OK {ms:.1f}ms address={addr} name={name}")
|
||||
if cooldown_s > 0:
|
||||
await asyncio.sleep(cooldown_s)
|
||||
|
||||
|
||||
# 指定MAC地址搜索并连接设备
|
||||
# Specify MAC address to search and connect devices
|
||||
async def scanByMac(device_mac):
|
||||
global BLEDevice
|
||||
print("Searching for Bluetooth devices......")
|
||||
BLEDevice = await bleak.BleakScanner.find_device_by_address(device_mac, timeout=20)
|
||||
|
||||
|
||||
# 数据更新时会调用此方法 This method will be called when data is updated
|
||||
def updateData(DeviceModel):
|
||||
# 直接打印出设备数据字典 Directly print out the device data dictionary
|
||||
print(DeviceModel.deviceData)
|
||||
# 获得X轴加速度 Obtain X-axis acceleration
|
||||
# print(DeviceModel.get("AccX"))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
# 方式一：广播搜索和连接蓝牙设备
|
||||
# # Method 1:Broadcast search and connect Bluetooth devices
|
||||
# asyncio.run(scan())
|
||||
|
||||
# # 方式二：指定MAC地址搜索并连接设备
|
||||
# # Method 2: Specify MAC address to search and connect devices
|
||||
asyncio.run(scanByMac("FA:E8:88:06:FE:F3"))
|
||||
|
||||
if BLEDevice is not None:
|
||||
# 创建设备 Create device
|
||||
device = device_model.DeviceModel("MyBle5.0", BLEDevice, updateData)
|
||||
# 开始连接设备 Start connecting devices
|
||||
asyncio.run(device.openDevice())
|
||||
if ok_times:
|
||||
print(f"[{label}] runs={runs} success={len(ok_times)} fail={fail} avg={mean(ok_times):.1f}ms min={min(ok_times):.1f}ms max={max(ok_times):.1f}ms")
|
||||
else:
|
||||
print("This BLEDevice was not found!!")
|
||||
print(f"[{label}] runs={runs} success=0 fail={fail}")
|
||||
|
||||
|
||||
def parse_args():
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument("--address", default="FA:E8:88:06:FE:F3")
|
||||
parser.add_argument("--name", default="WT901BLE67")
|
||||
parser.add_argument("--runs", type=int, default=10)
|
||||
parser.add_argument("--timeout", type=float, default=30.0)
|
||||
parser.add_argument("--cooldown", type=float, default=0.3)
|
||||
parser.add_argument("--mode", choices=["mac", "name", "both"], default="both")
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
async def main():
|
||||
args = parse_args()
|
||||
|
||||
if args.mode in ("mac", "both"):
|
||||
await run_trials(
|
||||
"mac",
|
||||
lambda: find_device_by_address(args.address, args.timeout),
|
||||
args.runs,
|
||||
args.cooldown,
|
||||
)
|
||||
|
||||
if args.mode in ("name", "both"):
|
||||
await run_trials(
|
||||
"name",
|
||||
lambda: find_device_by_name(args.name, args.timeout),
|
||||
args.runs,
|
||||
args.cooldown,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
asyncio.run(main())
|
||||
|
||||
@ -98,6 +98,7 @@ class ConfigManager:
|
||||
self.config['DEVICES'] = {
|
||||
'imu_enable': 'False',
|
||||
'imu_use_mock': 'False',
|
||||
'imu_ble_name': '',
|
||||
'imu_mac_address': '',
|
||||
'pressure_port': 'COM8',
|
||||
'pressure_baudrate': '115200'
|
||||
@ -235,6 +236,7 @@ class ConfigManager:
|
||||
return {
|
||||
'enable': self.config.getboolean('DEVICES', 'imu_enable', fallback=False),
|
||||
'use_mock': self.config.getboolean('DEVICES', 'imu_use_mock', fallback=False),
|
||||
'ble_name': self.config.get('DEVICES', 'imu_ble_name', fallback=''),
|
||||
'mac_address': self.config.get('DEVICES', 'imu_mac_address', fallback='FA:E8:88:06:FE:F3'),
|
||||
}
|
||||
|
||||
|
||||
@ -18,6 +18,7 @@ import logging
|
||||
from flask_socketio import SocketIO, emit
|
||||
import configparser
|
||||
import argparse
|
||||
from collections import defaultdict
|
||||
|
||||
# 添加当前目录到路径
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
||||
@ -81,6 +82,7 @@ class AppServer:
|
||||
|
||||
# 数据推送状态
|
||||
self.is_pushing_data = False
|
||||
self._device_op_locks = defaultdict(threading.RLock)
|
||||
|
||||
# 设备管理器
|
||||
self.config_manager = None
|
||||
@ -97,6 +99,9 @@ class AppServer:
|
||||
# 注册路由和事件
|
||||
self._register_routes()
|
||||
self._register_socketio_events()
|
||||
|
||||
def _get_device_lock(self, device_name: str):
|
||||
return self._device_op_locks[device_name]
|
||||
|
||||
def _init_logging(self):
|
||||
"""初始化日志配置"""
|
||||
@ -228,6 +233,10 @@ class AppServer:
|
||||
self.logger.info('正在初始化设备协调器...')
|
||||
self.device_coordinator = DeviceCoordinator(self.socketio)
|
||||
|
||||
# Flask应用启动后，异步初始化设备（如果尚未初始化）
|
||||
if self.device_coordinator and not self.device_coordinator.is_initialized:
|
||||
threading.Thread(target=self._initialize_devices, daemon=True).start()
|
||||
|
||||
# 初始化录制管理器
|
||||
self.logger.info('正在初始化录制管理器...')
|
||||
|
||||
@ -247,8 +256,8 @@ class AppServer:
|
||||
if self.socketio:
|
||||
self.socketio.run(self.app, host=host, port=port, debug=debug, allow_unsafe_werkzeug=True)
|
||||
else:
|
||||
self.app.run(host=host, port=port, debug=debug)
|
||||
|
||||
self.app.run(host=host, port=port, debug=debug)
|
||||
|
||||
except Exception as e:
|
||||
self.logger.error(f'应用初始化失败: {e}')
|
||||
raise
|
||||
@ -755,9 +764,7 @@ class AppServer:
|
||||
|
||||
self.logger.info(f'用户 {username} 登录成功')
|
||||
|
||||
# 登录成功后，异步初始化设备（如果尚未初始化）
|
||||
if self.device_coordinator and not self.device_coordinator.is_initialized:
|
||||
threading.Thread(target=self._initialize_devices, daemon=True).start()
|
||||
|
||||
|
||||
return jsonify({
|
||||
'success': True,
|
||||
@ -1964,7 +1971,8 @@ class AppServer:
|
||||
self.logger.info(f'开始重启 {device_type} 设备...')
|
||||
|
||||
# 调用设备协调器的重启方法
|
||||
success = self.device_coordinator.restart_device(device_type)
|
||||
with self._get_device_lock(device_type):
|
||||
success = self.device_coordinator.restart_device(device_type)
|
||||
|
||||
if success:
|
||||
self.logger.info(f'{device_type} 设备重启成功')
|
||||
@ -2023,6 +2031,25 @@ class AppServer:
|
||||
def initialize_device(device_name, manager):
|
||||
"""设备初始化工作函数"""
|
||||
try:
|
||||
with self._get_device_lock(device_name):
|
||||
restarting = bool(getattr(self.device_coordinator, '_restart_in_progress', {}).get(device_name, False))
|
||||
if restarting:
|
||||
device_results[device_name] = False
|
||||
self.logger.warning(f'{device_name}设备正在重启，跳过启动')
|
||||
return
|
||||
|
||||
initializing = bool(getattr(manager, '_initializing', False))
|
||||
if initializing:
|
||||
wait_deadline = time.time() + (60.0 if device_name == 'imu' else 30.0)
|
||||
while time.time() < wait_deadline and bool(getattr(manager, '_initializing', False)):
|
||||
time.sleep(0.2)
|
||||
if hasattr(manager, 'is_connected') and manager.is_connected:
|
||||
if hasattr(manager, 'is_streaming') and not manager.is_streaming:
|
||||
manager.start_streaming()
|
||||
device_results[device_name] = True
|
||||
self.logger.info(f'{device_name}设备已连接，启动成功')
|
||||
return
|
||||
|
||||
# 检查设备是否已连接，避免重复初始化
|
||||
if hasattr(manager, 'is_connected') and manager.is_connected:
|
||||
print(f"[DEBUG] {device_name} 已连接，跳过初始化")
|
||||
|
||||
BIN
frontend/src/renderer/src/assets/glb/head.glb
Normal file
BIN
frontend/src/renderer/src/assets/glb/head.glb
Normal file
Binary file not shown.
BIN
frontend/src/renderer/src/assets/glb/xyz-axes.png
Normal file
BIN
frontend/src/renderer/src/assets/glb/xyz-axes.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 7.1 KiB |
@ -1774,47 +1774,50 @@ function handleIMUData(data) {
|
||||
function updateHeadPoseMaxValues(headPose) {
|
||||
try {
|
||||
// 更新旋转角最值
|
||||
if (headPose.rotation < 0) {
|
||||
if (headPose.rotation > 0) {
|
||||
// 左旋（负值），取绝对值的最大值
|
||||
headPoseMaxValues.value.rotationLeftMax = Math.max(
|
||||
headPoseMaxValues.value.rotationLeftMax,
|
||||
Math.abs(headPose.rotation)
|
||||
headPose.rotation
|
||||
)
|
||||
} else if (headPose.rotation > 0) {
|
||||
} else if (headPose.rotation < 0) {
|
||||
// 右旋（正值）
|
||||
headPoseMaxValues.value.rotationRightMax = Math.max(
|
||||
headPoseMaxValues.value.rotationRightMax,
|
||||
headPose.rotation
|
||||
Math.abs(headPose.rotation)
|
||||
|
||||
)
|
||||
}
|
||||
|
||||
// 更新倾斜角最值
|
||||
if (headPose.tilt < 0) {
|
||||
if (headPose.tilt > 0) {
|
||||
// 左倾（负值），取绝对值的最大值
|
||||
headPoseMaxValues.value.tiltLeftMax = Math.max(
|
||||
headPoseMaxValues.value.tiltLeftMax,
|
||||
Math.abs(headPose.tilt)
|
||||
headPose.tilt
|
||||
|
||||
)
|
||||
} else if (headPose.tilt > 0) {
|
||||
} else if (headPose.tilt < 0) {
|
||||
// 右倾（正值）
|
||||
headPoseMaxValues.value.tiltRightMax = Math.max(
|
||||
headPoseMaxValues.value.tiltRightMax,
|
||||
headPose.tilt
|
||||
Math.abs(headPose.tilt)
|
||||
)
|
||||
}
|
||||
|
||||
// 更新俯仰角最值
|
||||
if (headPose.pitch < 0) {
|
||||
if (headPose.pitch >0) {
|
||||
// 下俯（负值），取绝对值的最大值
|
||||
headPoseMaxValues.value.pitchDownMax = Math.max(
|
||||
headPoseMaxValues.value.pitchDownMax,
|
||||
Math.abs(headPose.pitch)
|
||||
headPose.pitch
|
||||
|
||||
)
|
||||
} else if (headPose.pitch > 0) {
|
||||
} else if (headPose.pitch < 0) {
|
||||
// 上仰（正值）
|
||||
headPoseMaxValues.value.pitchUpMax = Math.max(
|
||||
headPoseMaxValues.value.pitchUpMax,
|
||||
headPose.pitch
|
||||
Math.abs(headPose.pitch)
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
@ -1,13 +1,16 @@
|
||||
<template>
|
||||
<div id="containermodel"></div>
|
||||
<div id="containermodel">
|
||||
<img class="axes-overlay" :src="axesBgUrl" alt="" />
|
||||
</div>
|
||||
</template>
|
||||
|
||||
<script setup>
|
||||
import { onMounted, onUnmounted, watch } from 'vue'
|
||||
import * as THREE from 'three';
|
||||
import { GLTFLoader } from 'three/examples/jsm/loaders/GLTFLoader.js';
|
||||
import maleUrl from '@/assets/glb/male.glb?url'
|
||||
import femaleUrl from '@/assets/glb/female.glb?url'
|
||||
import modelUrl from '@/assets/glb/head.glb?url'
|
||||
import axesBgUrl from '@/assets/glb/xyz-axes.png?url'
|
||||
|
||||
|
||||
const props = defineProps({
|
||||
rotation: { type: [Number, String], default: 0 },
|
||||
@ -25,7 +28,7 @@ let onResizeHandler = null;
|
||||
|
||||
// 渲染循环控制
|
||||
let lastRenderTime = 0;
|
||||
const TARGET_FPS = 10; // 限制为25帧以平衡流畅度与性能
|
||||
const TARGET_FPS = 20; // 限制为25帧以平衡流畅度与性能
|
||||
const FRAME_INTERVAL = 1000 / TARGET_FPS;
|
||||
|
||||
onMounted(() => {
|
||||
@ -121,7 +124,7 @@ function setupLights() {
|
||||
|
||||
function loadModel() {
|
||||
const loader = new GLTFLoader();
|
||||
const url = (props.gender === '女') ? femaleUrl : maleUrl;
|
||||
const url = modelUrl;
|
||||
|
||||
loader.load(url,
|
||||
(gltf) => {
|
||||
@ -132,11 +135,11 @@ function loadModel() {
|
||||
const box = new THREE.Box3().setFromObject(model);
|
||||
const size = box.getSize(new THREE.Vector3());
|
||||
const maxDim = Math.max(size.x, size.y, size.z);
|
||||
const scale = 5 / maxDim;
|
||||
const scale = 3.8 / maxDim;
|
||||
|
||||
model.scale.set(scale, scale, scale);
|
||||
const center = box.getCenter(new THREE.Vector3());
|
||||
model.position.set(-center.x * scale, -center.y * scale + 0.2, -center.z * scale);
|
||||
model.position.set(-center.x * scale, -center.y * scale-0.2, -center.z * scale);
|
||||
model.rotation.set(0, 0, 0);
|
||||
|
||||
model.traverse((child) => {
|
||||
@ -270,4 +273,19 @@ watch(
|
||||
position: relative;
|
||||
overflow: hidden; /* 防止 canvas 溢出 */
|
||||
}
|
||||
</style>
|
||||
|
||||
.axes-overlay {
|
||||
position: absolute;
|
||||
inset: 0;
|
||||
width: 100%;
|
||||
height: 100%;
|
||||
object-fit: contain;
|
||||
z-index: 2;
|
||||
pointer-events: none;
|
||||
}
|
||||
|
||||
:global(#containermodel canvas) {
|
||||
position: relative;
|
||||
z-index: 1;
|
||||
}
|
||||
</style>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user