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 datetime from biohead.algo import * from biohead.utils import calc_feature from biohead.utils import norm from google.protobuf import timestamp_pb2 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): """Helper to build CommandHeader.Request""" now = datetime.datetime.utcnow() timestamp = timestamp_pb2.Timestamp() timestamp.FromDatetime(now) return common_pb2.CommandHeader.Request( device_id=device_id, timestamp=timestamp ) def build_facial_expression(result): """Map your local HeadJoints result to proto FacialExpression""" expr = biohead_command_pb2.FacialExpression() # 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 # 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 # Eyeball expr.eyeball.left_x = result.left_eye_ball_x expr.eyeball.left_y = result.left_eye_ball_y expr.eyeball.right_x = result.right_eye_ball_x expr.eyeball.right_y = result.right_eye_ball_y # 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 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 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 # Jaw expr.jaw.x = result.jaw_x expr.jaw.y = result.jaw_y return expr def main(calib_file): # ====== Load config ====== with open(r"./config/config.yaml", "r") as f: config = yaml.load(f, Loader=yaml.FullLoader) with open(calib_file, "r") as f: calib = yaml.load(f, Loader=yaml.FullLoader) # ====== Init RealSense ====== w, h, fps = config['Camera']['image_width'], config['Camera']['image_height'], config['Camera']['fps'] 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) # ====== Init 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['Smooth']) ray_directions = deque(maxlen=config['Smooth']) result = HeadJoints() # ====== Init gRPC ====== device_id = "your_device_id" frame_queue = queue.Queue(maxsize=10) result_queue = queue.Queue() channel = grpc.insecure_channel('localhost:50051') stub = biohead_service_pb2_grpc.BioHeadServiceStub(channel) def request_stream(): while True: item = frame_queue.get() if item is None: break header, expr = item yield biohead_service_pb2.StreamFacialExpression.Request( header=header, expr=expr, eof=False ) def response_reader(responses): for response in responses: if response.HasField("expr_diff"): result_queue.put(response.expr_diff) responses = stub.StreamExpression(request_stream()) reader_thread = threading.Thread(target=response_reader, args=(responses,), daemon=True) reader_thread.start() print("[INFO] Started RealSense + gRPC streaming client.") while True: frames = align.process(pipe.wait_for_frames()) color_f = frames.get_color_frame() if not color_f: continue color = np.asanyarray(color_f.get_data()) h, w, _ = color.shape rgb = cv2.cvtColor(color, cv2.COLOR_BGR2RGB) res = mesh.process(rgb) if not res.multi_face_landmarks: cv2.imshow("RGB", color) if cv2.waitKey(1) & 0xFF == ord('q'): break continue uv = calc_feature(color, res, 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) # ====== Build proto request ====== header = build_command_header(device_id) expr = build_facial_expression(result) if not frame_queue.full(): frame_queue.put((header, expr)) # ====== Draw server feedback if any ====== # if not result_queue.empty(): # expr_diff = result_queue.get() # y0 = 30 # dy = 30 # # # Draw expr_diff fields on screen # # For simplicity, let's just show some sample values # if expr_diff.HasField("eyebrow"): # txt = f"Eyebrow L-out:{expr_diff.eyebrow.left_outside_y:.2f}" # cv2.putText(color, txt, (30, y0), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # y0 += dy # # if expr_diff.HasField("eyelid"): # txt = f"Eyelid L-upper:{expr_diff.eyelid.left_upper_y:.2f}" # cv2.putText(color, txt, (30, y0), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # y0 += dy # # if expr_diff.HasField("jaw"): # txt = f"Jaw X:{expr_diff.jaw.x:.2f} Y:{expr_diff.jaw.y:.2f}" # cv2.putText(color, txt, (30, y0), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2) # y0 += dy cv2.imshow("RGB", color) if cv2.waitKey(1) & 0xFF == ord('q'): break frame_queue.put(None) pipe.stop() cv2.destroyAllWindows() print("[INFO] Client shut down.") if __name__ == '__main__': main("./config/calibrated_interval.yaml") # import sys # sys.path.insert(0, "./generated") # import grpc # import time # import random # import sys # import traceback # import logging # from google.protobuf import timestamp_pb2 # from generated.cmvr.api import common_pb2 # from generated.cmvr.api import biohead_command_pb2 # from generated.cmvr.api import biohead_service_pb2 # from generated.cmvr.api import biohead_service_pb2_grpc # # # 打印出biohead_command_pb2的生成类结构 # print(dir(biohead_command_pb2)) # # # 设置详细日志 # logging.basicConfig(level=logging.DEBUG) # logger = logging.getLogger('grpc_test') # logger.setLevel(logging.DEBUG) # # # 添加控制台处理器 # console_handler = logging.StreamHandler() # console_handler.setLevel(logging.DEBUG) # formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s') # console_handler.setFormatter(formatter) # logger.addHandler(console_handler) # # # 检查生成的代码结构 # logger.debug("在biohead_service_pb2中生成的类：") # for attr in dir(biohead_service_pb2): # if "FacialExpression" in attr or "Stream" in attr: # logger.debug(f" - {attr}") # # def create_facial_expression(): # """创建随机的面部表情数据""" # expr = biohead_command_pb2.FacialExpression() # # # 眉毛 # expr.eyebrow.left_outside_y = random.uniform(0.0, 1.0) # expr.eyebrow.left_inside_y = random.uniform(0.0, 1.0) # expr.eyebrow.right_outside_y = random.uniform(0.0, 1.0) # expr.eyebrow.right_inside_y = random.uniform(0.0, 1.0) # # # 眼睑 # expr.eyelid.left_upper_y = random.uniform(0.0, 1.0) # expr.eyelid.left_lower_y = random.uniform(0.0, 1.0) # expr.eyelid.right_upper_y = random.uniform(0.0, 1.0) # expr.eyelid.right_lower_y = random.uniform(0.0, 1.0) # # # 眼球 # expr.eyeball.left_y = random.uniform(0, 1.0) # 动态值 # expr.eyeball.right_y = random.uniform(0, 1.0) # 动态值 # # # # 嘴巴 # expr.mouth.upper_lip_y = random.uniform(0.0, 1.0) # expr.mouth.lower_lip_y = random.uniform(0.0, 1.0) # # # # 左唇角 # expr.mouth.left_lip.upper_y = random.uniform(0.0, 1.0) # expr.mouth.left_lip.corner_y = random.uniform(0.0, 1.0) # # # 右唇角 # expr.mouth.right_lip.upper_y = random.uniform(0.0, 1.0) # expr.mouth.right_lip.corner_y = random.uniform(0.0, 1.0) # # # 下巴 # expr.jaw.x = random.uniform(0, 1.0) # expr.jaw.y = random.uniform(0, 1.0) # # return expr # # def create_request(device_id): # """创建流式请求""" # # 使用正确的请求类名 # # 根据proto文件，请求类名应该是 StreamFacialExpressionRequest # request = biohead_command_pb2.StreamFacialExpression.Request() # # # 设置请求头 # request.header.device_id = device_id # now = time.time() # request.header.timestamp.seconds = int(now) # request.header.timestamp.nanos = int((now - int(now)) * 1e9) # # # 设置表情数据 # expr = create_facial_expression() # request.expr.CopyFrom(expr) # request.eof = False # # # 记录请求详情 # logger.debug(f"为设备 {device_id} 创建请求") # logger.debug(f"表情字段: {expr.ListFields()}") # # return request # # def stream_expression_test(device_id="bio_head", num_requests=5, interval=1.0): # """测试流式表情接口""" # logger.info(f"开始测试流式表情接口，设备: {device_id}") # logger.info(f"将发送 {num_requests} 个请求，频率为 {1/interval:.1f} Hz") # # # 创建gRPC通道 # channel = grpc.insecure_channel('localhost:50051') # stub = biohead_service_pb2_grpc.BioHeadServiceStub(channel) # # # 创建生成器函数 # def request_generator(): # try: # for i in range(num_requests): # request = create_request(device_id) # logger.info(f"发送请求 #{i+1}") # logger.debug(f"请求内容: {request}") # yield request # time.sleep(interval) # # # 发送结束标志 # end_request = biohead_command_pb2.SetFacialExpression.Feedback() # end_request.header.device_id = device_id # end_request.eof = True # logger.info("发送EOF请求") # yield end_request # except Exception as e: # logger.error(f"请求生成器出错: {str(e)}") # logger.error(traceback.format_exc()) # # # 调用流式方法 # try: # responses = stub.StreamExpression(request_generator()) # # # 处理响应 # response_count = 0 # for response in responses: # response_count += 1 # header = response.header # logger.info(f"收到响应 #{response_count}") # logger.info(f" 成功: {header.success}") # logger.info(f" 时间戳: {header.timestamp.seconds}.{header.timestamp.nanos:09d}") # # if not header.success: # logger.error(f" 错误: {header.error_message}") # # if response.HasField("expr_diff"): # diff = response.expr_diff # logger.info(" 收到表情差异") # # 记录差异详情 # logger.debug(f" 眉毛差异: L-out: {diff.eyebrow.left_outside_y:.4f}") # # logger.info(f"总共收到 {response_count} 个响应") # # except grpc.RpcError as e: # logger.error(f"gRPC错误: {e.code()}: {e.details()}") # logger.error(f"调试错误信息: {e.debug_error_string()}") # except Exception as e: # logger.error(f"意外错误: {str(e)}") # logger.error(traceback.format_exc()) # # logger.info("流式表情测试完成") # # if __name__ == '__main__': # # 测试参数 # DEVICE_ID = "bio_head" # NUM_REQUESTS = 5 # INTERVAL = 1.0 # # try: # stream_expression_test(device_id=DEVICE_ID, # num_requests=NUM_REQUESTS, # interval=INTERVAL) # except Exception as e: # logger.error(f"测试失败: {str(e)}") # logger.error(traceback.format_exc()) # sys.exit(1) # #