Welcome to Raspbot V2 repository

5. Face following

5.1. Experimental Objectives

In this class, we will start to implement face tracking with the camera gimbal. By identifying the face object and detecting the difference between the x, y coordinates of the circumscribed circle of the identified face and the center of the screen, the PID algorithm is used to control the Y-axis servo and the vehicle body to make the identified target located in the center of the screen

5.2. Experimental Code

Source code path:

/home/pi/project_demo/08.AI_Visual_Interaction_Course/05.Face_tracking/05_Face_tracking.ipynb

​x
#bgr8转jpeg格式 bgr8 to jpeg format
import enum
import cv2

def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])

xxxxxxxxxx
import sys
sys.path.append('/home/pi/project_demo/lib')
#导入麦克纳姆小车驱动库 Import Mecanum Car Driver Library
from McLumk_Wheel_Sports import *

import cv2
import mediapipe as mp
import ipywidgets.widgets as widgets
import threading
import time
import sys
import math

image_widget = widgets.Image(format='jpeg', width=640, height=480)

xxxxxxxxxx
global face_x, face_y, face_w, face_h
face_x = face_y = face_w = face_h = 0
global target_valuex
target_valuex = 2048
global target_valuey
target_valuey = 2048

xxxxxxxxxx
import PID
#xservo_pid = PID.PositionalPID(1.1, 0.4, 0.01)#1.1 0.4 0.01
direction_pid = PID.PositionalPID(0.8, 0, 0.2)
yservo_pid = PID.PositionalPID(0.8, 0.2, 0.01)
speed_pid = PID.PositionalPID(1.1, 0, 0.2)

xxxxxxxxxx
# 定义 target_servox 和 target_servoy 在外部 Define target_servox and target_servoy externally
target_servox = 90
target_servoy = 25
def servo_reset():
    bot.Ctrl_Servo(1,90)
    bot.Ctrl_Servo(2,80)
servo_reset()

xxxxxxxxxx
# 线程功能操作库 Thread function operation library
import inspect
import ctypes
def _async_raise(tid, exctype):
    """raises the exception, performs cleanup if needed"""
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        # """if it returns a number greater than one, you're in trouble,
        # and you should call it again with exc=NULL to revert the effect"""
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        
def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)

xxxxxxxxxx
class FaceDetector:
    def __init__(self, minDetectionCon=0.5):
        self.mpFaceDetection = mp.solutions.face_detection
        self.mpDraw = mp.solutions.drawing_utils
        self.facedetection = self.mpFaceDetection.FaceDetection(min_detection_confidence=minDetectionCon)

    def findFaces(self, frame):
        img_RGB = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
        self.results = self.facedetection.process(img_RGB)
        bboxs = []
        bbox=0,0,0,0
        center_x=center_y=0
        if self.results.detections:
            for id, detection in enumerate(self.results.detections):
                bboxC = detection.location_data.relative_bounding_box
                ih, iw, ic = frame.shape
                bbox = int(bboxC.xmin * iw), int(bboxC.ymin * ih), \
                       int(bboxC.width * iw), int(bboxC.height * ih)
                #计算中心点
                center_x = bbox[0] + bbox[2] // 2
                center_y = bbox[1] + bbox[3] // 2
                bboxs.append([id, bbox, detection.score])
                frame= self.fancyDraw(frame, bbox)
                # cv2.putText(frame, f'{int(detection.score[0] * 100)}%',
                #            (bbox[0], bbox[1] - 20), cv2.FONT_HERSHEY_PLAIN,
                #            3, (255, 0, 255), 2)
        return frame, bboxs, self.results.detections, bbox, center_x

    def fancyDraw(self, frame, bbox, l=30, t=5):
        x, y, w, h = bbox
        x1, y1 = x + w, y + h
        cv2.rectangle(frame, (x, y),(x + w, y + h), (0,255,0), 2)
        # Top left x,y
        cv2.line(frame, (x, y), (x + l, y), (0,255,0), t)
        cv2.line(frame, (x, y), (x, y + l), (0,255,0), t)
        # Top right x1,y
        cv2.line(frame, (x1, y), (x1 - l, y), (0,255,0), t)
        cv2.line(frame, (x1, y), (x1, y + l), (0,255,0), t)
        # Bottom left x1,y1
        cv2.line(frame, (x, y1), (x + l, y1), (0,255,0), t)
        cv2.line(frame, (x, y1), (x, y1 - l), (0,255,0), t)
        # Bottom right x1,y1
        cv2.line(frame, (x1, y1), (x1 - l, y1), (0,255,0), t)
        cv2.line(frame, (x1, y1), (x1, y1 - l), (0,255,0), t)
        return frame

xxxxxxxxxx
image = cv2.VideoCapture(0)
image.set(3,320)
image.set(4,240)
# image.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
# image.set(cv2.CAP_PROP_BRIGHTNESS, 62) #设置亮度 -64 - 64  0.0 Set Brightness -64 - 64 0.0
# image.set(cv2.CAP_PROP_CONTRAST, 63) #设置对比度 -64 - 64  2.0 Set Contrast -64 - 64 2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 4800) #设置曝光值 1.0 - 5000  156.0 Set the exposure value 1.0 - 5000 156.0

