From d45f2367831431a0e60c48daa8b42ffec17aa8a6 Mon Sep 17 00:00:00 2001 From: tankaitao <1767759995@qq.com> Date: Thu, 21 Aug 2025 13:49:41 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- biohead/__init__.py | 0 biohead/algo.py | 299 ++++++++++++++ biohead/define.py | 118 ++++++ biohead/utils.py | 139 +++++++ calibration.py | 92 +++++ config/calibrated_interval.yaml | 102 +++++ config/config.yaml | 54 +++ demo.py | 82 ++++ generated/cmvr/api/biohead_command_pb2.py | 67 +++ .../cmvr/api/biohead_command_pb2_grpc.py | 4 + generated/cmvr/api/biohead_service_pb2.py | 27 ++ .../cmvr/api/biohead_service_pb2_grpc.py | 174 ++++++++ generated/cmvr/api/common_pb2.py | 35 ++ generated/cmvr/api/common_pb2_grpc.py | 4 + grpc_cli.py | 388 ++++++++++++++++++ grpc_lb.py | 306 ++++++++++++++ protos/cmvr/api/biohead_command.proto | 175 ++++++++ protos/cmvr/api/biohead_service.proto | 22 + protos/cmvr/api/common.proto | 30 ++ requirements.txt | 69 ++++ script/grpc_gen.sh | 27 ++ text_dgceshi.py | 198 +++++++++ 22 files changed, 2412 insertions(+) create mode 100644 biohead/__init__.py create mode 100644 biohead/algo.py create mode 100644 biohead/define.py create mode 100644 biohead/utils.py create mode 100644 calibration.py create mode 100644 config/calibrated_interval.yaml create mode 100644 config/config.yaml create mode 100644 demo.py create mode 100644 generated/cmvr/api/biohead_command_pb2.py create mode 100644 generated/cmvr/api/biohead_command_pb2_grpc.py create mode 100644 generated/cmvr/api/biohead_service_pb2.py create mode 100644 generated/cmvr/api/biohead_service_pb2_grpc.py create mode 100644 generated/cmvr/api/common_pb2.py create mode 100644 generated/cmvr/api/common_pb2_grpc.py create mode 100644 grpc_cli.py create mode 100644 grpc_lb.py create mode 100644 protos/cmvr/api/biohead_command.proto create mode 100644 protos/cmvr/api/biohead_service.proto create mode 100644 protos/cmvr/api/common.proto create mode 100644 requirements.txt create mode 100644 script/grpc_gen.sh create mode 100644 text_dgceshi.py diff --git a/biohead/__init__.py b/biohead/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/biohead/algo.py b/biohead/algo.py new file mode 100644 index 0000000..3d9275c --- /dev/null +++ b/biohead/algo.py @@ -0,0 +1,299 @@ +import numpy as np +from biohead.define import HeadJoints, FaceMap +import math +from scipy.optimize import fsolve +from biohead.define import HeadJoints, FaceMap +class LowPassFilter: + def __init__(self, alpha=0.2): + self.alpha = alpha + self.last_val = None + + def __call__(self, val): + if self.last_val is None: + self.last_val = val + else: + self.last_val = self.alpha * val + (1 - self.alpha) * self.last_val + return self.last_val + +lip_z_lpf_left = LowPassFilter(alpha=0.5) +lip_z_lpf_right = LowPassFilter(alpha=0.5) + + + +def calc_eyebrow(uv, result: HeadJoints): + try: + # LEFT + left_outer = np.array(uv[FaceMap.left_eyelid_outside]) + left_inner = np.array(uv[FaceMap.left_eyelid_inside]) + eye_width_left = np.linalg.norm(left_inner - left_outer) + 1e-6 + dir_eye_left = (left_inner - left_outer) / eye_width_left + perp_left = np.cross(np.array([0, 0, 1]), dir_eye_left) + perp_left /= np.linalg.norm(perp_left) + 1e-6 + mid_left = (left_outer + left_inner) / 2 + delta_out_left = np.array(uv[FaceMap.left_eyebrow_outside]) - mid_left + delta_in_left = np.array(uv[FaceMap.left_eyebrow_inside]) - mid_left + result.left_eyebrow_outside_y = abs(np.dot(delta_out_left, perp_left)) / eye_width_left + result.left_eyebrow_inside_y = abs(np.dot(delta_in_left, perp_left)) / eye_width_left + + # RIGHT + right_outer = np.array(uv[FaceMap.right_eyelid_outside]) + right_inner = np.array(uv[FaceMap.right_eyelid_inside]) + eye_width_right = np.linalg.norm(right_inner - right_outer) + 1e-6 + dir_eye_right = (right_inner - right_outer) / eye_width_right + perp_right = np.cross(np.array([0, 0, 1]), dir_eye_right) + perp_right /= np.linalg.norm(perp_right) + 1e-6 + mid_right = (right_outer + right_inner) / 2 + delta_out_right = np.array(uv[FaceMap.right_eyebrow_outside]) - mid_right + delta_in_right = np.array(uv[FaceMap.right_eyebrow_inside]) - mid_right + result.right_eyebrow_outside_y = abs(np.dot(delta_out_right, perp_right)) / eye_width_right + result.right_eyebrow_inside_y = abs(np.dot(delta_in_right, perp_right)) / eye_width_right + return result + except KeyError as e: + print("Eyebrow landmarks missing:", e) + + + + +def calc_eyelid(uv, result: HeadJoints): + try: + # LEFT + left_outer = np.array(uv[FaceMap.left_eyelid_outside]) + left_inner = np.array(uv[FaceMap.left_eyelid_inside]) + eye_width_left = np.linalg.norm(left_inner - left_outer) + 1e-6 + dir_eye_left = (left_inner - left_outer) / eye_width_left + perp_left = np.cross(np.array([0, 0, 1]), dir_eye_left) + perp_left /= np.linalg.norm(perp_left) + 1e-6 + mid_left = (left_outer + left_inner) / 2 + delta_upper_left = np.array(uv[FaceMap.left_eyelid_upper]) - mid_left + delta_lower_left = np.array(uv[FaceMap.left_eyelid_lower]) - mid_left + result.left_eye_upper_lid_y = abs(np.dot(delta_upper_left, perp_left)) / eye_width_left + result.left_eye_lower_lid_y = abs(np.dot(delta_lower_left, perp_left)) / eye_width_left + + # RIGHT + right_outer = np.array(uv[FaceMap.right_eyelid_outside]) + right_inner = np.array(uv[FaceMap.right_eyelid_inside]) + eye_width_right = np.linalg.norm(right_inner - right_outer) + 1e-6 + dir_eye_right = (right_inner - right_outer) / eye_width_right + perp_right = np.cross(np.array([0, 0, 1]), dir_eye_right) + perp_right /= np.linalg.norm(perp_right) + 1e-6 + mid_right = (right_outer + right_inner) / 2 + delta_upper_right = np.array(uv[FaceMap.right_eyelid_upper]) - mid_right + delta_lower_right = np.array(uv[FaceMap.right_eyelid_lower]) - mid_right + result.right_eye_upper_lid_y = abs(np.dot(delta_upper_right, perp_right)) / eye_width_right + result.right_eye_lower_lid_y = abs(np.dot(delta_lower_right, perp_right)) / eye_width_right + return result + except KeyError as e: + print("Eyelid landmarks missing:", e) + +def calc_eyeball(uv, result: HeadJoints): + try: + # LEFT + left_outer = np.array(uv[FaceMap.left_eyelid_outside]) + left_inner = np.array(uv[FaceMap.left_eyelid_inside]) + eye_width_left = np.linalg.norm(left_inner - left_outer) + 1e-6 + dir_eye_left = (left_inner - left_outer) / eye_width_left + perp_left = np.cross(np.array([0, 0, 1]), dir_eye_left) + perp_left /= np.linalg.norm(perp_left) + 1e-6 + mid_left = (left_outer + left_inner) / 2 + eyeball_center_left = np.array(uv[FaceMap.left_eyeball_center]) + delta_left = eyeball_center_left - mid_left + result.left_eye_ball_x = np.dot(delta_left, dir_eye_left) / eye_width_left + result.left_eye_ball_y = np.dot(delta_left, perp_left) / eye_width_left + + # RIGHT + right_outer = np.array(uv[FaceMap.right_eyelid_outside]) + right_inner = np.array(uv[FaceMap.right_eyelid_inside]) + eye_width_right = np.linalg.norm(right_inner - right_outer) + 1e-6 + dir_eye_right = (right_inner - right_outer) / eye_width_right + perp_right = np.cross(np.array([0, 0, 1]), dir_eye_right) + perp_right /= np.linalg.norm(perp_right) + 1e-6 + mid_right = (right_outer + right_inner) / 2 + eyeball_center_right = np.array(uv[FaceMap.right_eyeball_center]) + delta_right = eyeball_center_right - mid_right + result.right_eye_ball_x = np.dot(delta_right, dir_eye_right) / eye_width_right + result.right_eye_ball_y = np.dot(delta_right, perp_right) / eye_width_right + return result + except KeyError as e: + print("Eyeball landmarks missing:", e) + + + +def _jaw_ik_eqs_3d(thetas, y_d, z_d, L1, L2): + theta1, theta2 = thetas + t1, t2 = math.radians(theta1), math.radians(theta2) + + y = L1 * math.cos(t1) + L2 * math.cos(t2) + z = L1 * math.sin(t1) + L2 * math.sin(t2) + + return [ + y - y_d, + z - z_d + ] + +def _solve_jaw_servo_angles(y_d, z_d, L1, L2, init_guess=(0.0, 0.0)): + sol = fsolve(_jaw_ik_eqs_3d, init_guess, args=(y_d, z_d, L1, L2)) + return sol[0], sol[1] + +def _normalize_angle(angle, min_angle=-100, max_angle=100): + return max(0.0, min(1.0, (angle - min_angle) / (max_angle - min_angle))) + +def calc_mouth(uv, result: HeadJoints): + try: + left_uv = uv[FaceMap.left_head] + right_uv = uv[FaceMap.right_head] + top_uv = uv[FaceMap.top_head] + bottom_uv = uv[FaceMap.bottom_head] + + head_width = abs(right_uv[0] - left_uv[0]) + 1e-6 + head_height = abs(top_uv[1] - bottom_uv[1]) + 1e-6 + + # === 新增：计算头部平均深度用于 z 归一化 === + head_depth = ( + abs(left_uv[2]) + abs(right_uv[2]) + + abs(top_uv[2]) + abs(bottom_uv[2]) + + abs(uv[FaceMap.mid_up_lip][2]) + ) / 5 + 1e-6 + + upper_center_u = (left_uv[0] + right_uv[0] + top_uv[0]) / 3 + upper_center_v = (left_uv[1] + right_uv[1] + top_uv[1]) / 3 + bottom_center_u = bottom_uv[0] + bottom_center_v = bottom_uv[1] + + upper_mouth_points = { + "upper_right_lip": FaceMap.right_upper_lip, + "upper_left_lip": FaceMap.left_upper_lip, + "right_corner_lip": FaceMap.right_mouth_corner, + "left_corner_lip": FaceMap.left_mouth_corner, + "upper_lip": FaceMap.mid_up_lip + } + + jaw_y_accum = 0.0 + jaw_z_accum = 0.0 + jaw_count = 0 + + for name, idx in upper_mouth_points.items(): + u, v, z = uv[idx] + delta_u = u - upper_center_u + delta_v = v - upper_center_v + x_val = delta_u / head_width + y_val = delta_v / head_height + + # === 滤波并归一化 z 值 === + if name == "left_corner_lip": + z = lip_z_lpf_left(z) + elif name == "right_corner_lip": + z = lip_z_lpf_right(z) + z_val = z / head_depth + + setattr(result, f"{name}_x", x_val) + setattr(result, f"{name}_y", y_val) + setattr(result, f"{name}_z", z_val) + + if name in ("left_corner_lip", "right_corner_lip"): + jaw_y_accum += y_val * head_height + jaw_z_accum += z_val * head_height + jaw_count += 1 + + # === IK 解算（仅嘴角） === + if jaw_count > 0: + jaw_displacement_y = jaw_y_accum / jaw_count + jaw_displacement_z = jaw_z_accum / jaw_count + else: + jaw_displacement_y = 0.0 + jaw_displacement_z = 0.0 + + L1, L2 = 145, 177 + theta1, theta2 = _solve_jaw_servo_angles( + y_d=jaw_displacement_y, + z_d=jaw_displacement_z, + L1=L1, + L2=L2, + init_guess=(0.0, 0.0) + ) + + norm_theta1 = _normalize_angle(theta1) + norm_theta2 = _normalize_angle(theta2) + print("norm_theta1", norm_theta1) + print("norm_theta2", norm_theta2) + + result.upper_left_lip_y = norm_theta1 + result.upper_right_lip_y = norm_theta1 + result.left_corner_lip_y = norm_theta2 + result.right_corner_lip_y = norm_theta2 + + # === 下唇位置记录（不参与 IK） === + lower_mouth_points = { + "lower_right_lip": FaceMap.right_lower_lip, + "lower_left_lip": FaceMap.left_lower_lip, + "lower_lip": FaceMap.mid_down_lip + } + + for name, idx in lower_mouth_points.items(): + u, v, z = uv[idx] + delta_u = u - bottom_center_u + delta_v = v - bottom_center_v + x_val = delta_u / head_width + y_val = delta_v / head_height + z_val = z / head_depth + setattr(result, f"{name}_x", x_val) + setattr(result, f"{name}_y", y_val) + setattr(result, f"{name}_z", z_val) + + + return result + + except KeyError as e: + print("Mouth landmarks missing:", e) + return result + + + +def calc_jaw(uv, result: HeadJoints): + try: + # === 获取头部关键点 === + upper_indices = [ + FaceMap.left_eyebrow_inside, + FaceMap.right_eyebrow_inside, + FaceMap.left_eyelid_inside, + FaceMap.right_eyelid_inside, + FaceMap.top_head + ] + + lower_indices = [ + FaceMap.mid_down_lip, + FaceMap.left_mouth_corner, + FaceMap.right_mouth_corner, + FaceMap.bottom_head + ] + + upper_points = np.array([uv[idx] for idx in upper_indices]) + lower_points = np.array([uv[idx] for idx in lower_indices]) + + upper_center = np.mean(upper_points, axis=0) + lower_center = np.mean(lower_points, axis=0) + + # === 获取头高进行归一化 === + top_uv = np.array(uv[FaceMap.top_head]) + bottom_uv = np.array(uv[FaceMap.bottom_head]) + head_height = abs(top_uv[1] - bottom_uv[1]) + 1e-6 + head_width = abs(uv[FaceMap.left_head][0] - uv[FaceMap.right_head][0]) + 1e-6 + + # === 张嘴判断：中心点垂直位移归一化 === + vertical_offset = abs(lower_center[1] - upper_center[1]) + mouth_open_ratio = vertical_offset / head_height + + if mouth_open_ratio > 0.035: + result.jaw_y = mouth_open_ratio + else: + result.jaw_y = 0.0 + + # === 左右偏移保留 === + upper_lip = np.array(uv[FaceMap.mid_up_lip]) + lower_lip = np.array(uv[FaceMap.mid_down_lip]) + result.jaw_x = abs(lower_lip[0] - upper_lip[0]) / head_width + + return result + + except KeyError as e: + print("Jaw landmarks missing:", e) + return result diff --git a/biohead/define.py b/biohead/define.py new file mode 100644 index 0000000..57413f5 --- /dev/null +++ b/biohead/define.py @@ -0,0 +1,118 @@ +from dataclasses import dataclass, asdict + + +@dataclass +class HeadJoints: + # Eyebrow + left_eyebrow_outside_y: float = 0.0 + left_eyebrow_inside_y: float = 0.0 + right_eyebrow_outside_y: float = 0.0 + right_eyebrow_inside_y: float = 0.0 + + # Eyelid + left_eye_upper_lid_y: float = 0.0 + left_eye_lower_lid_y: float = 0.0 + right_eye_upper_lid_y: float = 0.0 + right_eye_lower_lid_y: float = 0.0 + + # Eyeball + left_eye_ball_x: float = 0.0 + left_eye_ball_y: float = 0.0 + right_eye_ball_x: float = 0.0 + right_eye_ball_y: float = 0.0 + + # Nose + left_nose_y: float = 0.0 + right_nose_y: float = 0.0 + + # Mouth + upper_lip_y: float = 0.0 + upper_lip_z: float = 0.0 + lower_lip_y: float = 0.0 + lower_lip_z: float = 0.0 + + upper_left_lip_x: float = 0.0 + upper_left_lip_y: float = 0.0 + left_corner_lip_x: float = 0.0 + left_corner_lip_y: float = 0.0 + lower_left_lip_x: float = 0.0 + lower_left_lip_y: float = 0.0 + left_corner_lip_z: float = 0.0 + + upper_right_lip_x: float = 0.0 + upper_right_lip_y: float = 0.0 + right_corner_lip_x: float = 0.0 + right_corner_lip_y: float = 0.0 + lower_right_lip_x: float = 0.0 + lower_right_lip_y: float = 0.0 + right_corner_lip_z: float = 0.0 + + # Jaw + jaw_x: float = 0.0 + jaw_y: float = 0.0 + + def __str__(self): + return str(asdict(self)) + + def to_dict(self): + return asdict(self) + + +@dataclass(frozen=True) +class FaceMap: + # Base head anchors + left_head: int = 234 + right_head: int = 454 + top_head: int = 10 + bottom_head: int = 152 + front_head: int = 1 + + # Left eyebrow + left_eyebrow_outside: int = 300 + left_eyebrow_inside: int = 285 + + # Right eyebrow + right_eyebrow_outside: int = 70 + right_eyebrow_inside: int = 55 + + # Left eyelid + left_eyelid_outside: int = 263 + left_eyelid_inside: int = 362 + left_eyelid_upper: int = 386 + left_eyelid_lower: int = 374 + + # Right eyelid + right_eyelid_outside: int = 33 + right_eyelid_inside: int = 133 + right_eyelid_upper: int = 159 + right_eyelid_lower: int = 145 + + # Left eyeball + left_eyeball_center: int = 473 + left_eyeball_outside: int = 476 + left_eyeball_inside: int = 474 + left_eyeball_up: int = 475 + left_eyeball_down: int = 477 + + # Right eyeball + right_eyeball_center: int = 468 + right_eyeball_outside: int = 471 + right_eyeball_inside: int = 469 + right_eyeball_up: int = 470 + right_eyeball_down: int = 472 + + # Mouth + mid_up_lip: int = 0 + mid_down_lip: int = 17 + right_upper_lip: int = 39 + right_mouth_corner: int = 61 + right_lower_lip: int = 181 + left_upper_lip: int = 269 + left_mouth_corner: int = 291 + left_lower_lip: int = 405 + + def __str__(self): + return str(asdict(self)) + + def to_dict(self): + return asdict(self) diff --git a/biohead/utils.py b/biohead/utils.py new file mode 100644 index 0000000..02cb323 --- /dev/null +++ b/biohead/utils.py @@ -0,0 +1,139 @@ +import cv2 +from biohead.algo import * +import numpy as np + + +def calc_feature(color, landmarks, ray_origins, ray_directions): + h, w, _ = color.shape + face = landmarks.multi_face_landmarks[0].landmark + idxs = {"left":234, "right":454, "top":10, "bottom":152, "front":1} + kp = {k: np.array([face[v].x*w, face[v].y*h, face[v].z*w]) for k, v in idxs.items()} + left, right, top, bottom, front = kp.values() + + right_axis = right - left + right_axis /= np.linalg.norm(right_axis) + up_axis = top - bottom + up_axis /= np.linalg.norm(up_axis) + forward = np.cross(right_axis, up_axis) + forward /= np.linalg.norm(forward) + forward = -forward + + center = (left + right + top + bottom + front) / 5 + ray_origins.append(center) + ray_directions.append(forward) + origin = np.mean(ray_origins, axis=0) + forward = np.mean(ray_directions, axis=0) + forward /= np.linalg.norm(forward) + + uv = {} + for j in FaceMap().to_dict().values(): + p = np.array([face[j].x*w, face[j].y*h, face[j].z*w]) + vec = p - origin + u = np.dot(vec, right_axis) + v = np.dot(vec, up_axis) + z = np.dot(vec, forward) + uv[j] = (u, v, z) + x2d, y2d = int(p[0]), int(p[1]) + cv2.circle(color, (x2d, y2d), 2, (0, 255, 255), -1) + + left_mouth_corner = np.array([face[FaceMap.left_mouth_corner].x * w, face[FaceMap.left_mouth_corner].y * h, face[FaceMap.left_mouth_corner].z * w]) + right_mouth_corner = np.array([face[FaceMap.right_mouth_corner].x * w, face[FaceMap.right_mouth_corner].y * h, face[FaceMap.right_mouth_corner].z * w]) + + # 计算唇角的z值：可以取左右唇角的z值的平均值 + lip_corners_z = (left_mouth_corner[2] + right_mouth_corner[2]) / 2 # 计算左右唇角的z值的平均值 + + print(f"唇角的z值为: {lip_corners_z}") + + return uv + + +def update_text(color, result: HeadJoints): + y0 = 30 + dy = 30 + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = 0.7 + color_text = (0, 255, 255) + + # 眉毛 + r_brow_text = f"R_eyebrow out:{result.right_eyebrow_outside_y:.2f} in:{result.right_eyebrow_inside_y:.2f}" + l_brow_text = f"L_eyebrow out:{result.left_eyebrow_outside_y:.2f} in:{result.left_eyebrow_inside_y:.2f}" + cv2.putText(color, r_brow_text, (30, y0), font, font_scale, color_text, 2) + cv2.putText(color, l_brow_text, (30, y0 + dy), font, font_scale, color_text, 2) + y0 += 2*dy + + # 眼睛 + r_eye_text = f"R_eyelid U:{result.right_eye_upper_lid_y:.2f} L:{result.right_eye_lower_lid_y:.2f}" + l_eye_text = f"L_eyelid U:{result.left_eye_upper_lid_y:.2f} L:{result.left_eye_lower_lid_y:.2f}" + cv2.putText(color, r_eye_text, (30, y0), font, font_scale, color_text, 2) + cv2.putText(color, l_eye_text, (30, y0 + dy), font, font_scale, color_text, 2) + y0 += 2 * dy + + # ---- 眼球位置 ---- + l_eyeball_text = f"L_eyeBall X:{result.left_eye_ball_x:.2f} Y:{result.left_eye_ball_y:.2f}" + r_eyeball_text = f"R_eyeBall X:{result.right_eye_ball_x:.2f} Y:{result.right_eye_ball_y:.2f}" + cv2.putText(color, r_eyeball_text, (30, y0), font, font_scale, color_text, 2) + cv2.putText(color, l_eyeball_text, (30, y0 + dy), font, font_scale, color_text, 2) + y0 += 2*dy + + # ---- 嘴部中央上下 ---- + upper_text = f"UpperLip Y:{result.upper_lip_y:.2f} Z:{result.upper_lip_z:.2f}" + lower_text = f"LowerLip Y:{result.lower_lip_y:.2f} Z:{result.lower_lip_z:.2f}" + cv2.putText(color, upper_text, (30, y0), font, font_scale, color_text, 2) + y0 += dy + cv2.putText(color, lower_text, (30, y0), font, font_scale, color_text, 2) + y0 += dy + + # ---- 嘴部左右详细6点 ---- + mouth_points = [ + ("upper_left_lip", result.upper_left_lip_x, result.upper_left_lip_y), + ("upper_right_lip", result.upper_right_lip_x, result.upper_right_lip_y), + ("left_corner_lip", result.left_corner_lip_x, result.left_corner_lip_y,result.left_corner_lip_z), + ("right_corner_lip", result.right_corner_lip_x, result.right_corner_lip_y,result.right_corner_lip_z), + ("lower_left_lip", result.lower_left_lip_x, result.lower_left_lip_y), + ("lower_right_lip", result.lower_right_lip_x, result.lower_right_lip_y), + ] + + print(f"Left corner lip Z: {result.left_corner_lip_z}") + print(f"Right corner lip Z: {result.right_corner_lip_z}") + for name, x_val, y_val,z_val in mouth_points: + text = f"{name}: X:{x_val:.2f} Y:{y_val:.2f} Z:{z_val:.2f}" + cv2.putText(color, text, (30, y0), font, font_scale, color_text, 2) + y0 += dy + + + + # Jaw + jaw_text = f"Jaw X:{result.jaw_x:.2f} Y:{result.jaw_y:.2f}" + cv2.putText(color, jaw_text, (30, y0), font, font_scale, color_text, 2) + y0 += dy + + +def norm(result: HeadJoints, interval, clamp=True): + """ + Normalize result fields to 0-1 range based on interval spec. + + Args: + result: HeadJoints instance + interval: dict with key -> [min, max] + clamp: if True, limit outputs to [0,1] + + Returns: + dict of normalized values + """ + result_dict = result.to_dict() + normalized = HeadJoints() + for key, value in result_dict.items(): + if key in interval: + min_val, max_val = interval[key] + denom = max_val - min_val + 1e-6 + norm_val = (value - min_val) / denom + if clamp: + norm_val = max(0.0, min(1.0, norm_val)) + setattr(normalized, key, norm_val) + else: + setattr(normalized, key, value) + + return normalized + + + diff --git a/calibration.py b/calibration.py new file mode 100644 index 0000000..68e72da --- /dev/null +++ b/calibration.py @@ -0,0 +1,92 @@ +import cv2 +import yaml +import numpy as np +import mediapipe as mp +import pyrealsense2 as rs +from collections import deque +from biohead.algo import * +from biohead.utils import calc_feature + + + +def init_interval_dict(): + interval = {} + for key in HeadJoints().to_dict().keys(): + interval[key] = [np.inf, -np.inf] + return interval + + +def update_interval(interval, result): + for key, val in result.to_dict().items(): + if key in interval: + pre_min, pre_max = interval[key] + cur_min = min(getattr(result, key), pre_min) + cur_max = max(getattr(result, key), pre_max) + interval[key] = [cur_min, cur_max] + + +def save_interval_yaml(interval, filename="./config/calibrated_interval.yaml"): + # Wrap in AngleInterval + out = {} + for k, v in interval.items(): + out[k] = [float(v[0]), float(v[1])] + with open(filename, "w") as f: + yaml.dump(out, f) + + +def calibrate(): + with open(r"./config/config.yaml", "r") as f: + config = yaml.load(f, Loader=yaml.FullLoader) + + 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) + + 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']) + + interval = init_interval_dict() + 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: continue + + uv = calc_feature(color, res, ray_origins, ray_directions) + result = HeadJoints() + 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) + + update_interval(interval, result) + + cv2.imshow("Calibration", color) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + pipe.stop() + cv2.destroyAllWindows() + save_interval_yaml(interval) + + +if __name__ == '__main__': + calibrate() \ No newline at end of file diff --git a/config/calibrated_interval.yaml b/config/calibrated_interval.yaml new file mode 100644 index 0000000..d2b16bc --- /dev/null +++ b/config/calibrated_interval.yaml @@ -0,0 +1,102 @@ +jaw_x: +- 0.0004073124637932826 +- 0.005306197134562373 +jaw_y: +- 0.5868226041938256 +- 0.6081414962857482 +left_corner_lip_x: +- 0.15057437460015816 +- 0.17567584916429632 +left_corner_lip_y: +- 0.0 +- 0.14435270604405134 +left_corner_lip_z: +- -0.678306009954014 +- -0.617388735286828 +left_eye_ball_x: +- 0.1693536503233645 +- 0.2484767246157658 +left_eye_ball_y: +- -0.07811946172518947 +- -0.01969297662740576 +left_eye_lower_lid_y: +- 0.11538695815120105 +- 0.15497636563787587 +left_eye_upper_lid_y: +- 0.07101118461170748 +- 0.22619778420794312 +left_eyebrow_inside_y: +- 0.570284044687832 +- 0.6265691053443071 +left_eyebrow_outside_y: +- 0.5735039144619316 +- 0.6294781590411128 +left_nose_y: +- 0.0 +- 0.0 +lower_left_lip_x: +- 0.1034313574760133 +- 0.11993661202538933 +lower_left_lip_y: +- 0.19336114715522087 +- 0.241912554324055 +lower_lip_y: +- 0.16533659997368738 +- 0.2250915095502857 +lower_lip_z: +- -1.0241197800523982 +- -0.9003193279990209 +lower_right_lip_x: +- -0.08522635824436955 +- -0.06554386160122612 +lower_right_lip_y: +- 0.19510216258981433 +- 0.23805688117998655 +right_corner_lip_x: +- -0.2039567893396664 +- -0.14047775093846088 +right_corner_lip_y: +- 0.0 +- 0.14435270604405134 +right_corner_lip_z: +- -0.6037151740280395 +- -0.5311408274540772 +right_eye_ball_x: +- -0.1410040894476193 +- -0.0847869174583541 +right_eye_ball_y: +- -0.008475643149547086 +- 0.09480833978653566 +right_eye_lower_lid_y: +- 0.11856081542899619 +- 0.16616325146582991 +right_eye_upper_lid_y: +- 0.10740131157788248 +- 0.27411944580061903 +right_eyebrow_inside_y: +- 0.5944020384385427 +- 0.684570847315001 +right_eyebrow_outside_y: +- 0.5579769779708734 +- 0.6230707915475744 +right_nose_y: +- 0.0 +- 0.0 +upper_left_lip_x: +- 0.0886363719078837 +- 0.12721476980232435 +upper_left_lip_y: +- 0.0 +- 0.0 +upper_lip_y: +- -0.38298305315411324 +- -0.3478934779141343 +upper_lip_z: +- -1.1761244454421937 +- -1.0962268213421478 +upper_right_lip_x: +- -0.12843447547289463 +- -0.0732961299064583 +upper_right_lip_y: +- 0.0 +- 0.0 diff --git a/config/config.yaml b/config/config.yaml new file mode 100644 index 0000000..b17fe62 --- /dev/null +++ b/config/config.yaml @@ -0,0 +1,54 @@ +Camera: + image_width: 848 + image_height: 480 + fps: 30 + +MediaPipe: + min_detection_confidence: 0.5 + min_tracking_confidence: 0.5 + +Smooth: 8 + +AngleInterval: + # Eyebrow + left_eyebrow_outside_y: [0.5, 0.7] + left_eyebrow_inside_y: [0.4, 0.9] + right_eyebrow_outside_y: [0.5, 0.7] + right_eyebrow_inside_y: [0.4, 0.9] + + # Eyelid + left_eye_upper_lid_y: [0.0, 0.25] + left_eye_lower_lid_y: [0.0, 0.13] + right_eye_upper_lid_y: [0.0, 0.25] + right_eye_lower_lid_y: [0.0, 0.13] + + # Eyeball + left_eye_ball_x: [0.0, 1.0] + left_eye_ball_y: [0.0, 1.0] + right_eye_ball_x: [0.0, 1.0] + right_eye_ball_y: [0.0, 1.0] + + # Mouth + upper_lip_y: [0.0, 1.0] + upper_lip_z: [0.0, 1.0] + lower_lip_y: [0.0, 1.0] + lower_lip_z: [0.0, 1.0] + + upper_left_lip_x: [0.0, 1.0] + upper_left_lip_y: [-0.25, -0.2] + left_corner_lip_x: [0.0, 1.0] + left_corner_lip_y: [-0.28, -0.2] + lower_left_lip_x: [0.0, 1.0] + lower_left_lip_y: [-0.33, -0.28] + + upper_right_lip_x: [0.0, 1.0] + upper_right_lip_y: [-0.25, -0.2] + right_corner_lip_x: [0.0, 1.0] + right_corner_lip_y: [-0.28, -0.2] + lower_right_lip_x: [0.0, 1.0] + lower_right_lip_y: [-0.33, -0.28] + + # Jaw + jaw_x: [0.0, 1.0] + jaw_y: [0.1, 0.3] + diff --git a/demo.py b/demo.py new file mode 100644 index 0000000..b55e204 --- /dev/null +++ b/demo.py @@ -0,0 +1,82 @@ +import cv2 +import yaml +import numpy as np +import mediapipe as mp +import pyrealsense2 as rs +from collections import deque + +from biohead.algo import * +from biohead.utils import calc_feature +from biohead.utils import update_text +from biohead.utils import norm + + +def main(calib_file): + 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) + + 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) + + 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() + + 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: 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) + + update_text(color, result) + cv2.imshow("RGB", color) + + # canvas = np.zeros((400, 400, 3), dtype=np.uint8) + # arr = np.array(list(uv.values())) + # if len(arr) > 0: + # umin, vmin = arr.min(0) + # umax, vmax = arr.max(0) + # for (u, v) in arr: + # x2 = int((u - umin) / (umax - umin + 1e-6) * 360 + 20) + # y2 = int((vmax - v) / (vmax - vmin + 1e-6) * 360 + 20) + # cv2.circle(canvas, (x2, y2), 3, (0, 255, 0), -1) + # cv2.imshow("Projected Plane", canvas) + + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + pipe.stop() + cv2.destroyAllWindows() + + +if __name__ == '__main__': + main("./config/calibrated_interval.yaml") diff --git a/generated/cmvr/api/biohead_command_pb2.py b/generated/cmvr/api/biohead_command_pb2.py new file mode 100644 index 0000000..9462a4b --- /dev/null +++ b/generated/cmvr/api/biohead_command_pb2.py @@ -0,0 +1,67 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: cmvr/api/biohead_command.proto +# Protobuf Python Version: 4.25.1 +"""Generated protocol buffer code.""" +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +from google.protobuf.internal import builder as _builder +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + +from cmvr.api import common_pb2 as cmvr_dot_api_dot_common__pb2 + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x1e\x63mvr/api/biohead_command.proto\x12\x08\x63mvr.api\x1a\x15\x63mvr/api/common.proto\"\xdd\x08\n\x10\x46\x61\x63ialExpression\x12\x33\n\x07\x65yebrow\x18\x03 \x01(\x0b\x32\".cmvr.api.FacialExpression.Eyebrow\x12\x31\n\x06\x65yelid\x18\x04 \x01(\x0b\x32!.cmvr.api.FacialExpression.Eyelid\x12\x33\n\x07\x65yeball\x18\x05 \x01(\x0b\x32\".cmvr.api.FacialExpression.Eyeball\x12-\n\x04nose\x18\x06 \x01(\x0b\x32\x1f.cmvr.api.FacialExpression.Nose\x12/\n\x05mouth\x18\x07 \x01(\x0b\x32 .cmvr.api.FacialExpression.Mouth\x12+\n\x03jaw\x18\x08 \x01(\x0b\x32\x1e.cmvr.api.FacialExpression.Jaw\x1ai\n\x07\x45yebrow\x12\x16\n\x0eleft_outside_y\x18\x01 \x01(\x02\x12\x15\n\rleft_inside_y\x18\x02 \x01(\x02\x12\x17\n\x0fright_outside_y\x18\x03 \x01(\x02\x12\x16\n\x0eright_inside_y\x18\x04 \x01(\x02\x1a\x62\n\x06\x45yelid\x12\x14\n\x0cleft_upper_y\x18\x01 \x01(\x02\x12\x14\n\x0cleft_lower_y\x18\x02 \x01(\x02\x12\x15\n\rright_upper_y\x18\x03 \x01(\x02\x12\x15\n\rright_lower_y\x18\x04 \x01(\x02\x1aK\n\x07\x45yeball\x12\x0e\n\x06left_x\x18\x01 \x01(\x02\x12\x0e\n\x06left_y\x18\x02 \x01(\x02\x12\x0f\n\x07right_x\x18\x03 \x01(\x02\x12\x0f\n\x07right_y\x18\x04 \x01(\x02\x1a\'\n\x04Nose\x12\x0e\n\x06left_y\x18\x01 \x01(\x02\x12\x0f\n\x07right_y\x18\x02 \x01(\x02\x1a\xbc\x03\n\x05Mouth\x12\x13\n\x0bupper_lip_y\x18\x01 \x01(\x02\x12\x13\n\x0bupper_lip_z\x18\x02 \x01(\x02\x12\x13\n\x0blower_lip_y\x18\x03 \x01(\x02\x12\x13\n\x0blower_lip_z\x18\x04 \x01(\x02\x12:\n\x08left_lip\x18\x05 \x01(\x0b\x32(.cmvr.api.FacialExpression.Mouth.LeftLip\x12<\n\tright_lip\x18\x06 \x01(\x0b\x32).cmvr.api.FacialExpression.Mouth.RightLip\x1aq\n\x07LeftLip\x12\x0f\n\x07upper_x\x18\x01 \x01(\x02\x12\x0f\n\x07upper_y\x18\x02 \x01(\x02\x12\x10\n\x08\x63orner_x\x18\x03 \x01(\x02\x12\x10\n\x08\x63orner_y\x18\x04 \x01(\x02\x12\x0f\n\x07lower_x\x18\x05 \x01(\x02\x12\x0f\n\x07lower_y\x18\x06 \x01(\x02\x1ar\n\x08RightLip\x12\x0f\n\x07upper_x\x18\x01 \x01(\x02\x12\x0f\n\x07upper_y\x18\x02 \x01(\x02\x12\x10\n\x08\x63orner_x\x18\x03 \x01(\x02\x12\x10\n\x08\x63orner_y\x18\x04 \x01(\x02\x12\x0f\n\x07lower_x\x18\x05 \x01(\x02\x12\x0f\n\x07lower_y\x18\x06 \x01(\x02\x1a\x1b\n\x03Jaw\x12\t\n\x01x\x18\x01 \x01(\x02\x12\t\n\x01y\x18\x02 \x01(\x02\"\xf0\x01\n\x13SetFacialExpression\x1aj\n\x07Request\x12/\n\x06header\x18\x01 \x01(\x0b\x32\x1f.cmvr.api.CommandHeader.Request\x12.\n\nexpression\x18\x02 \x01(\x0b\x32\x1a.cmvr.api.FacialExpression\x1am\n\x08\x46\x65\x65\x64\x62\x61\x63k\x12\x30\n\x06header\x18\x01 \x01(\x0b\x32 .cmvr.api.CommandHeader.Feedback\x12\x14\n\x0c\x65xecution_id\x18\x02 \x01(\t\x12\x19\n\x11\x65xecution_time_ms\x18\x03 \x01(\x02\"\xf8\x01\n\x16StreamFacialExpression\x1aq\n\x07Request\x12/\n\x06header\x18\x01 \x01(\x0b\x32\x1f.cmvr.api.CommandHeader.Request\x12(\n\x04\x65xpr\x18\x02 \x01(\x0b\x32\x1a.cmvr.api.FacialExpression\x12\x0b\n\x03\x65of\x18\x03 \x01(\x08\x1ak\n\x08\x46\x65\x65\x64\x62\x61\x63k\x12\x30\n\x06header\x18\x01 \x01(\x0b\x32 .cmvr.api.CommandHeader.Feedback\x12-\n\texpr_diff\x18\x02 \x01(\x0b\x32\x1a.cmvr.api.FacialExpression\"\x84\x02\n\tGetStatus\x1a:\n\x07Request\x12/\n\x06header\x18\x01 \x01(\x0b\x32\x1f.cmvr.api.CommandHeader.Request\x1a\xba\x01\n\x08\x46\x65\x65\x64\x62\x61\x63k\x12\x30\n\x06header\x18\x01 \x01(\x0b\x32 .cmvr.api.CommandHeader.Feedback\x12\x11\n\tis_moving\x18\x02 \x01(\x08\x12\x17\n\x0flast_request_id\x18\x03 \x01(\t\x12\x19\n\x11\x63urrent_positions\x18\x04 \x03(\x02\x12\x18\n\x10\x63\x61mera_recording\x18\x05 \x01(\x08\x12\x1b\n\x13\x61\x63tive_recording_id\x18\x06 \x01(\t\"\xa4\x01\n\rEmergencyStop\x1a:\n\x07Request\x12/\n\x06header\x18\x01 \x01(\x0b\x32\x1f.cmvr.api.CommandHeader.Request\x1aW\n\x08\x46\x65\x65\x64\x62\x61\x63k\x12\x30\n\x06header\x18\x01 \x01(\x0b\x32 .cmvr.api.CommandHeader.Feedback\x12\x19\n\x11stopped_processes\x18\x02 \x01(\tb\x06proto3') + +_globals = globals() +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'cmvr.api.biohead_command_pb2', _globals) +if _descriptor._USE_C_DESCRIPTORS == False: + DESCRIPTOR._options = None + _globals['_FACIALEXPRESSION']._serialized_start=68 + _globals['_FACIALEXPRESSION']._serialized_end=1185 + _globals['_FACIALEXPRESSION_EYEBROW']._serialized_start=386 + _globals['_FACIALEXPRESSION_EYEBROW']._serialized_end=491 + _globals['_FACIALEXPRESSION_EYELID']._serialized_start=493 + _globals['_FACIALEXPRESSION_EYELID']._serialized_end=591 + _globals['_FACIALEXPRESSION_EYEBALL']._serialized_start=593 + _globals['_FACIALEXPRESSION_EYEBALL']._serialized_end=668 + _globals['_FACIALEXPRESSION_NOSE']._serialized_start=670 + _globals['_FACIALEXPRESSION_NOSE']._serialized_end=709 + _globals['_FACIALEXPRESSION_MOUTH']._serialized_start=712 + _globals['_FACIALEXPRESSION_MOUTH']._serialized_end=1156 + _globals['_FACIALEXPRESSION_MOUTH_LEFTLIP']._serialized_start=927 + _globals['_FACIALEXPRESSION_MOUTH_LEFTLIP']._serialized_end=1040 + _globals['_FACIALEXPRESSION_MOUTH_RIGHTLIP']._serialized_start=1042 + _globals['_FACIALEXPRESSION_MOUTH_RIGHTLIP']._serialized_end=1156 + _globals['_FACIALEXPRESSION_JAW']._serialized_start=1158 + _globals['_FACIALEXPRESSION_JAW']._serialized_end=1185 + _globals['_SETFACIALEXPRESSION']._serialized_start=1188 + _globals['_SETFACIALEXPRESSION']._serialized_end=1428 + _globals['_SETFACIALEXPRESSION_REQUEST']._serialized_start=1211 + _globals['_SETFACIALEXPRESSION_REQUEST']._serialized_end=1317 + _globals['_SETFACIALEXPRESSION_FEEDBACK']._serialized_start=1319 + _globals['_SETFACIALEXPRESSION_FEEDBACK']._serialized_end=1428 + _globals['_STREAMFACIALEXPRESSION']._serialized_start=1431 + _globals['_STREAMFACIALEXPRESSION']._serialized_end=1679 + _globals['_STREAMFACIALEXPRESSION_REQUEST']._serialized_start=1457 + _globals['_STREAMFACIALEXPRESSION_REQUEST']._serialized_end=1570 + _globals['_STREAMFACIALEXPRESSION_FEEDBACK']._serialized_start=1572 + _globals['_STREAMFACIALEXPRESSION_FEEDBACK']._serialized_end=1679 + _globals['_GETSTATUS']._serialized_start=1682 + _globals['_GETSTATUS']._serialized_end=1942 + _globals['_GETSTATUS_REQUEST']._serialized_start=1211 + _globals['_GETSTATUS_REQUEST']._serialized_end=1269 + _globals['_GETSTATUS_FEEDBACK']._serialized_start=1756 + _globals['_GETSTATUS_FEEDBACK']._serialized_end=1942 + _globals['_EMERGENCYSTOP']._serialized_start=1945 + _globals['_EMERGENCYSTOP']._serialized_end=2109 + _globals['_EMERGENCYSTOP_REQUEST']._serialized_start=1211 + _globals['_EMERGENCYSTOP_REQUEST']._serialized_end=1269 + _globals['_EMERGENCYSTOP_FEEDBACK']._serialized_start=2022 + _globals['_EMERGENCYSTOP_FEEDBACK']._serialized_end=2109 +# @@protoc_insertion_point(module_scope) diff --git a/generated/cmvr/api/biohead_command_pb2_grpc.py b/generated/cmvr/api/biohead_command_pb2_grpc.py new file mode 100644 index 0000000..2daafff --- /dev/null +++ b/generated/cmvr/api/biohead_command_pb2_grpc.py @@ -0,0 +1,4 @@ +# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! +"""Client and server classes corresponding to protobuf-defined services.""" +import grpc + diff --git a/generated/cmvr/api/biohead_service_pb2.py b/generated/cmvr/api/biohead_service_pb2.py new file mode 100644 index 0000000..973acd4 --- /dev/null +++ b/generated/cmvr/api/biohead_service_pb2.py @@ -0,0 +1,27 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: cmvr/api/biohead_service.proto +# Protobuf Python Version: 4.25.1 +"""Generated protocol buffer code.""" +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +from google.protobuf.internal import builder as _builder +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + +from cmvr.api import biohead_command_pb2 as cmvr_dot_api_dot_biohead__command__pb2 + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x1e\x63mvr/api/biohead_service.proto\x12\x08\x63mvr.api\x1a\x1e\x63mvr/api/biohead_command.proto2\x87\x03\n\x0e\x42ioHeadService\x12`\n\rSetExpression\x12%.cmvr.api.SetFacialExpression.Request\x1a&.cmvr.api.SetFacialExpression.Feedback\"\x00\x12m\n\x10StreamExpression\x12(.cmvr.api.StreamFacialExpression.Request\x1a).cmvr.api.StreamFacialExpression.Feedback\"\x00(\x01\x30\x01\x12N\n\x0fGetSystemStatus\x12\x1b.cmvr.api.GetStatus.Request\x1a\x1c.cmvr.api.GetStatus.Feedback\"\x00\x12T\n\rEmergencyStop\x12\x1f.cmvr.api.EmergencyStop.Request\x1a .cmvr.api.EmergencyStop.Feedback\"\x00\x62\x06proto3') + +_globals = globals() +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'cmvr.api.biohead_service_pb2', _globals) +if _descriptor._USE_C_DESCRIPTORS == False: + DESCRIPTOR._options = None + _globals['_BIOHEADSERVICE']._serialized_start=77 + _globals['_BIOHEADSERVICE']._serialized_end=468 +# @@protoc_insertion_point(module_scope) diff --git a/generated/cmvr/api/biohead_service_pb2_grpc.py b/generated/cmvr/api/biohead_service_pb2_grpc.py new file mode 100644 index 0000000..aa45fe0 --- /dev/null +++ b/generated/cmvr/api/biohead_service_pb2_grpc.py @@ -0,0 +1,174 @@ +# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! +"""Client and server classes corresponding to protobuf-defined services.""" +import grpc + +from cmvr.api import biohead_command_pb2 as cmvr_dot_api_dot_biohead__command__pb2 + + +class BioHeadServiceStub(object): + """生物头部机器人服务接口 + """ + + def __init__(self, channel): + """Constructor. + + Args: + channel: A grpc.Channel. + """ + self.SetExpression = channel.unary_unary( + '/cmvr.api.BioHeadService/SetExpression', + request_serializer=cmvr_dot_api_dot_biohead__command__pb2.SetFacialExpression.Request.SerializeToString, + response_deserializer=cmvr_dot_api_dot_biohead__command__pb2.SetFacialExpression.Feedback.FromString, + ) + self.StreamExpression = channel.stream_stream( + '/cmvr.api.BioHeadService/StreamExpression', + request_serializer=cmvr_dot_api_dot_biohead__command__pb2.StreamFacialExpression.Request.SerializeToString, + response_deserializer=cmvr_dot_api_dot_biohead__command__pb2.StreamFacialExpression.Feedback.FromString, + ) + self.GetSystemStatus = channel.unary_unary( + '/cmvr.api.BioHeadService/GetSystemStatus', + request_serializer=cmvr_dot_api_dot_biohead__command__pb2.GetStatus.Request.SerializeToString, + response_deserializer=cmvr_dot_api_dot_biohead__command__pb2.GetStatus.Feedback.FromString, + ) + self.EmergencyStop = channel.unary_unary( + '/cmvr.api.BioHeadService/EmergencyStop', + request_serializer=cmvr_dot_api_dot_biohead__command__pb2.EmergencyStop.Request.SerializeToString, + response_deserializer=cmvr_dot_api_dot_biohead__command__pb2.EmergencyStop.Feedback.FromString, + ) + + +class BioHeadServiceServicer(object): + """生物头部机器人服务接口 + """ + + def SetExpression(self, request, context): + """设置面部表情 + """ + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + def StreamExpression(self, request_iterator, context): + """流式表情控制 + rpc StreamExpression(StreamFacialExpression.Request) returns (StreamFacialExpression.Feedback){}; + + """ + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + def GetSystemStatus(self, request, context): + """获取状态 + """ + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + def EmergencyStop(self, request, context): + """紧急停止 + """ + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + +def add_BioHeadServiceServicer_to_server(servicer, server): + rpc_method_handlers = { + 'SetExpression': grpc.unary_unary_rpc_method_handler( + servicer.SetExpression, + request_deserializer=cmvr_dot_api_dot_biohead__command__pb2.SetFacialExpression.Request.FromString, + response_serializer=cmvr_dot_api_dot_biohead__command__pb2.SetFacialExpression.Feedback.SerializeToString, + ), + 'StreamExpression': grpc.stream_stream_rpc_method_handler( + servicer.StreamExpression, + request_deserializer=cmvr_dot_api_dot_biohead__command__pb2.StreamFacialExpression.Request.FromString, + response_serializer=cmvr_dot_api_dot_biohead__command__pb2.StreamFacialExpression.Feedback.SerializeToString, + ), + 'GetSystemStatus': grpc.unary_unary_rpc_method_handler( + servicer.GetSystemStatus, + request_deserializer=cmvr_dot_api_dot_biohead__command__pb2.GetStatus.Request.FromString, + response_serializer=cmvr_dot_api_dot_biohead__command__pb2.GetStatus.Feedback.SerializeToString, + ), + 'EmergencyStop': grpc.unary_unary_rpc_method_handler( + servicer.EmergencyStop, + request_deserializer=cmvr_dot_api_dot_biohead__command__pb2.EmergencyStop.Request.FromString, + response_serializer=cmvr_dot_api_dot_biohead__command__pb2.EmergencyStop.Feedback.SerializeToString, + ), + } + generic_handler = grpc.method_handlers_generic_handler( + 'cmvr.api.BioHeadService', rpc_method_handlers) + server.add_generic_rpc_handlers((generic_handler,)) + + + # This class is part of an EXPERIMENTAL API. +class BioHeadService(object): + """生物头部机器人服务接口 + """ + + @staticmethod + def SetExpression(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/cmvr.api.BioHeadService/SetExpression', + cmvr_dot_api_dot_biohead__command__pb2.SetFacialExpression.Request.SerializeToString, + cmvr_dot_api_dot_biohead__command__pb2.SetFacialExpression.Feedback.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) + + @staticmethod + def StreamExpression(request_iterator, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.stream_stream(request_iterator, target, '/cmvr.api.BioHeadService/StreamExpression', + cmvr_dot_api_dot_biohead__command__pb2.StreamFacialExpression.Request.SerializeToString, + cmvr_dot_api_dot_biohead__command__pb2.StreamFacialExpression.Feedback.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) + + @staticmethod + def GetSystemStatus(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/cmvr.api.BioHeadService/GetSystemStatus', + cmvr_dot_api_dot_biohead__command__pb2.GetStatus.Request.SerializeToString, + cmvr_dot_api_dot_biohead__command__pb2.GetStatus.Feedback.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) + + @staticmethod + def EmergencyStop(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_unary(request, target, '/cmvr.api.BioHeadService/EmergencyStop', + cmvr_dot_api_dot_biohead__command__pb2.EmergencyStop.Request.SerializeToString, + cmvr_dot_api_dot_biohead__command__pb2.EmergencyStop.Feedback.FromString, + options, channel_credentials, + insecure, call_credentials, compression, wait_for_ready, timeout, metadata) diff --git a/generated/cmvr/api/common_pb2.py b/generated/cmvr/api/common_pb2.py new file mode 100644 index 0000000..2357a5f --- /dev/null +++ b/generated/cmvr/api/common_pb2.py @@ -0,0 +1,35 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: cmvr/api/common.proto +# Protobuf Python Version: 4.25.1 +"""Generated protocol buffer code.""" +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import symbol_database as _symbol_database +from google.protobuf.internal import builder as _builder +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + +from google.protobuf import timestamp_pb2 as google_dot_protobuf_dot_timestamp__pb2 + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x15\x63mvr/api/common.proto\x12\x08\x63mvr.api\x1a\x1fgoogle/protobuf/timestamp.proto\"\xb8\x01\n\x0f\x44\x65viceLifecycle\x12\x32\n\x05state\x18\x01 \x01(\x0e\x32#.cmvr.api.DeviceLifecycle.Lifecycle\"q\n\tLifecycle\x12\x0e\n\nSTATE_INIT\x10\x00\x12\x0f\n\x0bSTATE_READY\x10\x01\x12\x11\n\rSTATE_RUNNING\x10\x02\x12\x0f\n\x0bSTATE_ERROR\x10\x03\x12\x0f\n\x0bSTATE_ESTOP\x10\x04\x12\x0e\n\nSTATE_STOP\x10\x05\"\xbf\x01\n\rCommandHeader\x1aK\n\x07Request\x12\x11\n\tdevice_id\x18\x01 \x01(\t\x12-\n\ttimestamp\x18\x02 \x01(\x0b\x32\x1a.google.protobuf.Timestamp\x1a\x61\n\x08\x46\x65\x65\x64\x62\x61\x63k\x12\x0f\n\x07success\x18\x01 \x01(\x08\x12\x15\n\rerror_message\x18\x02 \x01(\t\x12-\n\ttimestamp\x18\x03 \x01(\x0b\x32\x1a.google.protobuf.Timestampb\x06proto3') + +_globals = globals() +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'cmvr.api.common_pb2', _globals) +if _descriptor._USE_C_DESCRIPTORS == False: + DESCRIPTOR._options = None + _globals['_DEVICELIFECYCLE']._serialized_start=69 + _globals['_DEVICELIFECYCLE']._serialized_end=253 + _globals['_DEVICELIFECYCLE_LIFECYCLE']._serialized_start=140 + _globals['_DEVICELIFECYCLE_LIFECYCLE']._serialized_end=253 + _globals['_COMMANDHEADER']._serialized_start=256 + _globals['_COMMANDHEADER']._serialized_end=447 + _globals['_COMMANDHEADER_REQUEST']._serialized_start=273 + _globals['_COMMANDHEADER_REQUEST']._serialized_end=348 + _globals['_COMMANDHEADER_FEEDBACK']._serialized_start=350 + _globals['_COMMANDHEADER_FEEDBACK']._serialized_end=447 +# @@protoc_insertion_point(module_scope) diff --git a/generated/cmvr/api/common_pb2_grpc.py b/generated/cmvr/api/common_pb2_grpc.py new file mode 100644 index 0000000..2daafff --- /dev/null +++ b/generated/cmvr/api/common_pb2_grpc.py @@ -0,0 +1,4 @@ +# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! +"""Client and server classes corresponding to protobuf-defined services.""" +import grpc + diff --git a/grpc_cli.py b/grpc_cli.py new file mode 100644 index 0000000..45bf172 --- /dev/null +++ b/grpc_cli.py @@ -0,0 +1,388 @@ +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) +# +# diff --git a/grpc_lb.py b/grpc_lb.py new file mode 100644 index 0000000..02f6b0b --- /dev/null +++ b/grpc_lb.py @@ -0,0 +1,306 @@ +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) + + + + + diff --git a/protos/cmvr/api/biohead_command.proto b/protos/cmvr/api/biohead_command.proto new file mode 100644 index 0000000..888730a --- /dev/null +++ b/protos/cmvr/api/biohead_command.proto @@ -0,0 +1,175 @@ +syntax = "proto3"; + +import "cmvr/api/common.proto"; // 导入通用命令头定义，确保与其他服务接口一致 + +package cmvr.api; + +/** + * 面部表情控制参数定义 + * 包含生物头部机器人面部各部位的运动参数，用于精确控制面部表情 + */ +message FacialExpression { + /** + * 眉毛控制参数 + * 左右眉毛的内外侧垂直位置（0.0-1.0标准化值） + */ + message Eyebrow { + float left_outside_y = 1; // 左眉外侧垂直位置 + float left_inside_y = 2; // 左眉内侧垂直位置 + float right_outside_y = 3; // 右眉外侧垂直位置 + float right_inside_y = 4; // 右眉内侧垂直位置 + } + Eyebrow eyebrow = 3; // 眉毛参数封装 + + /** + * 眼睑控制参数 + * 左右眼睑的上下位置（0.0-1.0标准化值） + */ + message Eyelid { + float left_upper_y = 1; // 左上眼睑垂直位置 + float left_lower_y = 2; // 左下眼睑垂直位置 + float right_upper_y = 3; // 右上眼睑垂直位置 + float right_lower_y = 4; // 右下眼睑垂直位置 + } + Eyelid eyelid = 4; // 眼睑参数封装 + + /** + * 眼球控制参数 + * 左右眼球的二维坐标（-1.0到1.0标准化值，中心为0） + */ + message Eyeball { + float left_x = 1; // 左眼球水平位置 + float left_y = 2; // 左眼球垂直位置 + float right_x = 3; // 右眼球水平位置 + float right_y = 4; // 右眼球垂直位置 + } + Eyeball eyeball = 5; // 眼球参数封装 + + /** + * 鼻子控制参数 + * 鼻子左右侧的垂直位置（0.0-1.0标准化值） + */ + message Nose { + float left_y = 1; // 鼻子左侧垂直位置 + float right_y = 2; // 鼻子右侧垂直位置 + } + Nose nose = 6; // 鼻子参数封装 + + /** + * 嘴巴控制参数 + * 包含嘴唇整体位置和左右唇角细节 + */ + message Mouth { + float upper_lip_y = 1; // 上唇垂直位置 + float upper_lip_z = 2; // 上唇前后位置（Z轴） + float lower_lip_y = 3; // 下唇垂直位置 + float lower_lip_z = 4; // 下唇前后位置（Z轴） + + /** + * 左唇角控制参数 + * 包含上唇、唇角、下唇的坐标点 + */ + message LeftLip { + float upper_x = 1; // 左上唇水平位置 + float upper_y = 2; // 左上唇垂直位置 + float corner_x = 3; // 左唇角水平位置 + float corner_y = 4; // 左唇角垂直位置 + float lower_x = 5; // 左下唇水平位置 + float lower_y = 6; // 左下唇垂直位置 + } + LeftLip left_lip = 5; // 左唇角参数封装 + + /** + * 右唇角控制参数 + * 结构与左唇角对称 + */ + message RightLip { + float upper_x = 1; // 右上唇水平位置 + float upper_y = 2; // 右上唇垂直位置 + float corner_x = 3; // 右唇角水平位置 + float corner_y = 4; // 右唇角垂直位置 + float lower_x = 5; // 右下唇水平位置 + float lower_y = 6; // 右下唇垂直位置 + } + RightLip right_lip = 6; // 右唇角参数封装 + } + Mouth mouth = 7; // 嘴巴参数封装 + + /** + * 下巴控制参数 + * 下巴的二维移动坐标（X/Y轴） + */ + message Jaw { + float x = 1; // 下巴水平位置 + float y = 2; // 下巴垂直位置 + } + Jaw jaw = 8; // 下巴参数封装 +} + +/** + * 设置面部表情命令（集成通用命令头） + * 用于单次设置生物头部的面部表情 + */ +message SetFacialExpression { + message Request { + CommandHeader.Request header = 1; // 通用命令头，包含设备ID和时间戳 + FacialExpression expression = 2; // 具体面部表情参数 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头，包含操作状态和时间戳 + string execution_id = 2; // 表情执行任务ID + float execution_time_ms = 3; // 表情执行耗时（毫秒） + } +} + +/** + * 流式面部表情命令（集成通用命令头） + * 用于连续发送多个面部表情，实现表情动画效果 + */ +message StreamFacialExpression { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + FacialExpression expr = 2; // 执行 + bool eof = 3; // 标记是否为流结束 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + FacialExpression expr_diff = 2; // 执行误差 + } +} + +/** + * 获取生物头部状态命令（集成通用命令头） + * 用于查询当前头部各部位的状态和位置 + */ +message GetStatus { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + bool is_moving = 2; // 是否有部位在移动 + string last_request_id = 3; // 上一次请求的ID + repeated float current_positions = 4; // 当前各部位位置参数 + bool camera_recording = 5; // 相机是否在录制 + string active_recording_id = 6; // 活跃录制任务的ID + } +} + +/** + * 紧急停止命令（集成通用命令头） + * 用于立即停止所有面部动作 + */ +message EmergencyStop { + message Request { + CommandHeader.Request header = 1; // 通用命令头 + } + + message Feedback { + CommandHeader.Feedback header = 1; // 通用反馈头 + string stopped_processes = 2; // 已停止的进程列表 + } +} \ No newline at end of file diff --git a/protos/cmvr/api/biohead_service.proto b/protos/cmvr/api/biohead_service.proto new file mode 100644 index 0000000..ea1e029 --- /dev/null +++ b/protos/cmvr/api/biohead_service.proto @@ -0,0 +1,22 @@ +syntax = "proto3"; + +package cmvr.api; +import "cmvr/api/biohead_command.proto"; + +// 生物头部机器人服务接口 +service BioHeadService { + // 设置面部表情 + rpc SetExpression(SetFacialExpression.Request) returns (SetFacialExpression.Feedback){}; + + // 流式表情控制 + //rpc StreamExpression(StreamFacialExpression.Request) returns (StreamFacialExpression.Feedback){}; + + rpc StreamExpression (stream StreamFacialExpression.Request) returns (stream StreamFacialExpression.Feedback){}; + + + // 获取状态 + rpc GetSystemStatus(GetStatus.Request) returns (GetStatus.Feedback){}; + + // 紧急停止 + rpc EmergencyStop(EmergencyStop.Request) returns (EmergencyStop.Feedback){}; +} \ No newline at end of file diff --git a/protos/cmvr/api/common.proto b/protos/cmvr/api/common.proto new file mode 100644 index 0000000..19c35bd --- /dev/null +++ b/protos/cmvr/api/common.proto @@ -0,0 +1,30 @@ +syntax = "proto3"; + +package cmvr.api; + +import "google/protobuf/timestamp.proto"; + +message DeviceLifecycle { + enum Lifecycle { + STATE_INIT = 0; + STATE_READY = 1; + STATE_RUNNING = 2; + STATE_ERROR = 3; + STATE_ESTOP = 4; + STATE_STOP = 5; + } + Lifecycle state = 1; +} + +message CommandHeader { + message Request { + string device_id = 1; // 目标设备名称 + google.protobuf.Timestamp timestamp = 2; // 请求时间 + } + + message Feedback { + bool success = 1; // 是否成功 + string error_message = 2; // 错误信息（成功时为空） + google.protobuf.Timestamp timestamp = 3; // 回复时间 + } +} diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..c6638fd --- /dev/null +++ b/requirements.txt @@ -0,0 +1,69 @@ +pyyaml +pyrealsense2 +mediapipe +opencv-python +numpy +tqdm +protobuf==4.25.3 +grpcio==1.73.1 +grpcio-tools==1.62.3 + + + +(biohead) tankaitao@cmvr:~/cmvr-biohead$ python grpc_cli.py +2025-07-14 15:19:52,078 - biohead_client - INFO - 配置文件加载完成 +INFO:biohead_client:配置文件加载完成 +2025-07-14 15:19:52,317 - biohead_client - INFO - 相机初始化完成 +INFO:biohead_client:相机初始化完成 +libEGL warning: MESA-LOADER: failed to open iris: /usr/lib/dri/iris_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open iris: /usr/lib/dri/iris_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open swrast: /usr/lib/dri/swrast_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open iris: /usr/lib/dri/iris_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open iris: /usr/lib/dri/iris_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open swrast: /usr/lib/dri/swrast_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open iris: /usr/lib/dri/iris_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open iris: /usr/lib/dri/iris_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +libEGL warning: MESA-LOADER: failed to open swrast: /usr/lib/dri/swrast_dri.so: cannot open shared object file: No such file or directory (search paths /usr/lib/x86_64-linux-gnu/dri:\$${ORIGIN}/dri:/usr/lib/dri, suffix _dri) + +2025-07-14 15:19:52,391 - biohead_client - INFO - MediaPipe初始化完成 +INFO:biohead_client:MediaPipe初始化完成 +INFO: Created TensorFlow Lite XNNPACK delegate for CPU. +WARNING: All log messages before absl::InitializeLog() is called are written to STDERR +W0000 00:00:1752477592.394130 763313 inference_feedback_manager.cc:114] Feedback manager requires a model with a single signature inference. Disabling support for feedback tensors. +2025-07-14 15:19:52,395 - biohead_client - INFO - gRPC连接建立 +INFO:biohead_client:gRPC连接建立 +2025-07-14 15:19:52,395 - biohead_client - INFO - BioHead客户端启动 +INFO:biohead_client:BioHead客户端启动 +2025-07-14 15:19:52,395 - biohead_client - INFO - 流式传输线程启动 +INFO:biohead_client:流式传输线程启动 +W0000 00:00:1752477592.422326 763314 inference_feedback_manager.cc:114] Feedback manager requires a model with a single signature inference. Disabling support for feedback tensors. +W0000 00:00:1752477592.801464 763316 landmark_projection_calculator.cc:186] Using NORM_RECT without IMAGE_DIMENSIONS is only supported for the square ROI. Provide IMAGE_DIMENSIONS or use PROJECTION_MATRIX. +2025-07-14 15:19:52,803 - biohead_client - ERROR - 主循环错误: Protocol message CommandHeader has no "device_id" field. +ERROR:biohead_client:主循环错误: Protocol message CommandHeader has no "device_id" field. +2025-07-14 15:19:52,804 - biohead_client - ERROR - Traceback (most recent call last): + File "/home/tankaitao/cmvr-biohead/grpc_cli.py", line 666, in run + header = build_command_header(self.device_id) + File "/home/tankaitao/cmvr-biohead/grpc_cli.py", line 465, in build_command_header + header.device_id = device_id +AttributeError: Protocol message CommandHeader has no "device_id" field. + +ERROR:biohead_client:Traceback (most recent call last): + File "/home/tankaitao/cmvr-biohead/grpc_cli.py", line 666, in run + header = build_command_header(self.device_id) + File "/home/tankaitao/cmvr-biohead/grpc_cli.py", line 465, in build_command_header + header.device_id = device_id +AttributeError: Protocol message CommandHeader has no "device_id" field. + +2025-07-14 15:19:53,493 - biohead_client - INFO - 资源清理完成 +INFO:biohead_client:资源清理完成 +2025-07-14 15:19:53,494 - biohead_client - INFO - BioHead客户端退出 +INFO:biohead_client:BioHead客户端退出 + diff --git a/script/grpc_gen.sh b/script/grpc_gen.sh new file mode 100644 index 0000000..0cde367 --- /dev/null +++ b/script/grpc_gen.sh @@ -0,0 +1,27 @@ +#!/bin/bash + +set -e + +PROTO_ROOT=protos +OUT_DIR=generated + +echo "[INFO] Generating gRPC Python code..." + +# 生成protobuf到指定包结构 +python -m grpc_tools.protoc \ + -Iprotos \ + --python_out=generated \ + --grpc_python_out=generated \ + protos/cmvr/api/common.proto \ + protos/cmvr/api/biohead_command.proto \ + protos/cmvr/api/biohead_service.proto + +echo "[INFO] Adding __init__.py files to make packages..." +# 确保生成的包结构里有__init__.py +touch ${OUT_DIR}/__init__.py +mkdir -p ${OUT_DIR}/cmvr +touch ${OUT_DIR}/cmvr/__init__.py +mkdir -p ${OUT_DIR}/cmvr/api +touch ${OUT_DIR}/cmvr/api/__init__.py + +echo "[INFO] gRPC Python code generation completed." \ No newline at end of file diff --git a/text_dgceshi.py b/text_dgceshi.py new file mode 100644 index 0000000..fa60765 --- /dev/null +++ b/text_dgceshi.py @@ -0,0 +1,198 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +interactive_servo_test.py — 基于 Python 的 ESP32 舵机控制交互脚本 +依赖：pip install pyserial +用法： + python3 interactive_servo_test.py /dev/ttyUSB0 [baudrate] +功能： + 1. 控制单个舵机 + 2. 控制多个舵机 + 3. 退出 + 4. 自动增减模式：从 0 到 180° 每步 +6° → -6° 循环，50Hz 频率发送 + 5. 设置自动模式舵机数量（通道 1~N） +""" +import sys +import time +import serial + +FRAME_HEADER = 0xAA # 帧头 + + +def wakeup_esp32(port, baud=115200): + """通过 RTS/DTR 线复位并唤醒 ESP32""" + try: + ser = serial.Serial(port, baud, timeout=1) + ser.setDTR(False) + ser.setRTS(False) + time.sleep(0.01) + ser.setDTR(True) + ser.setRTS(True) + ser.write(b"\r\n") + time.sleep(1) + except Exception as e: + print(f"ESP32 唤醒失败: {e}") + finally: + try: + ser.close() + except: + pass + + +def open_serial(port, baud): + """打开并配置串口""" + try: + ser = serial.Serial(port, baud, timeout=1) + print(f"已打开串口: {port} @ {baud}bps") + return ser + except Exception as e: + print(f"打开串口失败: {e}") + sys.exit(1) + + +def calc_checksum(data: bytearray) -> int: + """计算 XOR 校验和""" + cs = 0 + for b in data: + cs ^= b + return cs + + +def pack_commands(cmds): + """ + cmds: list of (addr, channel, angle, duration_ms) + 返回：完整帧字节数组 + 格式： [FRAME_HEADER][count] [addr, ch, ang, durL, durH]... [checksum] + """ + pkt = bytearray([FRAME_HEADER, len(cmds)]) + for addr, ch, ang, dur in cmds: + pkt.extend([addr, ch, ang, dur & 0xFF, (dur >> 8) & 0xFF]) + pkt.append(calc_checksum(pkt)) + return pkt + + +def print_packet(pkt): + """打印十六进制数据包""" + print("发送数据包:", ' '.join(f"0x{b:02X}" for b in pkt)) + + +def input_hex(prompt): + """输入 0x 开头或十进制整型""" + val = input(prompt).strip() + try: + return int(val, 0) + except ValueError: + print(f"无效数字: {val}") + return input_hex(prompt) + + +def input_cmd_single(): + """交互输入单条命令""" + addr = input_hex("输入 addr (hex or dec): ") + ch = input_hex("输入 channel (hex or dec): ") + ang = input_hex("输入 angle (0-180): ") + dur = input_hex("输入 duration(ms): ") + return [(addr, ch, ang, dur)] + + +def input_cmd_multiple(): + """交互输入多条命令""" + cnt = input_hex("输入命令数量: ") + cmds = [] + for i in range(cnt): + print(f"第 {i+1} 条:") + cmds.extend(input_cmd_single()) + return cmds + + +def auto_mode(ser, channels): + """自动增减模式：0→180→0，步长6，50Hz，多舵机""" + addr = input_hex("自动模式: 输入 addr (hex or dec): ") + interval = 1.0 / 50.0 + dur_ms = int(interval * 1000) + angle = 0 + direction = 1 + step = 6 + + print(f"启动自动模式 @50Hz, addr=0x{addr:02X}, channels={channels}, step={step}°") + try: + while True: + cmds = [(addr, ch, angle, dur_ms) for ch in channels] + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) + # 更新角度 + angle += direction * step + if angle >= 180: + angle = 180 + direction = -1 + elif angle <= 0: + angle = 0 + direction = 1 + time.sleep(interval) + except KeyboardInterrupt: + print("\n自动模式已停止，返回菜单") + + +def set_channels(): + """自定义舵机数量，返回通道列表 1~N""" + cnt = input_hex("输入自动模式舵机数量 N: ") + if cnt < 1: + print("数量必须 >= 1") + return None + channels = list(range(1, cnt+1)) + print(f"已设置通道列表: {channels}") + return channels + + +def main(): + if len(sys.argv) < 2: + print(f"用法: {sys.argv[0]} /dev/ttyUSB0 [baudrate]") + sys.exit(1) + port = sys.argv[1] + baud = int(sys.argv[2]) if len(sys.argv) > 2 else 115200 + + print("唤醒 ESP32...") + wakeup_esp32(port, baud) + ser = open_serial(port, baud) + channels = list(range(1, 10)) # 默认通道 1-9 + + try: + while True: + print("\n=== 操作菜单 ===") + print("1. 控制单个舵机") + print("2. 控制多个舵机") + print("3. 退出") + print("4. 自动增减模式(50Hz)") + print("5. 设置自动模式舵机数量") + opt = input("选择: ").strip() + if opt == '1': + cmds = input_cmd_single() + elif opt == '2': + cmds = input_cmd_multiple() + elif opt == '4': + auto_mode(ser, channels) + continue + elif opt == '5': + new_ch = set_channels() + if new_ch: + channels = new_ch + continue + elif opt == '3': + break + else: + print("无效选项，重试") + continue + + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) + print("✅ 指令已发送") + except KeyboardInterrupt: + print("\n用户中断，退出") + finally: + ser.close() + print("串口已关闭") + +if __name__ == '__main__': + main()