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)