一、实验目标

        前面我们介绍了使用 MediaPipe 实现手势识别,这章节我们利用 MediaPipe 结合手势识别和麦轮以及云台的控制,实现小车跟随手部,用手势控制小车运动的功能。

二、实验源码

import sys
sys.path.append('/home/pi/project_demo/lib')
#导入麦克纳姆小车驱动库 Import Mecanum Car Driver Library
from McLumk_Wheel_Sports import *
import cv2
import ipywidgets.widgets as widgets
import time
import sys
import math
from Raspbot_Lib import Raspbot
import threading


speed = 25
image_widget = widgets.Image(format='jpeg', width=640, height=480)
global target_valuex
target_valuex = 1500
global target_valuey
target_valuey = 850


bot = Raspbot()

# 初始化云台 Init Servo
target_servox = 90
target_servoy = 60
def servo_reset():
    bot.Ctrl_Servo(1,target_servox)
    bot.Ctrl_Servo(2,target_servoy)
servo_reset()
import PID
direction_pid = PID.PositionalPID(0.9, 0, 0.2)
yservo_pid = PID.PositionalPID(0.9, 0.2, 0.01)

from gesture_action import handDetector
hand_detector = handDetector(detectorCon=0.8)
cap = cv2.VideoCapture(0)
# 线程功能操作库 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)
def Gesture_follow():
    try:
        while True:
            global bot
            ret, frame = cap.read()
            img_height, img_width, _ = frame.shape
            hand_detector.findHands(frame, draw=False) 
            if len(hand_detector.lmList) != 0:
                # 转向控制部分
                # Turning control section
                # MediaPipe中, 手部最中心的指关节的编号为9
                # In MediaPipe, the index of the central finger joint is 9
                x,y = hand_detector.findPoint(9)
                cv2.circle(frame,(int(x),int(y)),5,(0,255,255),10)
                # 在x轴上控制麦轮
                # Control the mouse wheel on the x-axis
                direction_pid.SystemOutput = x
                direction_pid.SetStepSignal(int(img_width/2))
                direction_pid.SetInertiaTime(0.01, 0.05)
                target_valuex = int(direction_pid.SystemOutput) 
    
                # 在y轴上控制云台, 摄像头中心与指关节9在竖直方向的差值<25时不调节,减少抖动
                # Control the pan/tilt in the y-axis; if the difference between the camera center and finger joint 9 in the vertical direction is <25, do not adjust to reduce jitter
                if math.fabs(int(img_height/2) - y) > 25:
                    yservo_pid.SystemOutput = y
                    yservo_pid.SetStepSignal(int(img_height/2))
                    yservo_pid.SetInertiaTime(0.01, 0.05)
                    target_valuey = int(850+yservo_pid.SystemOutput)
                    target_servoy = int((target_valuey-500)/10)                    
                    if target_servoy > 110:
                        target_servoy = 110
                    if target_servoy < 0:
                        target_servoy = 0        
                    bot.Ctrl_Servo(2, target_servoy)
    
                # 前进控制部分
                # Forward control section
                finger_number = hand_detector.get_gesture()
                finger_str=f"Number:{finger_number}"
    
                # 手势 0 控制小车停止
                # Gesture 0 controls the robot to stop
                if(finger_number == "Zero"):
                    stop_robot()
                else:
                    if -40 < target_valuex < 40:
                        target_valuex = 0   
                    move_param_forward(speed, target_valuex) 
            else:
                stop_robot()
            try:
                image_widget.value = bgr8_to_jpeg(frame)
            except:
                continue
    finally:
        stop_robot()
        cap.release()
        del bot
display(image_widget)
thread1 = threading.Thread(target=Gesture_follow)
thread1.daemon=True
thread1.start()

三、实验现象

        代码块运行后,将手部放置于小车摄像头可视范围内,当做出除了手势 “0” 以外的其余任意手势时,小车将会追踪手部中心点移动。当做出“0”的手势时,小车将不会跟随手部运动。

Logo

有“AI”的1024 = 2048,欢迎大家加入2048 AI社区

更多推荐