389 lines
13 KiB
Python
389 lines
13 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 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)
|
||
#
|
||
#
|