v2.0增加建议云台人脸跟踪效果和开源设计文件
This commit is contained in:
myperworld committed
1 parent
cc9de2f90b
commit
cc61eda1c7
1857 files changed
+527789
-1
No files matched your search
@@ -0,0 +1,470 @@
|
||||
'''
|
||||
主题：MAIXCAM 云台
|
||||
作者：Tommy
|
||||
日期：2025年-12月-21日
|
||||
'''
|
||||
from maix import camera, display, image, nn, app, sys
|
||||
from maix import uart, pinmap, time, err, gpio, network
|
||||
import paho.mqtt.client as mqtt
|
||||
import asyncio
|
||||
import gc
|
||||
import traceback
|
||||
|
||||
|
||||
DEVICE_ID = "XXX"
|
||||
|
||||
# 两套WiFi/MQTT配置
|
||||
WIFI_CONFIGS = [
|
||||
{
|
||||
"SSID": "X",
|
||||
"PASSWORD": "Y",
|
||||
"MQTT_BROKER": "Z",
|
||||
},
|
||||
]
|
||||
|
||||
MQTT_PORT = 1883
|
||||
|
||||
# 发送的主题
|
||||
MQTT_PUB_TOPIC_IMAGE = "hangtianyuan/head_video" # 图像发布主题
|
||||
|
||||
# 订阅的主题
|
||||
MQTT_SUB_TOPIC_TEXT = "hangtianyuan/text" # 文本指令订阅主题
|
||||
MQTT_SUB_TOPIC_MOVE = "hangtianyuan/set_move" # 舵机移动指令订阅主题
|
||||
# 批量订阅
|
||||
MQTT_SUB_TOPICS = [
|
||||
(MQTT_SUB_TOPIC_TEXT, 0),
|
||||
(MQTT_SUB_TOPIC_MOVE, 0),
|
||||
]
|
||||
|
||||
# LED配置
|
||||
LED_PIN_NAME = "A14"
|
||||
LED_GPIO_NAME = "GPIOA14"
|
||||
BLINK_INTERVAL = 500 # LED闪烁间隔(ms)
|
||||
|
||||
# 人脸追踪参数
|
||||
kp_x = 0.05 # 水平方向 0.04
|
||||
kp_y = 0.07 # 垂直方向
|
||||
angle_threshold = 1
|
||||
s1_range = [0, 180]
|
||||
s2_range = [0, 180]
|
||||
no_face_threshold = 30
|
||||
|
||||
# 滤波缓存
|
||||
last_dev_x = 0.0
|
||||
last_dev_y = 0.0
|
||||
filter_alpha = 0.85
|
||||
|
||||
# ------------------------------
|
||||
# 全局状态变量（新增track_mode控制跟踪开关）
|
||||
# ------------------------------
|
||||
mqtt_connected = False
|
||||
wifi_connected = False
|
||||
current_wifi_idx = 0
|
||||
led = None
|
||||
mqtt_client = None
|
||||
previous_millis = 0
|
||||
send_frame_interval = 8
|
||||
frame_count = 0
|
||||
track_mode = 1 # 0:关闭追踪，1:人脸跟踪模式
|
||||
serial_dev = None # 新增：舵机串口全局变量
|
||||
|
||||
# ------------------------------
|
||||
# 工具函数
|
||||
# ------------------------------
|
||||
def constrain(val, min_val, max_val):
|
||||
return min(max_val, max(min_val, val))
|
||||
|
||||
# ------------------------------
|
||||
# LED控制
|
||||
# ------------------------------
|
||||
def init_led():
|
||||
"""初始化LED引脚"""
|
||||
global led
|
||||
try:
|
||||
err.check_raise(
|
||||
pinmap.set_pin_function(LED_PIN_NAME, LED_GPIO_NAME),
|
||||
f"设置LED引脚{LED_PIN_NAME}功能失败"
|
||||
)
|
||||
led = gpio.GPIO(LED_GPIO_NAME, gpio.Mode.OUT)
|
||||
led.value(0) # 初始熄灭
|
||||
print(f"LED初始化完成：引脚{LED_PIN_NAME}（{LED_GPIO_NAME}）")
|
||||
except Exception as e:
|
||||
print(f"LED初始化失败: {e}")
|
||||
led = None
|
||||
|
||||
def update_led():
|
||||
"""更新LED状态：MQTT未连接闪烁，连接成功常亮"""
|
||||
global previous_millis, mqtt_connected
|
||||
if led is None:
|
||||
return
|
||||
|
||||
current_millis = time.time_ms()
|
||||
if not mqtt_connected:
|
||||
if current_millis - previous_millis >= BLINK_INTERVAL:
|
||||
previous_millis = current_millis
|
||||
led.toggle()
|
||||
else:
|
||||
if led.value() != 1:
|
||||
led.value(1)
|
||||
|
||||
# ------------------------------
|
||||
# WiFi连接（异步）
|
||||
# ------------------------------
|
||||
async def connect_wifi_async():
|
||||
"""异步尝试连接WiFi（自动切换两套配置）"""
|
||||
global wifi_connected, current_wifi_idx
|
||||
w = network.wifi.Wifi()
|
||||
|
||||
wifi_connected = False
|
||||
current_wifi_idx = 0
|
||||
|
||||
while current_wifi_idx < len(WIFI_CONFIGS) and not wifi_connected:
|
||||
config = WIFI_CONFIGS[current_wifi_idx]
|
||||
ssid = config["SSID"]
|
||||
password = config["PASSWORD"]
|
||||
|
||||
print(f"[WiFi] 尝试连接 {ssid} (第{current_wifi_idx+1}套)")
|
||||
try:
|
||||
e = w.connect(ssid, password, wait=True, timeout=10)
|
||||
err.check_raise(e, f"WiFi连接失败")
|
||||
wifi_connected = True
|
||||
ip = w.get_ip()
|
||||
print(f"[WiFi] 连接成功！IP: {ip}")
|
||||
except Exception as e:
|
||||
print(f"[WiFi] {ssid} 连接失败: {e}")
|
||||
current_wifi_idx += 1
|
||||
await asyncio.sleep(1)
|
||||
|
||||
if not wifi_connected:
|
||||
print("[WiFi] 所有配置连接失败，无联网状态")
|
||||
|
||||
# ------------------------------
|
||||
# MQTT回调函数（新增track_mode控制逻辑）
|
||||
# ------------------------------
|
||||
def on_mqtt_connect(client, userdata, flags, rc):
|
||||
"""MQTT连接回调"""
|
||||
global mqtt_connected
|
||||
if rc == 0:
|
||||
mqtt_connected = True
|
||||
print("[MQTT] 连接成功")
|
||||
# 订阅主题（如有）
|
||||
if MQTT_SUB_TOPICS:
|
||||
client.subscribe(MQTT_SUB_TOPICS)
|
||||
print(f"[MQTT] 已订阅主题: {MQTT_SUB_TOPICS}")
|
||||
else:
|
||||
mqtt_connected = False
|
||||
print(f"[MQTT] 连接失败，错误码: {rc}")
|
||||
|
||||
def on_mqtt_message(client, userdata, msg):
|
||||
global track_mode, serial_dev
|
||||
if msg.topic == MQTT_SUB_TOPIC_TEXT:
|
||||
payload = msg.payload.decode().strip() # 去除首尾空格，提高容错
|
||||
print(f"文本指令: {payload}")
|
||||
# 解析跟踪模式指令
|
||||
if payload.lower() == "track_off":
|
||||
track_mode = 0
|
||||
print("[跟踪模式] 已关闭人脸跟踪，停止绘制人脸框和追踪")
|
||||
elif payload.lower() == "track_face":
|
||||
track_mode = 1
|
||||
print("[跟踪模式] 已开启人脸跟踪，开始绘制人脸框和追踪")
|
||||
elif payload.lower() == "track_hand":
|
||||
track_mode = 2
|
||||
print("[跟踪模式] 已开启手部跟踪，开始绘制手部框和追踪")
|
||||
elif msg.topic == MQTT_SUB_TOPIC_MOVE:
|
||||
payload = msg.payload.decode().strip()
|
||||
# 修复：判空保护 + 使用全局serial_dev
|
||||
if serial_dev is not None:
|
||||
try:
|
||||
serial_dev.write_str(payload)
|
||||
print(f"运动指令已发送: {payload}")
|
||||
except Exception as e:
|
||||
print(f"发送运动指令失败: {e} | 指令内容: {payload}")
|
||||
else:
|
||||
print(f"运动指令: {payload}，但串口未初始化")
|
||||
|
||||
def on_mqtt_disconnect(client, userdata, rc):
|
||||
"""MQTT断开回调"""
|
||||
global mqtt_connected
|
||||
mqtt_connected = False
|
||||
if rc != 0:
|
||||
print(f"[MQTT] 意外断开，错误码: {rc}")
|
||||
|
||||
# ------------------------------
|
||||
# MQTT初始化（异步）
|
||||
# ------------------------------
|
||||
async def init_mqtt_async():
|
||||
"""异步初始化MQTT（依赖WiFi，自动重试）"""
|
||||
global mqtt_client, mqtt_connected, current_wifi_idx
|
||||
mqtt_connected = False
|
||||
mqtt_client = None
|
||||
|
||||
# 等待WiFi连接（最多20秒）
|
||||
wait_time = 0
|
||||
while not app.need_exit() and not wifi_connected and wait_time < 20:
|
||||
await asyncio.sleep(1)
|
||||
wait_time += 1
|
||||
|
||||
if app.need_exit() or not wifi_connected:
|
||||
print("[MQTT] WiFi未连接，跳过MQTT初始化")
|
||||
return
|
||||
|
||||
# 兜底索引
|
||||
if current_wifi_idx >= len(WIFI_CONFIGS):
|
||||
current_wifi_idx = 0
|
||||
current_broker = WIFI_CONFIGS[current_wifi_idx]["MQTT_BROKER"]
|
||||
|
||||
try:
|
||||
mqtt_client = mqtt.Client(DEVICE_ID)
|
||||
mqtt_client.on_connect = on_mqtt_connect
|
||||
mqtt_client.on_message = on_mqtt_message
|
||||
mqtt_client.on_disconnect = on_mqtt_disconnect
|
||||
|
||||
# 重试连接
|
||||
retry_count = 0
|
||||
while not app.need_exit() and not mqtt_connected and retry_count < 5:
|
||||
try:
|
||||
print(f"[MQTT] 尝试连接 {current_broker}:{MQTT_PORT} (重试{retry_count+1}/5)")
|
||||
mqtt_client.connect(current_broker, MQTT_PORT, 60)
|
||||
mqtt_client.loop_start()
|
||||
|
||||
# 等待连接结果
|
||||
t = time.time()
|
||||
while not mqtt_connected and time.time() - t < 5:
|
||||
await asyncio.sleep(0.1)
|
||||
|
||||
if mqtt_connected:
|
||||
break
|
||||
else:
|
||||
mqtt_client.disconnect()
|
||||
retry_count += 1
|
||||
await asyncio.sleep(3)
|
||||
except Exception as e:
|
||||
print(f"[MQTT] 连接异常: {e}")
|
||||
retry_count += 1
|
||||
await asyncio.sleep(3)
|
||||
|
||||
if not mqtt_connected:
|
||||
print("[MQTT] 多次重试后仍无法连接")
|
||||
except Exception as e:
|
||||
print(f"[MQTT] 初始化失败: {e}")
|
||||
mqtt_client = None
|
||||
mqtt_connected = False
|
||||
|
||||
# ------------------------------
|
||||
# 图像MQTT发送
|
||||
# ------------------------------
|
||||
def send_image_via_mqtt(img):
|
||||
"""发送图像到MQTT（JPEG压缩）"""
|
||||
global mqtt_connected, mqtt_client
|
||||
if not mqtt_connected or mqtt_client is None:
|
||||
return False
|
||||
|
||||
try:
|
||||
jpeg_data = img.to_jpeg(quality=85).to_bytes()
|
||||
# 二进制发送（高效）
|
||||
mqtt_client.publish(MQTT_PUB_TOPIC_IMAGE, jpeg_data)
|
||||
gc.collect()
|
||||
return True
|
||||
except Exception as e:
|
||||
print(f"[MQTT] 图像发送失败: {e}")
|
||||
try:
|
||||
if mqtt_client:
|
||||
mqtt_client.reconnect()
|
||||
except:
|
||||
mqtt_connected = False
|
||||
return False
|
||||
|
||||
# ------------------------------
|
||||
# 系统初始化
|
||||
# ------------------------------
|
||||
def init_system():
|
||||
|
||||
"""初始化相机、舵机、串口、LED"""
|
||||
global serial_dev # 声明使用全局serial_dev
|
||||
|
||||
"""初始化相机、舵机、串口、LED"""
|
||||
init_led()
|
||||
|
||||
# 人脸检测器
|
||||
detector = nn.YOLOv8(model="/root/models/yolov8n_face.mud", dual_buff=True)
|
||||
img_width = detector.input_width()
|
||||
img_height = detector.input_height()
|
||||
print(f"[系统] 图像尺寸: {img_width}x{img_height}")
|
||||
image_center = (img_width // 2, img_height // 2)
|
||||
|
||||
# 相机/显示器
|
||||
cam = camera.Camera(img_width, img_height, detector.input_format())
|
||||
disp = display.Display()
|
||||
|
||||
# 串口（舵机）
|
||||
pin_function = {"A19": "UART1_TX", "A18": "UART1_RX"}
|
||||
device = "/dev/ttyS1"
|
||||
for pin, func in pin_function.items():
|
||||
err.check_raise(pinmap.set_pin_function(pin, func),
|
||||
f"Failed set pin{pin} function to {func}")
|
||||
serial_dev = uart.UART(device, 115200)
|
||||
|
||||
# 舵机初始角度
|
||||
current_s1, current_s2 = 90, 90
|
||||
initial_cmd = f"S1:{current_s1},S2:{current_s2};"
|
||||
serial_dev.write_str(initial_cmd)
|
||||
print(f"[舵机] 初始角度: {initial_cmd}")
|
||||
time.sleep(1)
|
||||
|
||||
return {
|
||||
"detector": detector, "cam": cam, "disp": disp, "serial_dev": serial_dev,
|
||||
"img_width": img_width, "img_height": img_height, "image_center": image_center,
|
||||
"current_s1": current_s1, "current_s2": current_s2
|
||||
}
|
||||
|
||||
# ------------------------------
|
||||
# 人脸检测处理（新增track_mode判断，关闭时不绘制人脸框）
|
||||
# ------------------------------
|
||||
def process_faces(img, objs):
|
||||
global track_mode
|
||||
# 跟踪模式关闭时，直接返回无脸坐标，不绘制任何框
|
||||
if not track_mode :
|
||||
return (-1, -1, -1)
|
||||
|
||||
max_face = None
|
||||
max_width = 0
|
||||
|
||||
if objs:
|
||||
for obj in objs:
|
||||
if obj.w > max_width:
|
||||
max_width = obj.w
|
||||
max_face = obj
|
||||
|
||||
# 绘制人脸框（仅跟踪模式开启时执行）
|
||||
for obj in objs:
|
||||
color = image.COLOR_GREEN if obj == max_face else image.COLOR_RED
|
||||
img.draw_rect(obj.x, obj.y, obj.w, obj.h, color=color, thickness=2)
|
||||
|
||||
if max_face:
|
||||
center_x = int(max_face.x + max_face.w / 2)
|
||||
center_y = int(max_face.y + max_face.h / 2)
|
||||
img.draw_keypoints([center_x, center_y], image.COLOR_GREEN, size=3)
|
||||
return (center_x, center_y, int(max_face.w))
|
||||
else:
|
||||
return (-1, -1, -1)
|
||||
|
||||
# ------------------------------
|
||||
# 人脸追踪
|
||||
# ------------------------------
|
||||
def track_face(center, image_center, current_angles, serial_dev):
|
||||
global last_dev_x, last_dev_y
|
||||
center_x, center_y = center
|
||||
current_s1, current_s2 = current_angles
|
||||
|
||||
# 偏差计算+滤波
|
||||
deviation_x = center_x - image_center[0]
|
||||
deviation_y = center_y - image_center[1]
|
||||
smoothed_x = filter_alpha * last_dev_x + (1 - filter_alpha) * deviation_x
|
||||
smoothed_y = filter_alpha * last_dev_y + (1 - filter_alpha) * deviation_y
|
||||
last_dev_x, last_dev_y = smoothed_x, smoothed_y
|
||||
|
||||
# 角度计算+限制
|
||||
# 金属舵机
|
||||
# new_s1 = constrain(int(current_s1 + smoothed_x * kp_x), s1_range[0], s1_range[1])
|
||||
# new_s2 = constrain(int(current_s2 + smoothed_y * kp_y), s2_range[0], s2_range[1])
|
||||
# 塑料舵机
|
||||
new_s1 = constrain(int(current_s1 - smoothed_x * kp_x), s1_range[0], s1_range[1])
|
||||
new_s2 = constrain(int(current_s2 - smoothed_y * kp_y), s2_range[0], s2_range[1])
|
||||
|
||||
# 发送舵机指令
|
||||
if abs(new_s1 - current_s1) > angle_threshold or abs(new_s2 - current_s2) > angle_threshold:
|
||||
cmd = f"S1:{new_s1},S2:{new_s2};"
|
||||
serial_dev.write_str(cmd)
|
||||
return (new_s1, new_s2, cmd)
|
||||
else:
|
||||
return (current_s1, current_s2, None)
|
||||
|
||||
# ------------------------------
|
||||
# 主循环（新增track_mode判断和状态显示）
|
||||
# ------------------------------
|
||||
async def main_loop_async(resources):
|
||||
"""异步主循环"""
|
||||
detector = resources["detector"]
|
||||
cam = resources["cam"]
|
||||
disp = resources["disp"]
|
||||
serial_dev = resources["serial_dev"]
|
||||
img_width = resources["img_width"]
|
||||
img_height = resources["img_height"]
|
||||
image_center = resources["image_center"]
|
||||
current_s1, current_s2 = resources["current_s1"], resources["current_s2"]
|
||||
|
||||
# 启动网络异步任务
|
||||
asyncio.create_task(connect_wifi_async())
|
||||
asyncio.create_task(init_mqtt_async())
|
||||
|
||||
# 主循环
|
||||
global frame_count, mqtt_connected, wifi_connected, track_mode
|
||||
while not app.need_exit():
|
||||
# 图像读取+人脸检测
|
||||
img = cam.read().flip(image.FlipDir.Y)
|
||||
objs = detector.detect(img, conf_th=0.4, iou_th=0.45)
|
||||
center_x, center_y, face_width = process_faces(img, objs)
|
||||
|
||||
# 绘制基础状态
|
||||
img.draw_string(20, 20, f"Center: ({center_x}, {center_y})", image.COLOR_GREEN, 1.0)
|
||||
img.draw_string(20, 40, f"Max Width: {face_width}", image.COLOR_YELLOW, 1.0)
|
||||
|
||||
# 绘制跟踪模式状态（直观显示当前模式）
|
||||
track_status = "ON" if track_mode else "OFF"
|
||||
track_color = image.COLOR_GREEN if track_mode else image.COLOR_RED
|
||||
img.draw_string(20, 100, f"Track Mode: {track_status}", track_color, 1.0)
|
||||
|
||||
# 人脸追踪（仅跟踪模式开启时执行）
|
||||
if track_mode and center_x != -1 and center_y != -1:
|
||||
new_s1, new_s2, cmd = track_face((center_x, center_y), image_center, (current_s1, current_s2), serial_dev)
|
||||
current_s1, current_s2 = new_s1, new_s2
|
||||
if cmd:
|
||||
img.draw_string(20, 60, f"Tracking: {cmd}", image.COLOR_BLUE, 1.0)
|
||||
elif track_mode and center_x == -1:
|
||||
# 跟踪模式开启但无脸
|
||||
img.draw_string(20, 60, "No face detected", image.COLOR_RED, 1.0)
|
||||
else:
|
||||
# 跟踪模式关闭
|
||||
img.draw_string(20, 60, "Track Mode: OFF", image.COLOR_ORANGE, 1.0)
|
||||
|
||||
# 网络状态显示
|
||||
net_status = f"WiFi: {'OK' if wifi_connected else 'NO'} | MQTT: {'OK' if mqtt_connected else 'NO'}"
|
||||
img.draw_string(20, 80, net_status, image.COLOR_YELLOW, 1.0)
|
||||
|
||||
# LED更新+图像发送
|
||||
update_led()
|
||||
frame_count += 1
|
||||
if frame_count >= send_frame_interval:
|
||||
send_image_via_mqtt(img)
|
||||
frame_count = 0
|
||||
|
||||
disp.show(img)
|
||||
await asyncio.sleep(0.001)
|
||||
|
||||
# ------------------------------
|
||||
# 程序入口
|
||||
# ------------------------------
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
system_resources = init_system()
|
||||
asyncio.run(main_loop_async(system_resources))
|
||||
except Exception as e:
|
||||
print(f"[异常] 程序异常: {e}")
|
||||
traceback.print_exc()
|
||||
finally:
|
||||
# 清理MQTT
|
||||
if mqtt_client:
|
||||
try:
|
||||
mqtt_client.loop_stop()
|
||||
mqtt_client.disconnect()
|
||||
print("[MQTT] 客户端已断开")
|
||||
except Exception as e:
|
||||
print(f"[MQTT] 断开失败: {e}")
|
||||
# 清理LED
|
||||
if led:
|
||||
try:
|
||||
led.value(0)
|
||||
print("[LED] 已关闭")
|
||||
except Exception as e:
|
||||
print(f"[LED] 关闭失败: {e}")
|
||||
print("[系统] 程序已退出")
|
||||
Reference in new issue
Block a user