先日、多関節2足歩行ロボットを作りました。↓
このロボットはカメラが使えるようにしているのですが、今の所はWEＢブラウザで操作するときにロボット目線の映像がモニタに表示されるだけとなっています。
これは少し勿体ないですので、今回はカメラを使った「物体追尾」の機能を追加したいと思います。
過去にRaspberryPiでOpenCvを使い、カメラが物体追尾するものを作成した事があるので、それを応用します。↓
とりあえずカラートラッキングと顔認証で、ロボットの顔を追尾させる機能を追加してみます。
また、M5StickCのimuと連動させて、画像を制御させてみたいと思います。
カラートラッキング
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 |
import cv2 import numpy as np #import PCA9685 import Adafruit_PCA9685 import math import time #まずは cv2.VideoCapture() で VideoCapture オブジェクトを取得します。 引数にはコンピュータに接続されているカメラの番号を指定しています。 cap= cv2.VideoCapture(0) x_medium = 0 y_medium = 0 #PCA9685 = PCA9685.PCA9685 pwm = Adafruit_PCA9685.PCA9685() pwm.set_pwm_freq(60) X_MAX = 540 X_MIN = 300 X_HOME = 420 Y_MAX = 600 Y_MIN = 150 Y_HOME = 375 #幅 W = cap.get(cv2.CAP_PROP_FRAME_WIDTH) #高さ H = cap.get(cv2.CAP_PROP_FRAME_HEIGHT) #フレームレート fps = cap.get(cv2.CAP_PROP_FPS) print('W,H,fps: ', W , H , fps) #サーボ駆動関数 def move(x_move, y_move): pwm.set_pwm(11, 0, x_move) #pwm.set_pwm(1, 0, y_move) def movex(x_move): pwm.set_pwm(11, 0, x_move) #def movey(y_move): # pwm.set_pwm(1, 0, y_move) #サーボ初期位置 move(X_HOME, Y_HOME) #初期位置？サーボ座標？？ now_degree_x, now_degree_y, move_degree_x, move_degree_y = X_HOME, Y_HOME, 0, 0 while True: #read()メソッドの返り値はフレームの画像が読み込めたかどうかを示すbool値と画 像の配列ndarrayのタプル。アンパックでそれぞれの変数に代入できる。 _, frame = cap.read() #cv2.cvtColorメソッドでRGBからHSVに変換できます。 hsv_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV) # red color low_red = np.array([161, 155, 84]) high_red = np.array([179, 255, 255]) red_mask = cv2.inRange(hsv_frame, low_red, high_red) contours, _ = cv2.findContours(red_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) contours = sorted(contours, key=lambda x:cv2.contourArea(x), reverse=True) for cnt in contours: (x, y, w, h) = cv2.boundingRect(cnt) #cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2) x_medium = int((x + x + w) / 2) y_medium = int((x + y + h) / 2) #640,480 move_degree_x = now_degree_x - (x_medium-320)*0.1 move_degree_y = now_degree_y - (y_medium-240)*0.1 print((x_medium-320)*0.3) if move_degree_x > X_MIN and move_degree_x < X_MAX: move_degree_x = math.floor(move_degree_x) movex(move_degree_x) now_degree_x = move_degree_x #time.sleep(0.1) #if move_degree_y > Y_MIN and move_degree_y < Y_MAX: # #move_degree_x = math.floor(move_degree_x) # move_degree_y = math.floor(move_degree_y) # movey(move_degree_y) #now_degree_x = move_degree_x # now_degree_y = move_degree_y #print(x_medium,y_medium) break cv2.line(frame, (x_medium, 0), (x_medium, 480), (0, 255, 0), 2) cv2.line(frame, (0, y_medium), (640, y_medium), (0, 255, 0), 2) cv2.imshow("Frame", frame) cv2.imshow("mask", red_mask) key = cv2.waitKey(1) if key == 27: break cap.release() cv2.destoyALLWindows() |
ロボットの顔が左右のX方向のみの可動となってますので、Y軸方向のプログラムはコメントアウトさせました。
少し反応が悪いのでプログラムの修正が必要ですが、一応こんな感じで、プログラム起動時に認識させた物体（色）に反応し、追尾してくれます。↓
顔認証
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 |
import numpy as np import cv2 import time import Adafruit_PCA9685 import math pwm = Adafruit_PCA9685.PCA9685() pwm.set_pwm_freq(60) faceCascade = cv2.CascadeClassifier('/home/pi/opencv/data/haarcascades/haarcascade_frontalface_default.xml') def move(x_move): pwm.set_pwm(11, 0, x_move) #pwm.set_pwm(1, 0, y_move) #X,Yのサーボで稼働させる範囲。適当によさげな範囲を指定。 X_MIN = 300 X_HOME = 420 X_MAX = 540 Y_MIN = 300 Y_HOME = 420 Y_MAX = 540 #カメラの位置初期化 move(X_HOME) #カメラ準備 cap = cv2.VideoCapture(0) # デフォルト画面サイズ横幅 #W = cap.get(cv2.CAP_PROP_FRAME_WIDTH) cap.set(3,320) # 横幅を設定 cap.set(4,320) # 縦幅を設定 #初期位置 now_degree_x, now_degree_y, move_degree_x, move_degree_y = X_HOME, Y_HOME, 0, 0 while(True): #画面取得 ret, img = cap.read() # 顔検出の負荷軽減のために、キャプチャした画像をモノクロにする gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) # 顔検出のパラメータの設定 faces = faceCascade.detectMultiScale( gray, scaleFactor=1.2, minNeighbors=5, minSize=(20, 20) ) for (x,y,w,h) in faces: cv2.rectangle(img,(x,y),(x+w,y+h),(255,0,0),2) roi_gray = gray[y:y+h, x:x+w] roi_color = img[y:y+h, x:x+w] img_x = int(x+w/2) img_y = int(y+h/2) move_degree_x = now_degree_x - (img_x-160)*0.2 move_degree_y = now_degree_y + (img_y-160)*0.1 move_degree_x = math.floor(move_degree_x) move_degree_y = math.floor(move_degree_y) #実際にカメラを動かすところ #move(move_degree_x) if move_degree_x > X_MIN and move_degree_x < X_MAX: move(move_degree_x) now_degree_x = move_degree_x now_degree_y = move_degree_y #画面への描写 cv2.imshow('video', img) k = cv2.waitKey(1) if k == ord('q'): break #else: #break cap.release() cv2.destroyAllWindows() |
同じくY軸方向のプログラムはコメントアウトしました。
こちらも反応が悪く修正が必要ですが、一応カメラに映った顔に反応し、追従してくれます。↓
6軸センサで画像制御
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 |
import cv2 import time from socket import socket, AF_INET, SOCK_DGRAM #import network img = cv2.imread("11508001_1_d2 (1).jpg") img2 = cv2.imread("11508_1_9_8b.png") img3 = cv2.imread("p1f82kyi.jpg") #wlan = network.WLAN(network.STA_IF) #wlan.active(True) #if not wlan.isconnected(): # print('connecting to network...') # wlan.connect('SSID', 'PASS') # while not wlan.isconnected(): # pass # print('network config:', wlan.ifconfig()) HOST = "IP" PORT = ポート番号 s = socket(AF_INET, SOCK_DGRAM) s.bind((HOST, PORT)) while(True): msg, address = s.recvfrom(8192) #print(f"message: {msg}\nfrom: {address}") msg = msg.decode() print(msg) if msg =="f": cv2.imshow("Image", img) cv2.waitKey(1) elif msg == "b": cv2.imshow("Image", img2) cv2.waitKey(1) elif msg == "c": cv2.imshow("Image", img3) cv2.waitKey(1) #time.sleep(0.1) s.close() |
音とパンチラのタイミングにこだわりました。
