MediaPipeを使ってRCカーのコントロール（ジェスチャーインターフェース）

Mediapipe

MediaPipeとは？ Google社が提供するライブストリーミングのためのオープンソースのMLソリューションです。 このMediaPipeを利用すると高性能なAI画像処理アルゴリズムを利用したARアプリケーション等を簡単に作成できます。

概要

今回はPythonを用いて、ラジコンカーの操作を行なってみました。

　Python IDEでは、21点3Dランドマークのデータはnamed tuplesとして返されます。図1 のようにそれぞれの点の座標を抽出するにはnamed tupleから特定した配列データを取り出す必要がります。 図１ 21点ハンドランドマーク

　TCP/IPのSOCKET通信を用いて、ラジコン車のRaspberryPiとパソコンを繋ぎました。そして、ラジコン車のモーターとサーボーモーターの制御のためにPWM制御を行ないました。MediaPipeよりハンドトラッキングをし、手を空間上に動かした時のパターンをハンドルとアクセルとして設定しました。図2 に示すように手が前に倒す時に発進、後ろに倒した時に後退するように設定し、手が左右に回す時にラジコン車のハンドルが手の向きに切るように設定しました。

　図2 のアクセルの方の①は発進、②は停止そして③は後退のジェスチャーです。そしてハンドルの方の①は右方向、②は直進方向そして③は左方向です。ここでは、手の状況（手の角度と手首に対して中指先の深度）をTCP/IP通信より、ラジコン車のRaspberry Piに送信しました。Raspberry Piでは受信した信号より、モーターの制御とサーボーモーターの制御を行います。サーボーモーターの制御は手の角度の値と同じ角度になるよう制御しました。手の深度（デプス）情報は-3から1までの値を返されており、モーターの制御に必要なPWM信号は50Hzの5.0から7.0であったため、-3.0から1.0を5.0から6.0にmappingしました。そしてOPEN とCLOSEは鍵のON OFFと設定しました。

図2　ラジコン車の操作設定

結果

　Media Pipeからハンドハンドトラッキングし、遠隔操作がでできました。例えば、手が90度の時直進、90度以下は左と90度以上は右側にハンドルを切りました。そして、手のデプス値が-3.0の時最大速度で発進し、-1.0の時停止、1.0の時は最大速度で後退しました。実験の様子を図3 に示します。このように人間手をインターフェースとして活用することができました。

図３　ジェスチャーインターフェースの実験様子

方法・手順

手順は以下の通りです。

• MediaPipeより画像の中から手の関節の座標の検出

• 手のひらの中心の抽出

• 手の状況の識別

• 手の角度を求める

• RaspberryPiを用いて、

  • RCカーのモータ制御
  • RCカーのハンドル（サーボモーター）制御

• Socketを用いてPCとRaspberryPiの通信

• コードのまとめ

*MediaPipeのHandsを使っています。

インストール

Mediapipeが初めての方はここをクリックし、インストールを行なってください。 Python IDEでパッケージのインストールは以下のようにpipで行うことができます。パッケージはmediapipe、opencv、socket ```:コマンド又はターミナル上で pip install mediapipe pip install opencv-python pip install socket ```

MediaPipeより画像の中から手の関節の座標の検出

MediaPipe のHandsを用いて、以下の関数より手の関節の座標の検出を行います。

def take_coordinates(coordinates):
  if coordinates == None:
    return 0
  keypoints = []
  for data_point in coordinates:
    xyz_datapoints = data_point.landmark
    for xyz in xyz_datapoints:
      X_value = round(xyz.x*10000, 2)
      Y_value = round(xyz.y*10000, 2)
      Z_value = round(xyz.z, 3)
      xy = [X_value,Y_value, Z_value]
      keypoints.append(xy)
  return keypoints

この関数の引数は、results.multi_hand_landmarks です。

keypoints = take_coordinates(results.multi_hand_landmarks)

これによって取得した手の関節の座標を用いて手の角度
プログラムソースのまとめは最後に説明します。

手のひらの中心の抽出

　手のひらの中心点の抽出を行います。手のひらの中心点を求める関数は以下の通りです。

