307 lines
11 KiB
Python
307 lines
11 KiB
Python
import sys
|
||
sys.path.insert(0, "./generated")
|
||
|
||
import cv2
|
||
import yaml
|
||
import numpy as np
|
||
import mediapipe as mp
|
||
import pyrealsense2 as rs
|
||
from collections import deque
|
||
import grpc
|
||
import threading
|
||
import queue
|
||
import time
|
||
import traceback
|
||
|
||
from biohead.algo import *
|
||
from biohead.utils import calc_feature
|
||
from biohead.utils import norm
|
||
|
||
from datetime import datetime
|
||
from google.protobuf.timestamp_pb2 import Timestamp
|
||
from generated.cmvr.api import common_pb2
|
||
from generated.cmvr.api import biohead_service_pb2
|
||
from generated.cmvr.api import biohead_command_pb2
|
||
from generated.cmvr.api import biohead_service_pb2_grpc
|
||
|
||
|
||
def build_command_header(device_id):
|
||
"""构建命令头部proto消息 - 使用common.proto中的定义"""
|
||
# 使用common.proto中的CommandHeader.Request
|
||
header = common_pb2.CommandHeader.Request()
|
||
|
||
# 设置device_id
|
||
header.device_id = device_id
|
||
|
||
# 设置timestamp
|
||
now = datetime.utcnow()
|
||
timestamp = Timestamp()
|
||
timestamp.FromDatetime(now)
|
||
header.timestamp.CopyFrom(timestamp)
|
||
|
||
return header
|
||
|
||
|
||
def build_facial_expression(result):
|
||
"""将算法结果转换为proto面部表情消息 - 完整映射所有字段"""
|
||
expr = biohead_command_pb2.FacialExpression()
|
||
|
||
|
||
try:
|
||
# 眉毛部分
|
||
if hasattr(expr, 'eyebrow'):
|
||
expr.eyebrow.left_outside_y = result.left_eyebrow_outside_y
|
||
expr.eyebrow.left_inside_y = result.left_eyebrow_inside_y
|
||
expr.eyebrow.right_outside_y = result.right_eyebrow_outside_y
|
||
expr.eyebrow.right_inside_y = result.right_eyebrow_inside_y
|
||
|
||
# 眼睑部分s
|
||
if hasattr(expr, 'eyelid'):
|
||
expr.eyelid.left_upper_y = result.left_eye_upper_lid_y
|
||
expr.eyelid.left_lower_y = result.left_eye_lower_lid_y
|
||
expr.eyelid.right_upper_y = result.right_eye_upper_lid_y
|
||
expr.eyelid.right_lower_y = result.right_eye_lower_lid_y
|
||
print(expr.eyelid.left_upper_y, expr.eyelid.left_lower_y,expr.eyelid.right_upper_y, expr.eyelid.right_lower_y)
|
||
|
||
# 眼球部分
|
||
if hasattr(expr, 'eyeball'):
|
||
expr.eyeball.left_y = result.left_eye_ball_y
|
||
expr.eyeball.right_y = result.right_eye_ball_y
|
||
|
||
|
||
# 鼻子部分
|
||
# 注意：算法结果中没有鼻子数据，但proto定义了nose字段
|
||
# 如果需要，可以从算法中提取并设置
|
||
|
||
# 嘴巴部分
|
||
if hasattr(expr, 'mouth'):
|
||
expr.mouth.upper_lip_y = result.upper_lip_y
|
||
expr.mouth.upper_lip_z = result.upper_lip_z
|
||
expr.mouth.lower_lip_y = result.lower_lip_y
|
||
expr.mouth.lower_lip_z = result.lower_lip_z
|
||
|
||
# 左嘴唇细节
|
||
if hasattr(expr.mouth, 'left_lip'):
|
||
expr.mouth.left_lip.upper_x = result.upper_left_lip_x
|
||
expr.mouth.left_lip.upper_y = result.upper_left_lip_y
|
||
expr.mouth.left_lip.corner_x = result.left_corner_lip_x
|
||
expr.mouth.left_lip.corner_y = result.left_corner_lip_y
|
||
expr.mouth.left_lip.lower_x = result.lower_left_lip_x
|
||
expr.mouth.left_lip.lower_y = result.lower_left_lip_y
|
||
|
||
# 右嘴唇细节
|
||
if hasattr(expr.mouth, 'right_lip'):
|
||
expr.mouth.right_lip.upper_x = result.upper_right_lip_x
|
||
expr.mouth.right_lip.upper_y = result.upper_right_lip_y
|
||
expr.mouth.right_lip.corner_x = result.right_corner_lip_x
|
||
expr.mouth.right_lip.corner_y = result.right_corner_lip_y
|
||
expr.mouth.right_lip.lower_x = result.lower_right_lip_x
|
||
expr.mouth.right_lip.lower_y = result.lower_right_lip_y
|
||
|
||
# 下巴部分
|
||
if hasattr(expr, 'jaw'):
|
||
expr.jaw.x = result.jaw_y
|
||
expr.jaw.y = result.jaw_y
|
||
|
||
except AttributeError as e:
|
||
print(f"构建面部表情错误: {str(e)}")
|
||
traceback.print_exc()
|
||
|
||
return expr
|
||
|
||
def main(calib_file):
|
||
# 加载配置文件
|
||
with open("./config/config.yaml", "r") as f:
|
||
config = yaml.safe_load(f)
|
||
|
||
with open(calib_file, "r") as f:
|
||
calib = yaml.safe_load(f)
|
||
|
||
# 初始化RealSense相机
|
||
w, h, fps = config['Camera']['image_width'], config['Camera']['image_height'], config['Camera'].get('fps', 50)
|
||
pipe = rs.pipeline()
|
||
cfg = rs.config()
|
||
cfg.enable_stream(rs.stream.color, w, h, rs.format.bgr8, fps)
|
||
pipe.start(cfg)
|
||
align = rs.align(rs.stream.color)
|
||
|
||
# 初始化MediaPipe面部网格
|
||
mp_mesh = mp.solutions.face_mesh
|
||
mesh = mp_mesh.FaceMesh(
|
||
max_num_faces=1,
|
||
refine_landmarks=True,
|
||
min_detection_confidence=config['MediaPipe']['min_detection_confidence'],
|
||
min_tracking_confidence=config['MediaPipe']['min_tracking_confidence']
|
||
)
|
||
|
||
# 初始化平滑处理队列
|
||
ray_origins = deque(maxlen=config.get('Smooth', 5))
|
||
ray_directions = deque(maxlen=config.get('Smooth', 5))
|
||
result = HeadJoints()
|
||
|
||
# 初始化gRPC客户端
|
||
device_id = "bio_head"
|
||
frame_queue = queue.Queue(maxsize=30) # 增大队列容量
|
||
|
||
# 创建gRPC通道和存根
|
||
# channel = grpc.insecure_channel('localhost:50051')
|
||
channel = grpc.insecure_channel('10.148.108.162:50052')
|
||
|
||
stub = biohead_service_pb2_grpc.BioHeadServiceStub(channel)
|
||
|
||
# 请求生成器函数 - 使用正确的请求类型
|
||
def request_generator():
|
||
try:
|
||
while True:
|
||
item = frame_queue.get()
|
||
if item is None:
|
||
# 发送结束请求
|
||
request = biohead_command_pb2.StreamFacialExpression.Request(
|
||
header=build_command_header(device_id),
|
||
eof=True
|
||
)
|
||
yield request
|
||
break
|
||
|
||
header, expr = item
|
||
request = biohead_command_pb2.StreamFacialExpression.Request(
|
||
header=header,
|
||
expr=expr,
|
||
eof=False
|
||
)
|
||
yield request
|
||
frame_queue.task_done()
|
||
except Exception as e:
|
||
print(f"请求生成器出错: {str(e)}")
|
||
traceback.print_exc()
|
||
|
||
# 启动gRPC流式调用
|
||
response_stream = stub.StreamExpression(request_generator())
|
||
|
||
# 响应处理函数
|
||
def process_responses():
|
||
try:
|
||
for response in response_stream:
|
||
# 根据proto定义，响应应该是StreamFacialExpression.Feedback类型
|
||
if response.HasField("header"):
|
||
print(f"服务器响应 - 成功: {response.header.success}, 消息: {response.header.error_message}")
|
||
|
||
if response.HasField("expr_diff"):
|
||
print(f"收到表情差异 - 左眼位置差异: {response.expr_diff.eyeball.left_y:.2f}")
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"gRPC错误 - 代码: {e.code()}, 详情: {e.details()}")
|
||
print(f"调试信息: {e.debug_error_string()}")
|
||
except Exception as e:
|
||
print(f"响应处理异常: {str(e)}")
|
||
traceback.print_exc()
|
||
|
||
|
||
|
||
|
||
print("[INFO] 启动RealSense + gRPC流式客户端")
|
||
|
||
try:
|
||
frame_count = 0
|
||
start_time = time.time()
|
||
frame_skip = 2 # 每2帧处理1次，降低数据生成速度
|
||
frame_counter = 0
|
||
last_called_time = time.time() # 用于跟踪上次调用时间
|
||
|
||
while True:
|
||
current_time = time.time()
|
||
|
||
# 判断是否已经2秒过去
|
||
# time.sleep(0.1)
|
||
# if current_time - last_called_time >= 0.15: # 2秒间隔
|
||
frame_counter += 1
|
||
last_called_time = current_time # 更新上次调用时间
|
||
|
||
# 获取相机帧
|
||
frames = pipe.wait_for_frames()
|
||
aligned_frames = align.process(frames)
|
||
color_frame = aligned_frames.get_color_frame()
|
||
if not color_frame:
|
||
continue
|
||
|
||
# 处理图像与面部特征
|
||
color_image = np.asanyarray(color_frame.get_data())
|
||
rgb_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB)
|
||
results = mesh.process(rgb_image)
|
||
if not results.multi_face_landmarks:
|
||
cv2.putText(color_image, "未检测到面部", (20, 40),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
|
||
cv2.imshow("RGB", color_image)
|
||
if cv2.waitKey(1) & 0xFF == ord('q'):
|
||
break
|
||
continue
|
||
|
||
# 计算特征与归一化
|
||
uv = calc_feature(color_image, results, ray_origins, ray_directions)
|
||
result = calc_eyebrow(uv, result)
|
||
result = calc_eyelid(uv, result)
|
||
result = calc_eyeball(uv, result)
|
||
result = calc_mouth(uv, result)
|
||
result = calc_jaw(uv, result)
|
||
result = norm(result, calib)
|
||
|
||
# 计算并显示最新的FPS
|
||
elapsed_time = time.time() - start_time
|
||
fps = frame_count / elapsed_time if elapsed_time > 0 else 0
|
||
|
||
# 构建并发送数据
|
||
try:
|
||
header = build_command_header(device_id)
|
||
expr = build_facial_expression(result)
|
||
except Exception as e:
|
||
print(f"构建gRPC消息错误: {str(e)}")
|
||
traceback.print_exc()
|
||
continue
|
||
|
||
# 入队
|
||
try:
|
||
frame_queue.put((header, expr), timeout=0.01)
|
||
print(f"帧入队成功，队列大小: {frame_queue.qsize()}")
|
||
except queue.Full:
|
||
print(f"[WARNING] 队列已满（{frame_queue.qsize()}/{frame_queue.maxsize}），跳过当前帧")
|
||
|
||
# 显示状态
|
||
frame_count += 1
|
||
cv2.putText(color_image, f"FPS: {fps:.1f} | 队列: {frame_queue.qsize()}/30", (20, 40),
|
||
cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
|
||
cv2.imshow("RGB", color_image)
|
||
|
||
key = cv2.waitKey(1)
|
||
if key & 0xFF == ord('q'):
|
||
break
|
||
elif key & 0xFF == ord(' '):
|
||
cv2.waitKey(0)
|
||
|
||
except KeyboardInterrupt:
|
||
print("用户中断")
|
||
except Exception as e:
|
||
print(f"主循环错误: {str(e)}")
|
||
traceback.print_exc()
|
||
finally:
|
||
print("[INFO] 关闭客户端...")
|
||
frame_queue.put(None)
|
||
pipe.stop()
|
||
cv2.destroyAllWindows()
|
||
response_thread.join(timeout=2.0)
|
||
channel.close()
|
||
print("[INFO] 客户端已关闭")
|
||
|
||
|
||
if __name__ == '__main__':
|
||
try:
|
||
main("./config/calibrated_interval.yaml")
|
||
except Exception as e:
|
||
print(f"程序异常: {str(e)}")
|
||
traceback.print_exc()
|
||
time.sleep(5)
|
||
|
||
|
||
|
||
|
||
|