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)
|
|||
|
|
#
|
|||
|
|
#
|