def centroid_palm(keypoints): 
    if keypoints == 0:
        return 0
    x_bar = (keypoints[0][0] + keypoints[9][0])/2
    x_bar = round(x_bar, 2)
    y_bar = (keypoints[0][1] + keypoints[9][1])/2
    y_bar = round(y_bar, 2)
    return x_bar, y_bar

手の状況の識別

　上記の関節座標の数値より以下のようにOPEN、CLOSE、手の角度、手が前倒しか後ろ倒しかの識別を行います。 　OPENとCLOSEの識別は手首の点からそれぞれの指の関節教理を比較し、識別を行います。関数は以下の通りです。

OPENの認識

def open_check_by_distance(keypoints, center):
    def thumb_open_check(keypoints, center):
        d4 = np.sqrt(np.square(keypoints[4][0] - center[0]) + np.square(keypoints[4][1] - center[1]))
        d3 = np.sqrt(np.square(keypoints[3][0] - center[0]) + np.square(keypoints[3][1] - center[1]))
        if d4 > d3:
            return True
        else:
            return False
    def index_open_check(keypoints, center):
        d5 = np.sqrt(np.square(keypoints[5][0] - center[0]) + np.square(keypoints[5][1] - center[1]))
        d6 = np.sqrt(np.square(keypoints[6][0] - center[0]) + np.square(keypoints[6][1] - center[1]))
        d7 = np.sqrt(np.square(keypoints[7][0] - center[0]) + np.square(keypoints[7][1] - center[1]))
        d8 = np.sqrt(np.square(keypoints[8][0] - center[0]) + np.square(keypoints[8][1] - center[1]))
        if d8 > d7 > d6 > d5:
            return True
        else:
            return False
    def middle_open_check(keypoints, center):
        d9 = np.sqrt(np.square(keypoints[9][0] - center[0]) + np.square(keypoints[9][1] - center[1]))
        d10 = np.sqrt(np.square(keypoints[10][0] - center[0]) + np.square(keypoints[10][1] - center[1]))
        d11 = np.sqrt(np.square(keypoints[11][0] - center[0]) + np.square(keypoints[11][1] - center[1]))
        d12 = np.sqrt(np.square(keypoints[12][0] - center[0]) + np.square(keypoints[12][1] - center[1]))
        if d12 > d11 > d10 > d9:
            return True
        else:
            return False
    def ring_open_check(keypoints, center):
        d13 = np.sqrt(np.square(keypoints[13][0] - center[0]) + np.square(keypoints[13][1] - center[1]))
        d14 = np.sqrt(np.square(keypoints[14][0] - center[0]) + np.square(keypoints[14][1] - center[1]))
        d15 = np.sqrt(np.square(keypoints[15][0] - center[0]) + np.square(keypoints[15][1] - center[1]))
        d16 = np.sqrt(np.square(keypoints[16][0] - center[0]) + np.square(keypoints[16][1] - center[1]))
        if d16 > d15 > d14 > d13:
            return True
        else:
            return False
    def pinky_open_check(keypoints, center):
        d17 = np.sqrt(np.square(keypoints[17][0] - center[0]) + np.square(keypoints[17][1] - center[1]))
        d18 = np.sqrt(np.square(keypoints[18][0] - center[0]) + np.square(keypoints[18][1] - center[1]))
        d19 = np.sqrt(np.square(keypoints[19][0] - center[0]) + np.square(keypoints[19][1] - center[1]))
        d20 = np.sqrt(np.square(keypoints[20][0] - center[0]) + np.square(keypoints[20][1] - center[1]))
        if d20 > d19 > d18 > d17:
            return True
        else:
            return False
    thumb = thumb_open_check(keypoints, center)
    index = index_open_check(keypoints, center)
    middle = middle_open_check(keypoints, center)
    ring = ring_open_check(keypoints, center)
    pinky = pinky_open_check(keypoints, center)
    if thumb == True and index == True and middle == True and ring == True and pinky == True:
        return True
    else:
        return False

CLOSEの識別

