595 lines
22 KiB
Python
595 lines
22 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
设备测试类
|
||
用于测试和模拟四个设备的推流功能：深度相机、普通相机、压力板、IMU
|
||
"""
|
||
|
||
import os
|
||
import sys
|
||
import time
|
||
import threading
|
||
import logging
|
||
import json
|
||
import base64
|
||
import numpy as np
|
||
import cv2
|
||
from flask import Flask, render_template, jsonify
|
||
from flask_socketio import SocketIO, emit
|
||
from typing import Dict, Any, Optional
|
||
from collections import deque
|
||
import random
|
||
import math
|
||
|
||
# 添加父目录到路径
|
||
parent_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
|
||
sys.path.append(parent_dir)
|
||
sys.path.append(os.path.dirname(parent_dir))
|
||
|
||
# 导入设备管理器（使用绝对路径）
|
||
try:
|
||
from devices.camera_manager import CameraManager
|
||
from devices.imu_manager import IMUManager
|
||
from devices.pressure_manager import PressureManager
|
||
from devices.femtobolt_manager import FemtoBoltManager
|
||
from devices.device_coordinator import DeviceCoordinator
|
||
from devices.utils.config_manager import ConfigManager
|
||
except ImportError:
|
||
# 如果上面的导入失败，尝试直接导入
|
||
from camera_manager import CameraManager
|
||
import imu_manager
|
||
import pressure_manager
|
||
import femtobolt_manager
|
||
import device_coordinator
|
||
from utils import config_manager
|
||
|
||
IMUManager = imu_manager.IMUManager
|
||
PressureManager = pressure_manager.PressureManager
|
||
FemtoBoltManager = femtobolt_manager.FemtoBoltManager
|
||
DeviceCoordinator = device_coordinator.DeviceCoordinator
|
||
ConfigManager = config_manager.ConfigManager
|
||
|
||
|
||
class DeviceTestServer:
|
||
"""设备测试服务器"""
|
||
|
||
def __init__(self, host='localhost', port=5001):
|
||
"""
|
||
初始化测试服务器
|
||
|
||
Args:
|
||
host: 服务器主机
|
||
port: 服务器端口
|
||
"""
|
||
self.host = host
|
||
self.port = port
|
||
|
||
# Flask应用
|
||
self.app = Flask(__name__,
|
||
template_folder=os.path.join(os.path.dirname(__file__), 'templates'),
|
||
static_folder=os.path.join(os.path.dirname(__file__), 'static'))
|
||
self.app.config['SECRET_KEY'] = 'device_test_secret_key'
|
||
|
||
# SocketIO
|
||
self.socketio = SocketIO(self.app, cors_allowed_origins="*",
|
||
async_mode='threading', logger=False, engineio_logger=False)
|
||
|
||
# 日志配置
|
||
logging.basicConfig(level=logging.INFO)
|
||
self.logger = logging.getLogger(self.__class__.__name__)
|
||
|
||
# 设备管理器
|
||
self.config_manager = ConfigManager()
|
||
self.device_coordinator = None
|
||
|
||
# 设备管理器和模拟数据生成器
|
||
self.device_managers = {
|
||
'camera': CameraManager(self.socketio, self.config_manager),
|
||
'femtobolt': FemtoBoltManager(self.socketio, self.config_manager),
|
||
'imu': IMUManager(self.socketio, self.config_manager),
|
||
'pressure': PressureManager(self.socketio, self.config_manager)
|
||
}
|
||
self.mock_data_generators = {
|
||
# 'imu': MockIMUGenerator(),
|
||
# 'pressure': MockPressureGenerator()
|
||
}
|
||
|
||
# 测试状态
|
||
self.is_testing = False
|
||
self.test_threads = {}
|
||
|
||
# 注册路由和事件
|
||
self._register_routes()
|
||
self._register_socketio_events()
|
||
|
||
def _register_routes(self):
|
||
"""注册Flask路由"""
|
||
|
||
@self.app.route('/')
|
||
def index():
|
||
"""主页"""
|
||
return render_template('deviceTest.html')
|
||
|
||
@self.app.route('/api/device/status')
|
||
def get_device_status():
|
||
"""获取设备状态"""
|
||
if self.device_coordinator:
|
||
status = self.device_coordinator.get_device_status()
|
||
else:
|
||
status = {
|
||
'coordinator': {'is_initialized': False, 'is_running': False},
|
||
'devices': {}
|
||
}
|
||
return jsonify(status)
|
||
|
||
@self.app.route('/api/test/start')
|
||
def start_test():
|
||
"""开始测试"""
|
||
try:
|
||
self.start_device_test()
|
||
return jsonify({'success': True, 'message': '测试已开始'})
|
||
except Exception as e:
|
||
return jsonify({'success': False, 'message': str(e)})
|
||
|
||
@self.app.route('/api/test/stop')
|
||
def stop_test():
|
||
"""停止测试"""
|
||
try:
|
||
self.stop_device_test()
|
||
return jsonify({'success': True, 'message': '测试已停止'})
|
||
except Exception as e:
|
||
return jsonify({'success': False, 'message': str(e)})
|
||
|
||
def _register_socketio_events(self):
|
||
"""注册SocketIO事件"""
|
||
|
||
@self.socketio.on('connect')
|
||
def handle_connect():
|
||
self.logger.info(f'客户端连接: {id}')
|
||
emit('status', {'message': '连接成功'})
|
||
|
||
@self.socketio.on('disconnect')
|
||
def handle_disconnect():
|
||
self.logger.info(f'客户端断开连接')
|
||
|
||
@self.socketio.on('start_test')
|
||
def handle_start_test():
|
||
"""处理开始测试事件"""
|
||
try:
|
||
self.start_device_test()
|
||
emit('test_status', {'status': 'started', 'message': '测试已开始'})
|
||
except Exception as e:
|
||
emit('test_status', {'status': 'error', 'message': str(e)})
|
||
|
||
@self.socketio.on('stop_test')
|
||
def handle_stop_test():
|
||
"""处理停止测试事件"""
|
||
try:
|
||
self.stop_device_test()
|
||
emit('test_status', {'status': 'stopped', 'message': '测试已停止'})
|
||
except Exception as e:
|
||
emit('test_status', {'status': 'error', 'message': str(e)})
|
||
|
||
# 注册各设备命名空间的连接事件
|
||
@self.socketio.on('connect', namespace='/camera')
|
||
def handle_camera_connect():
|
||
self.logger.info('相机命名空间客户端连接')
|
||
emit('status', {'message': '相机命名空间连接成功'}, namespace='/camera')
|
||
|
||
@self.socketio.on('connect', namespace='/femtobolt')
|
||
def handle_femtobolt_connect():
|
||
self.logger.info('深度相机命名空间客户端连接')
|
||
emit('status', {'message': '深度相机命名空间连接成功'}, namespace='/femtobolt')
|
||
|
||
@self.socketio.on('connect', namespace='/imu')
|
||
def handle_imu_connect():
|
||
self.logger.info('IMU命名空间客户端连接')
|
||
emit('status', {'message': 'IMU命名空间连接成功'}, namespace='/imu')
|
||
|
||
@self.socketio.on('connect', namespace='/pressure')
|
||
def handle_pressure_connect():
|
||
self.logger.info('压力板命名空间客户端连接')
|
||
emit('status', {'message': '压力板命名空间连接成功'}, namespace='/pressure')
|
||
|
||
@self.socketio.on('disconnect', namespace='/camera')
|
||
def handle_camera_disconnect():
|
||
self.logger.info('相机命名空间客户端断开连接')
|
||
|
||
@self.socketio.on('disconnect', namespace='/femtobolt')
|
||
def handle_femtobolt_disconnect():
|
||
self.logger.info('深度相机命名空间客户端断开连接')
|
||
|
||
@self.socketio.on('disconnect', namespace='/imu')
|
||
def handle_imu_disconnect():
|
||
self.logger.info('IMU命名空间客户端断开连接')
|
||
|
||
@self.socketio.on('disconnect', namespace='/pressure')
|
||
def handle_pressure_disconnect():
|
||
self.logger.info('压力板命名空间客户端断开连接')
|
||
|
||
def start_device_test(self):
|
||
"""开始设备测试"""
|
||
if self.is_testing:
|
||
self.logger.warning('测试已在运行')
|
||
return
|
||
|
||
try:
|
||
self.logger.info('开始设备测试...')
|
||
self.is_testing = True
|
||
|
||
# 并行启动真实设备管理器
|
||
failed_devices = []
|
||
device_threads = {}
|
||
device_results = {}
|
||
|
||
def initialize_device(device_name, manager):
|
||
"""设备初始化工作函数"""
|
||
try:
|
||
print(f"[DEBUG] 尝试初始化设备: {device_name}")
|
||
if manager.initialize():
|
||
print(f"[DEBUG] {device_name} 初始化成功，开始启动流")
|
||
manager.start_streaming()
|
||
device_results[device_name] = True
|
||
self.logger.info(f'{device_name}真实设备启动成功')
|
||
else:
|
||
print(f"[DEBUG] {device_name} 初始化失败")
|
||
device_results[device_name] = False
|
||
self.logger.error(f'{device_name}真实设备启动失败，将使用模拟数据')
|
||
except Exception as e:
|
||
print(f"[DEBUG] {device_name} 初始化异常: {e}")
|
||
device_results[device_name] = False
|
||
self.logger.error(f'{device_name}真实设备启动异常: {e}，将使用模拟数据')
|
||
|
||
# 为每个设备创建初始化线程
|
||
for device_name, manager in self.device_managers.items():
|
||
thread = threading.Thread(
|
||
target=initialize_device,
|
||
args=(device_name, manager),
|
||
name=f'Init-{device_name}',
|
||
daemon=True
|
||
)
|
||
device_threads[device_name] = thread
|
||
thread.start()
|
||
|
||
# 等待所有设备初始化完成（最多等待30秒）
|
||
for device_name, thread in device_threads.items():
|
||
thread.join(timeout=30.0)
|
||
if thread.is_alive():
|
||
self.logger.warning(f'{device_name}设备初始化超时，将使用模拟数据')
|
||
device_results[device_name] = False
|
||
|
||
# 收集失败的设备
|
||
for device_name, success in device_results.items():
|
||
if not success:
|
||
failed_devices.append(device_name)
|
||
|
||
# 启动模拟数据生成线程（包括失败的真实设备）
|
||
for device_name, generator in self.mock_data_generators.items():
|
||
# 如果真实设备启动成功且不在失败列表中，跳过模拟数据生成
|
||
if device_name in self.device_managers and device_name not in failed_devices:
|
||
continue
|
||
|
||
thread = threading.Thread(
|
||
target=self._mock_device_worker,
|
||
args=(device_name, generator),
|
||
name=f'MockDevice-{device_name}',
|
||
daemon=True
|
||
)
|
||
thread.start()
|
||
self.test_threads[device_name] = thread
|
||
self.logger.info(f'{device_name}模拟数据生成器已启动')
|
||
|
||
# 输出启动结果摘要
|
||
successful_devices = [name for name, success in device_results.items() if success]
|
||
if successful_devices:
|
||
self.logger.info(f'成功启动的真实设备: {", ".join(successful_devices)}')
|
||
if failed_devices:
|
||
self.logger.info(f'使用模拟数据的设备: {", ".join(failed_devices)}')
|
||
|
||
self.logger.info('设备测试已启动')
|
||
|
||
except Exception as e:
|
||
self.logger.error(f'启动设备测试失败: {e}')
|
||
self.is_testing = False
|
||
raise
|
||
|
||
def stop_device_test(self):
|
||
"""停止设备测试"""
|
||
if not self.is_testing:
|
||
self.logger.warning('测试未运行')
|
||
return
|
||
|
||
try:
|
||
self.logger.info('停止设备测试...')
|
||
self.is_testing = False
|
||
|
||
# 停止真实设备管理器
|
||
for device_name, manager in self.device_managers.items():
|
||
manager.stop_streaming()
|
||
manager.disconnect()
|
||
self.logger.info(f'{device_name}真实设备已停止')
|
||
|
||
# 等待所有线程结束
|
||
for device_name, thread in self.test_threads.items():
|
||
if thread.is_alive():
|
||
thread.join(timeout=2.0)
|
||
|
||
self.test_threads.clear()
|
||
self.logger.info('设备测试已停止')
|
||
|
||
except Exception as e:
|
||
self.logger.error(f'停止设备测试失败: {e}')
|
||
|
||
def _mock_device_worker(self, device_name: str, generator):
|
||
"""模拟设备工作线程"""
|
||
self.logger.info(f'启动{device_name}模拟数据生成')
|
||
|
||
while self.is_testing:
|
||
try:
|
||
# 生成模拟数据
|
||
data = generator.generate_data()
|
||
|
||
# 发送到对应的命名空间
|
||
namespace = f'/{device_name}'
|
||
event_name = self._get_event_name(device_name)
|
||
|
||
self.socketio.emit(event_name, data, namespace=namespace)
|
||
|
||
# 控制发送频率
|
||
time.sleep(generator.get_interval())
|
||
|
||
except Exception as e:
|
||
self.logger.error(f'{device_name}模拟数据生成异常: {e}')
|
||
time.sleep(1.0)
|
||
|
||
self.logger.info(f'{device_name}模拟数据生成结束')
|
||
|
||
def _get_event_name(self, device_name: str) -> str:
|
||
"""获取设备对应的事件名称"""
|
||
event_map = {
|
||
'camera': 'camera_frame',
|
||
'femtobolt': 'femtobolt_frame',
|
||
'imu': 'imu_data',
|
||
'pressure': 'pressure_data'
|
||
}
|
||
return event_map.get(device_name, f'{device_name}_data')
|
||
|
||
def run(self, debug=False):
|
||
"""运行测试服务器"""
|
||
self.logger.info(f'启动设备测试服务器: http://{self.host}:{self.port}')
|
||
self.socketio.run(self.app, host=self.host, port=self.port, debug=debug)
|
||
|
||
|
||
# MockCameraGenerator已移除，使用真实的CameraManager
|
||
|
||
|
||
class MockFemtoBoltGenerator:
|
||
"""模拟FemtoBolt深度相机数据生成器"""
|
||
|
||
def __init__(self):
|
||
self.frame_count = 0
|
||
self.interval = 1.0 / 15 # 15 FPS
|
||
|
||
def generate_data(self) -> Dict[str, Any]:
|
||
"""生成模拟深度相机数据"""
|
||
# 生成深度图像
|
||
height, width = 480, 640
|
||
|
||
# 创建深度图像（模拟人体轮廓）
|
||
depth_image = np.full((height, width), 2000, dtype=np.uint16)
|
||
|
||
# 添加人体轮廓
|
||
center_x = width // 2
|
||
center_y = height // 2
|
||
|
||
# 头部
|
||
cv2.circle(depth_image, (center_x, center_y - 100), 40, 1500, -1)
|
||
# 身体
|
||
cv2.rectangle(depth_image, (center_x - 50, center_y - 60),
|
||
(center_x + 50, center_y + 100), 1600, -1)
|
||
# 手臂
|
||
cv2.rectangle(depth_image, (center_x - 80, center_y - 40),
|
||
(center_x - 50, center_y + 20), 1700, -1)
|
||
cv2.rectangle(depth_image, (center_x + 50, center_y - 40),
|
||
(center_x + 80, center_y + 20), 1700, -1)
|
||
|
||
# 转换为伪彩色
|
||
normalized = ((depth_image - 500) / (4500 - 500) * 255).astype(np.uint8)
|
||
colored = cv2.applyColorMap(normalized, cv2.COLORMAP_JET)
|
||
|
||
# 添加文字
|
||
cv2.putText(colored, f'Depth Frame {self.frame_count}',
|
||
(10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2)
|
||
|
||
# 编码为JPEG
|
||
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 85]
|
||
_, buffer = cv2.imencode('.jpg', colored, encode_param)
|
||
depth_data = base64.b64encode(buffer).decode('utf-8')
|
||
|
||
self.frame_count += 1
|
||
|
||
return {
|
||
'timestamp': time.time(),
|
||
'frame_count': self.frame_count,
|
||
'depth_image': depth_data,
|
||
'fps': 15,
|
||
'device_id': 'mock_femtobolt',
|
||
'depth_range': {'min': 500, 'max': 4500}
|
||
}
|
||
|
||
def get_interval(self) -> float:
|
||
return self.interval
|
||
|
||
|
||
class MockIMUGenerator:
|
||
"""模拟IMU传感器数据生成器"""
|
||
|
||
def __init__(self):
|
||
self.frame_count = 0
|
||
self.interval = 1.0 / 50 # 50 Hz
|
||
self.base_time = time.time()
|
||
|
||
def generate_data(self) -> Dict[str, Any]:
|
||
"""生成模拟IMU数据"""
|
||
current_time = time.time() - self.base_time
|
||
|
||
# 生成模拟的头部姿态数据（正弦波模拟头部运动）
|
||
rotation = 15 * math.sin(current_time * 0.5) # 旋转角
|
||
tilt = 10 * math.cos(current_time * 0.3) # 倾斜角
|
||
pitch = 8 * math.sin(current_time * 0.7) # 俯仰角
|
||
|
||
# 生成加速度和陀螺仪数据
|
||
accel_x = 0.1 * math.sin(current_time * 2.0)
|
||
accel_y = 0.1 * math.cos(current_time * 1.5)
|
||
accel_z = 9.8 + 0.2 * math.sin(current_time * 0.8)
|
||
|
||
gyro_x = 2.0 * math.cos(current_time * 1.2)
|
||
gyro_y = 1.5 * math.sin(current_time * 0.9)
|
||
gyro_z = 1.0 * math.sin(current_time * 1.1)
|
||
|
||
self.frame_count += 1
|
||
|
||
return {
|
||
'timestamp': time.time(),
|
||
'frame_count': self.frame_count,
|
||
'device_id': 'mock_imu',
|
||
'head_pose': {
|
||
'rotation': round(rotation, 2),
|
||
'tilt': round(tilt, 2),
|
||
'pitch': round(pitch, 2)
|
||
},
|
||
'accelerometer': {
|
||
'x': round(accel_x, 3),
|
||
'y': round(accel_y, 3),
|
||
'z': round(accel_z, 3)
|
||
},
|
||
'gyroscope': {
|
||
'x': round(gyro_x, 3),
|
||
'y': round(gyro_y, 3),
|
||
'z': round(gyro_z, 3)
|
||
},
|
||
'temperature': round(25.0 + 2.0 * math.sin(current_time * 0.1), 1)
|
||
}
|
||
|
||
def get_interval(self) -> float:
|
||
return self.interval
|
||
|
||
|
||
class MockPressureGenerator:
|
||
"""模拟压力传感器数据生成器"""
|
||
|
||
def __init__(self):
|
||
self.frame_count = 0
|
||
self.interval = 1.0 / 20 # 20 Hz
|
||
self.base_time = time.time()
|
||
|
||
def generate_data(self) -> Dict[str, Any]:
|
||
"""生成模拟压力数据"""
|
||
current_time = time.time() - self.base_time
|
||
|
||
# 生成模拟的足部压力数据（模拟重心转移）
|
||
base_pressure = 50
|
||
shift = 20 * math.sin(current_time * 0.3) # 重心左右转移
|
||
|
||
left_total = max(0, base_pressure + shift + random.uniform(-5, 5))
|
||
right_total = max(0, base_pressure - shift + random.uniform(-5, 5))
|
||
|
||
# 前后足压力分配
|
||
left_front = left_total * (0.4 + 0.1 * math.sin(current_time * 0.5))
|
||
left_rear = left_total - left_front
|
||
right_front = right_total * (0.4 + 0.1 * math.cos(current_time * 0.5))
|
||
right_rear = right_total - right_front
|
||
|
||
# 生成压力热力图
|
||
pressure_image = self._generate_pressure_heatmap(
|
||
left_front, left_rear, right_front, right_rear
|
||
)
|
||
|
||
self.frame_count += 1
|
||
|
||
return {
|
||
'timestamp': time.time(),
|
||
'frame_count': self.frame_count,
|
||
'device_id': 'mock_pressure',
|
||
'pressure_data': {
|
||
'left_total': round(left_total, 1),
|
||
'right_total': round(right_total, 1),
|
||
'left_front': round(left_front, 1),
|
||
'left_rear': round(left_rear, 1),
|
||
'right_front': round(right_front, 1),
|
||
'right_rear': round(right_rear, 1),
|
||
'total_pressure': round(left_total + right_total, 1),
|
||
'balance_ratio': round(left_total / (left_total + right_total) * 100, 1) if (left_total + right_total) > 0 else 50.0
|
||
},
|
||
'pressure_image': pressure_image
|
||
}
|
||
|
||
def _generate_pressure_heatmap(self, left_front, left_rear, right_front, right_rear) -> str:
|
||
"""生成压力热力图"""
|
||
# 创建足底压力图像
|
||
height, width = 300, 300
|
||
image = np.zeros((height, width, 3), dtype=np.uint8)
|
||
|
||
# 左足区域
|
||
left_foot_x = width // 4
|
||
left_front_intensity = int(min(255, left_front * 5))
|
||
left_rear_intensity = int(min(255, left_rear * 5))
|
||
|
||
# 左前足
|
||
cv2.rectangle(image, (left_foot_x - 30, height // 4),
|
||
(left_foot_x + 30, height // 2),
|
||
(0, 0, left_front_intensity), -1)
|
||
|
||
# 左后足
|
||
cv2.rectangle(image, (left_foot_x - 30, height // 2),
|
||
(left_foot_x + 30, height * 3 // 4),
|
||
(0, 0, left_rear_intensity), -1)
|
||
|
||
# 右足区域
|
||
right_foot_x = width * 3 // 4
|
||
right_front_intensity = int(min(255, right_front * 5))
|
||
right_rear_intensity = int(min(255, right_rear * 5))
|
||
|
||
# 右前足
|
||
cv2.rectangle(image, (right_foot_x - 30, height // 4),
|
||
(right_foot_x + 30, height // 2),
|
||
(0, 0, right_front_intensity), -1)
|
||
|
||
# 右后足
|
||
cv2.rectangle(image, (right_foot_x - 30, height // 2),
|
||
(right_foot_x + 30, height * 3 // 4),
|
||
(0, 0, right_rear_intensity), -1)
|
||
|
||
# 添加分割线
|
||
cv2.line(image, (width // 2, 0), (width // 2, height), (255, 255, 255), 2)
|
||
cv2.line(image, (0, height // 2), (width, height // 2), (255, 255, 255), 1)
|
||
|
||
# 编码为Base64
|
||
encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 85]
|
||
_, buffer = cv2.imencode('.jpg', image, encode_param)
|
||
return base64.b64encode(buffer).decode('utf-8')
|
||
|
||
def get_interval(self) -> float:
|
||
return self.interval
|
||
|
||
|
||
def main():
|
||
"""主函数"""
|
||
# 创建templates目录
|
||
templates_dir = os.path.join(os.path.dirname(__file__), 'templates')
|
||
os.makedirs(templates_dir, exist_ok=True)
|
||
|
||
# 创建测试服务器
|
||
server = DeviceTestServer(host='localhost', port=5001)
|
||
|
||
try:
|
||
# 运行服务器
|
||
server.run(debug=False)
|
||
except KeyboardInterrupt:
|
||
print('\n服务器已停止')
|
||
except Exception as e:
|
||
print(f'服务器运行异常: {e}')
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main() |