Compare commits
No commits in common. "my-feature-branch" and "main" have entirely different histories.
my-feature
...
main
299
biohead/algo.py
299
biohead/algo.py
@ -1,299 +0,0 @@
|
||||
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
|
||||
@ -1,118 +0,0 @@
|
||||
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)
|
||||
139
biohead/utils.py
139
biohead/utils.py
@ -1,139 +0,0 @@
|
||||
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
|
||||
|
||||
|
||||
|
||||
@ -1,92 +0,0 @@
|
||||
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()
|
||||
@ -1,102 +0,0 @@
|
||||
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
|
||||
@ -1,54 +0,0 @@
|
||||
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]
|
||||
|
||||
82
demo.py
82
demo.py
@ -1,82 +0,0 @@
|
||||
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")
|
||||
@ -1,67 +0,0 @@
|
||||
# -*- 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)
|
||||
@ -1,4 +0,0 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
"""Client and server classes corresponding to protobuf-defined services."""
|
||||
import grpc
|
||||
|
||||
@ -1,27 +0,0 @@
|
||||
# -*- 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)
|
||||
@ -1,174 +0,0 @@
|
||||
# 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)
|
||||
@ -1,35 +0,0 @@
|
||||
# -*- 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)
|
||||
@ -1,4 +0,0 @@
|
||||
# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT!
|
||||
"""Client and server classes corresponding to protobuf-defined services."""
|
||||
import grpc
|
||||
|
||||
388
grpc_cli.py
388
grpc_cli.py
@ -1,388 +0,0 @@
|
||||
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)
|
||||
#
|
||||
#
|
||||
306
grpc_lb.py
306
grpc_lb.py
@ -1,306 +0,0 @@
|
||||
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)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -1,175 +0,0 @@
|
||||
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; // 已停止的进程列表
|
||||
}
|
||||
}
|
||||
@ -1,22 +0,0 @@
|
||||
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){};
|
||||
}
|
||||
@ -1,30 +0,0 @@
|
||||
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; // 回复时间
|
||||
}
|
||||
}
|
||||
@ -1,69 +0,0 @@
|
||||
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客户端退出
|
||||
|
||||
@ -1,27 +0,0 @@
|
||||
#!/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."
|
||||
198
text_dgceshi.py
198
text_dgceshi.py
@ -1,198 +0,0 @@
|
||||
#!/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()
|
||||
Loading…
Reference in New Issue
Block a user