def close_check_by_distance(keypoints, center): #tested OK
   d3 = np.sqrt(np.square(keypoints[3][0] - center[0]) + np.square(keypoints[3][1] - center[1]))
   d4 = np.sqrt(np.square(keypoints[4][0] - center[0]) + np.square(keypoints[4][1] - center[1]))
   d5 = np.sqrt(np.square(keypoints[5][0] - keypoints[0][0]) + np.square(keypoints[5][1] - keypoints[0][1]))
   d8 = np.sqrt(np.square(keypoints[8][0] - keypoints[0][0]) + np.square(keypoints[8][1] - keypoints[0][1]))
   d9 = np.sqrt(np.square(keypoints[9][0] - keypoints[0][0]) + np.square(keypoints[9][1] - keypoints[0][1]))
   d12 = np.sqrt(np.square(keypoints[12][0] - keypoints[0][0]) + np.square(keypoints[12][1] - keypoints[0][1]))
   d13 = np.sqrt(np.square(keypoints[13][0] - keypoints[0][0]) + np.square(keypoints[13][1] - keypoints[0][1]))
   d16 = np.sqrt(np.square(keypoints[16][0] - keypoints[0][0]) + np.square(keypoints[16][1] - keypoints[0][1]))
   d17 = np.sqrt(np.square(keypoints[17][0] - keypoints[0][0]) + np.square(keypoints[17][1] - keypoints[0][1]))
   d20 = np.sqrt(np.square(keypoints[20][0] - keypoints[0][0]) + np.square(keypoints[20][1] - keypoints[0][1]))

   if d8 < d5 and d12 < d9 and d16 < d13 and d20 < d17 and d4 < d3:
       return True
   else:
       return False

手の角度を求める

手のひらの中心点と手首の点から手がどの方向に回っているかを識別します。 関数は以下の通りです。

def get_angle(keypoints, center):
    #(x',y')=(x, max-y)
    if keypoints == 0:
        return 0

    center = list(center)
    wrist = list(keypoints)
    wrist[1] = 10000-wrist[1] # y' = max - y
    center[1] = 10000-center[1] # y' = max - y
    Y = center[1]-wrist[1]
    X = center[0]-wrist[0]
    try:
        m = Y/X
    except ZeroDivisionError:
        m = 0
    angle = np.arctan(m)*180/(np.pi)
    if X > 0 and Y < 0:
        angle = angle + 360
    elif X < 0 and Y > 0:
        angle = angle + 180
    elif X < 0 and Y < 0:
        angle = angle + 180
    return round(angle, 1)

　手が前倒しか後ろ倒しかを求めるために、デプス情報を使います。MediaPipeでは、手首を基準点として他の関節のデプス情報が求められます。
　私は、中指の指先の点のz軸点から求めました。上記で求めた座標を参照。

Raspberry Piよりラジコンの操作

　ここではRaspberry Pi からの制御は省略します。手の状況をRaspberry Piに送信する方法だけ紹介します。[Socketを用いてPCとRaspberryPiの通信](#Socketを用いてPCとRaspberryPiの通信)

Socketを用いてPCとRaspberryPiの通信

私は以下のコードよりRaspberry PiとPCの接続をし、手の状況をRaspberry Piに送信しました。Socketの通信のためにsocket通信を参照してください。

1. PC

以下のコードをMediapipeのコードがあるフォルダーに保存します。

ファイル名は’SendToRaspPi’

import socket
from time import sleep

HEADER = 64
PORT = 5560
FORMAT = 'utf-8'
DISCONNECT_MESSAGE = "!DISCONNECT"
SERVER = 'raspberrypi.local'
ADDR = (SERVER, PORT)

client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client.connect(ADDR)
print("Connecting to raspberry pi")
def send(msg):
    message = msg.encode(FORMAT)
    msg_length = len(message)
    send_length = str(msg_length).encode(FORMAT)
    send_length += b' ' * (HEADER - len(send_length))
    client.send(send_length)
    client.send(message)
    sleep(0.05)

2. Raspberry Pi

このコードのMotorAndHandleは別の.pyファイルです。これよりモーターとサーボーモーターのGPIOより制御を行います。　

ファイル名は'socket_server'

import socket
import threading
import MotorAndHandle as mh
import servo_all

HEADER = 64
PORT = 5560
SERVER = '0.0.0.0'#socket.gethostbyname(socket.gethostname())
FORMAT = 'utf-8'
DISCONNECT_MESSAGE = "!DISCONNECT"
#print(SERVER)
ADDR = (SERVER, PORT)

server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.bind(ADDR)


def handle_client(conn, addr):
    print(f'new connection: {addr} connected.')

    connected = True
    while connected:
        msg_length = conn.recv(HEADER).decode(FORMAT)
        msg_length = int(msg_length)
        msg = conn.recv(msg_length).decode(FORMAT)
        #print(f"{addr}' {msg}")
        x = msg.split(', ')
        thumb = float(x[0])
        index = float(x[1])
        servo_all.hand(motor, handle)

        if msg == DISCONNECT_MESSAGE:
            conncted = False


def start():
    server.listen()
    print(f"LISTNING on {SERVER}")
    while True:
        conn, addr = server.accept()
        thread = threading.Thread(target=handle_client, args=(conn, addr))
        thread.start()
        print("ACTIVE CONNECTION {threading.activeCount() - 1}")


print("server is starting ........")
start()

MotorAndHandle.py

import RPi.GPIO as GPIO
from time import sleep

GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)