#csi
# from picamera2 import Picamera2, Preview
# import libcamera
# picam2 = Picamera2()  
# camera_config = picam2.create_preview_configuration(main={"format":'RGB888',"size":(320,240)})
# camera_config["transform"] = libcamera.Transform(hflip=1, vflip=1)
# picam2.configure(camera_config) 
# picam2.start()  

xxxxxxxxxx
image = cv2.VideoCapture(0)
image.set(3,320)
image.set(4,240)
# image.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
# image.set(cv2.CAP_PROP_BRIGHTNESS, 62) #设置亮度 -64 - 64  0.0 Set Brightness -64 - 64 0.0
# image.set(cv2.CAP_PROP_CONTRAST, 63) #设置对比度 -64 - 64  2.0 Set Contrast -64 - 64 2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 4800) #设置曝光值 1.0 - 5000  156.0 Set the exposure value 1.0 - 5000 156.0

#csi
# from picamera2 import Picamera2, Preview
# import libcamera
# picam2 = Picamera2()  
# camera_config = picam2.create_preview_configuration(main={"format":'RGB888',"size":(320,240)})
# camera_config["transform"] = libcamera.Transform(hflip=1, vflip=1)
# picam2.configure(camera_config) 
# picam2.start()  

With dead zone control, the real-time following performance is poor, the servo does not move within the dead zone, and the jitter is relatively stable

xxxxxxxxxx
def Face_Follow():
    global x,w,y,h
    speed=30
    face_detector = FaceDetector(0.75)
    while 1:
        ret, frame = image.read()
        #frame = picam2.capture_array()
        faces,_,descore,bbox,center_x= face_detector.findFaces(frame)
        x,y,w,h = bbox
        if descore:
            direction_pid.SystemOutput = center_x
            direction_pid.SetStepSignal(250)
            direction_pid.SetInertiaTime(0.01, 0.05)
            target_valuex = int(direction_pid.SystemOutput+65)
            # 输入Y轴方向参数PID控制输入 Input Y-axis direction parameter PID control input
            if math.fabs(180 - (y + h/2)) > 40:
                yservo_pid.SystemOutput = y + h/2
                yservo_pid.SetStepSignal(280)
                yservo_pid.SetInertiaTime(0.01, 0.05)
                target_valuey = int(1150+yservo_pid.SystemOutput)
                target_servoy = int((target_valuey-500)/10)                   
                #print("target_servoy %d", target_servoy)  
                if target_servoy > 100:
                    target_servoy = 100
                if target_servoy < 0:
                    target_servoy = 0        
                bot.Ctrl_Servo(2, target_servoy)

            speed_pid.SystemOutput = int(h/2)
            speed_pid.SetStepSignal(80)
            speed_pid.SetInertiaTime(0.01, 0.1)               
            speed_value = int(speed_pid.SystemOutput)

            # 打印文本到图像
            text = f"color_radius {int(h/2)} target_valuex {target_valuex}"
            font = cv2.FONT_HERSHEY_SIMPLEX
            font_scale = 1
            color = (255, 0, 0)  # 白色
            thickness = 2
            text_position = (10, 60)  # 文本位置
            cv2.putText(faces, text, text_position, font, font_scale, color, thickness)
            #print("color_radius %d target_valuex%d", h/2,target_valuex)

            if speed_value > 255:
                speed_value = 255
            if speed_value < 0:
                speed_value = 0
                
            if(target_valuex>50):
                rotate_left(int(speed/5))# speed
            elif(target_valuex<-50):
                rotate_right(int(speed/5))
            elif(75<h/2<100):#调试目标半径75~100 Debug target radius 65~90
                stop_robot()
            elif(h/2>60):#调试目标半径58 Debug target radius 58
                if(abs(target_valuex)<30):
                    move_backward(speed)
            elif(20<h/2<55):
                if(abs(target_valuex)<30):
                    move_forward(speed_value)
            else:stop_robot()

            #bot.Ctrl_Servo(2,target_servoy)
        
        else:
            stop_robot()
        try:
            image_widget.value = bgr8_to_jpeg(faces)
        except:
            continue

xxxxxxxxxx
display(image_widget)
thread1 = threading.Thread(target=Face_Follow)
thread1.daemon=True
thread1.start()

#picam2.stop()
#picam2.close()

xxxxxxxxxx
#结束进程，释放摄像头，需要结束时执行 End the process, release the camera, and execute when it is finished
stop_thread(thread1)
#释放摄像头资源 Release camera resources
image.release()
#复位舵机 Reset servo
bot.Ctrl_Servo(1,90)
bot.Ctrl_Servo(2,25)

5.3. Experimental Phenomenon

After the code block is run, we put a face in front of the camera. After the camera recognizes the face, it will control the Y axis of the gimbal and the body to move in the direction of the face.