Handle = 18
Motor = 22

GPIO.setup(Handle, GPIO.OUT)
GPIO.setup(Motor, GPIO.OUT)
pwm_motor = GPIO.PWM(Motor, 50)
pwm_handle = GPIO.PWM(Handle, 50)

def angle_to_percent(angle):
    if angle > 180 or angle < 0:
        return False
    start = 4
    end = 10
    ratio = (end - start)/180

    angle_as_percent = angle * ratio
    return start + angle_as_percent

#motor stop at 5.8 to 6.3
#backward from 5.7 to 5.0 or less
#forward from 6.4 to 7.0 or more
def motor_speed(motor, handle_angle):
    pwm_motor.start(motor)
    handle(handle_angle)

def handle(handle_value):
    pwm_handle.start(angle_to_percent(handle_value))
    sleep(0.05)

コードのまとめ

import cv2
import mediapipe as mp
import time
import numpy as np
import SendToRaspPi as rp#SendToRaspi.py ファイルを同フォルダに保存する
mp_drawing = mp.solutions.drawing_utils
mp_hands = mp.solutions.hands
# For static images:
decide = True
count = 0
ptime = 0
close_check = False
open_check = False



def open_check_by_distance(keypoints, center):
    def thumb_open_check(keypoints, center):
        d4 = np.sqrt(np.square(keypoints[4][0] - center[0]) + np.square(keypoints[4][1] - center[1]))
        d3 = np.sqrt(np.square(keypoints[3][0] - center[0]) + np.square(keypoints[3][1] - center[1]))
        if d4 > d3:
            return True
        else:
            return False
    def index_open_check(keypoints, center):
        d5 = np.sqrt(np.square(keypoints[5][0] - center[0]) + np.square(keypoints[5][1] - center[1]))
        d6 = np.sqrt(np.square(keypoints[6][0] - center[0]) + np.square(keypoints[6][1] - center[1]))
        d7 = np.sqrt(np.square(keypoints[7][0] - center[0]) + np.square(keypoints[7][1] - center[1]))
        d8 = np.sqrt(np.square(keypoints[8][0] - center[0]) + np.square(keypoints[8][1] - center[1]))
        if d8 > d7 > d6 > d5:
            return True
        else:
            return False
    def middle_open_check(keypoints, center):
        d9 = np.sqrt(np.square(keypoints[9][0] - center[0]) + np.square(keypoints[9][1] - center[1]))
        d10 = np.sqrt(np.square(keypoints[10][0] - center[0]) + np.square(keypoints[10][1] - center[1]))
        d11 = np.sqrt(np.square(keypoints[11][0] - center[0]) + np.square(keypoints[11][1] - center[1]))
        d12 = np.sqrt(np.square(keypoints[12][0] - center[0]) + np.square(keypoints[12][1] - center[1]))
        if d12 > d11 > d10 > d9:
            return True
        else:
            return False
    def ring_open_check(keypoints, center):
        d13 = np.sqrt(np.square(keypoints[13][0] - center[0]) + np.square(keypoints[13][1] - center[1]))
        d14 = np.sqrt(np.square(keypoints[14][0] - center[0]) + np.square(keypoints[14][1] - center[1]))
        d15 = np.sqrt(np.square(keypoints[15][0] - center[0]) + np.square(keypoints[15][1] - center[1]))
        d16 = np.sqrt(np.square(keypoints[16][0] - center[0]) + np.square(keypoints[16][1] - center[1]))
        if d16 > d15 > d14 > d13:
            return True
        else:
            return False
    def pinky_open_check(keypoints, center):
        d17 = np.sqrt(np.square(keypoints[17][0] - center[0]) + np.square(keypoints[17][1] - center[1]))
        d18 = np.sqrt(np.square(keypoints[18][0] - center[0]) + np.square(keypoints[18][1] - center[1]))
        d19 = np.sqrt(np.square(keypoints[19][0] - center[0]) + np.square(keypoints[19][1] - center[1]))
        d20 = np.sqrt(np.square(keypoints[20][0] - center[0]) + np.square(keypoints[20][1] - center[1]))
        if d20 > d19 > d18 > d17:
            return True
        else:
            return False
    thumb = thumb_open_check(keypoints, center)
    index = index_open_check(keypoints, center)
    middle = middle_open_check(keypoints, center)
    ring = ring_open_check(keypoints, center)
    pinky = pinky_open_check(keypoints, center)
    if thumb == True and index == True and middle == True and ring == True and pinky == True:
        return True
    else:
        return False

def close_check_by_distance(keypoints, center): #tested OK
   d3 = np.sqrt(np.square(keypoints[3][0] - center[0]) + np.square(keypoints[3][1] - center[1]))
   d4 = np.sqrt(np.square(keypoints[4][0] - center[0]) + np.square(keypoints[4][1] - center[1]))
   d5 = np.sqrt(np.square(keypoints[5][0] - keypoints[0][0]) + np.square(keypoints[5][1] - keypoints[0][1]))
   d8 = np.sqrt(np.square(keypoints[8][0] - keypoints[0][0]) + np.square(keypoints[8][1] - keypoints[0][1]))
   d9 = np.sqrt(np.square(keypoints[9][0] - keypoints[0][0]) + np.square(keypoints[9][1] - keypoints[0][1]))
   d12 = np.sqrt(np.square(keypoints[12][0] - keypoints[0][0]) + np.square(keypoints[12][1] - keypoints[0][1]))
   d13 = np.sqrt(np.square(keypoints[13][0] - keypoints[0][0]) + np.square(keypoints[13][1] - keypoints[0][1]))
   d16 = np.sqrt(np.square(keypoints[16][0] - keypoints[0][0]) + np.square(keypoints[16][1] - keypoints[0][1]))
   d17 = np.sqrt(np.square(keypoints[17][0] - keypoints[0][0]) + np.square(keypoints[17][1] - keypoints[0][1]))
   d20 = np.sqrt(np.square(keypoints[20][0] - keypoints[0][0]) + np.square(keypoints[20][1] - keypoints[0][1]))

   if d8 < d5 and d12 < d9 and d16 < d13 and d20 < d17 and d4 < d3:
       return True
   else:
       return False

def take_coordinates(coordinates):
  if coordinates == None:
    return 0
  keypoints = []
  for data_point in coordinates:
    xyz_datapoints = data_point.landmark
    for xyz in xyz_datapoints:
      X_value = round(xyz.x*10000, 2)
      Y_value = round(xyz.y*10000, 2)
      Z_value = round(xyz.z, 3)
      xy = [X_value,Y_value, Z_value]
      keypoints.append(xy)
  return keypoints

def centroid_palm(keypoints): #calculation not correct. Do it again
    if keypoints == 0:
        return 0
    x_bar = (keypoints[0][0] + keypoints[9][0])/2
    x_bar = round(x_bar, 2)
    y_bar = (keypoints[0][1] + keypoints[9][1])/2
    y_bar = round(y_bar, 2)
    return x_bar, y_bar

def get_angle(keypoints, center):
    #(x',y')=(x, max-y)
    if keypoints == 0:
        return 0

    center = list(center)
    wrist = list(keypoints)
    wrist[1] = 10000-wrist[1] # y' = max - y
    center[1] = 10000-center[1] # y' = max - y
    Y = center[1]-wrist[1]
    X = center[0]-wrist[0]
    try:
        m = Y/X
    except ZeroDivisionError:
        m = 0
    angle = np.arctan(m)*180/(np.pi)
    if X > 0 and Y < 0:
        angle = angle + 360
    elif X < 0 and Y > 0:
        angle = angle + 180
    elif X < 0 and Y < 0:
        angle = angle + 180
    return round(angle, 1)

def motor(value):#モーター制御のために必要な値にmappingを行う。モーターによって値が変わる
    leftMin = -0.3
    leftMax = 0.1
    rightMin = 6.5
    rightMax = 5.5

    # Figure out how 'wide' each range is
    leftSpan = leftMax - leftMin
    rightSpan = rightMax - rightMin

    # Convert the left range into a 0-1 range (float)
    valueScaled = float(value - leftMin) / float(leftSpan)

    # Convert the 0-1 range into a value in the right range.
    return rightMin + (valueScaled * rightSpan)

def calculate(keypoints):
    global decide
    global close_check
    global open_check

    if keypoints == 0:
        rp.send(f"{0}, {90}")
        return 0

    # ひらの中心を求める
    center = centroid_palm(keypoints)
    #手の傾きの検出
    angle = get_angle(keypoints[0], center)
    # ラジコン車制御するためのPWM値を検出
    motor_value = round(motor(keypoints[12][2]), 1)
    #手がopenであることの確認
    open_check = open_check_by_distance(keypoints, center)
    #openならば、RCカーがエンジンON（仮定）
    if open_check == True:
        # PCからRaspberry Piへモーターとサーボーの制御をするために送信を行う。
        rp.send(f"{motor_value}, {int(angle)}")
        print(f"sending{motor_value}, {angle}")
    # closeならば、RCカーがエンジンOFF（仮定）
    elif close_check:
        rp.send(f"{6.0}, {90}")
    return angle



# For webcam input:
pTime = 0
with mp_hands.Hands(
    min_detection_confidence=0.8,
    min_tracking_confidence=0.5) as hands:
  while cap.isOpened():
    success, image = cap.read()
    if not success:
      print("Ignoring empty camera frame.")
      # If loading a video, use 'break' instead of 'continue'.
      continue
    #FPSの計算の為
    cTime = time.time()
    fps = 1 / (cTime - pTime)
    pTime = cTime


    # Flip the image horizontally for a later selfie-view display, and convert
    # the BGR image to RGB.
    image = cv2.cvtColor(cv2.flip(image, 1), cv2.COLOR_BGR2RGB)
    cv2.putText(image, f'FPS: {int(fps)}', (800, 720), cv2.FONT_HERSHEY_PLAIN, 3, (255, 0, 0), 3)

    # To improve performance, optionally mark the image as not writeable to
    # pass by reference.
    image.flags.writeable = False
    results = hands.process(image)
    keypoints = take_coordinates(results.multi_hand_landmarks)
    if keypoints != 0:
        place = (int((keypoints[12][0]) / 10), int((keypoints[12][1]) / 15))
        cv2.putText(image, f'{float(get_angle(keypoints[0], centroid_palm(keypoints)))}', place, cv2.FONT_HERSHEY_PLAIN,
                    3, (255, 0, 0), 3)
    if keypoints == 0:
        place = (200, 200)

    # この関数が全ての数値を計算してる
    calculate(keypoints)

    # Draw the hand annotations on the image.
    image.flags.writeable = True
    image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)


    if results.multi_hand_landmarks:
      for hand_landmarks in results.multi_hand_landmarks:
        mp_drawing.draw_landmarks(
            image, hand_landmarks, mp_hands.HAND_CONNECTIONS)
    cv2.imshow('MediaPipe Hands', image)
    if cv2.waitKey(5) & 0xFF == 27:
      break
cap.release()

参考文献

• mediapipe last accesed on may, 2022