ARマーカーを2軸サーボで追従する

今回、教材を追従させるシステムのデモを作る際に、ARマーカーを追従させる必要に迫られたので、その時の自分用メモ。

## 実装の流れ
* ARマーカーを認識する
* ARマーカーの中心座標を取得
* 取得した座標を元にサーボモータを制御

OpneCVのarconvを使えば、ARマーカを識別したり、その座標や角度などを画像からとることができる。[^1]

ひとまず、ARマーカを生成。

```python:make.py
#/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import sys
#arucoライブラリ
aruco = cv2.aruco 
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

def make():
    for i in range(30):
        generator = aruco.drowMarker(dictionary, i, 100)
        cv2.imwrite('ar' + str(i) + 'png', generator)

if __name__ == '__main__';
    make():
```
このファイルを実行すれば30枚のARマーカー画像ができているはずだ。
<img width="548" alt="スクリーンショット 2019-07-27 14.01.53.PNG" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/110060/5d5a614f-5c90-590c-e749-94bf60708c36.png">

今回はar1.pngを使用する。
<img width="104" alt="スクリーンショット 2019-07-27 14.01.53.jpg" src="https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/110060/583757b5-ec63-8531-6709-6fdd001eeea2.jpeg">




次に実際のプログラムを見ていく。
まず、画像を取得し、そのフレームサイズを取得、リサイズする。

```python:main.py
while True:
            #ビデオキャプチャから画像を取得
            ret, frame = cap.read()
            #sizeを取得
            Height, Width = frame.shape[:2]
            # resize
            img = cv2.resize(frame,(int(Width),int(Height)))
```

次にマーカーを検出、識別し、中心座標を取得。

```python:main.py
            #マーカーを検出
            corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) 

            if len(corners) > 0:
                #print(ids[0])
                #マーカー01検出時
                if ids[0] == 1:
                    square_points = np.reshape(np.array(corners), (4, -1))
                    G = np.mean(square_points, axis = 0)
                    x = G[0]
                    y = G[1]
                    #合わせたい座標ポイント指定
                    markerrect = [[x,y-40]]
                    print(markerrect)
                    for rect in markerrect:
                        img_x = rect[0]
                        img_y = rect[1]
```

また、その際に移動座標を計算する。
サーボを設置している角度によって数値を変える必要がある。適宜調節すること。
また、上を向く動作を制限させる必要があったので、稼働範囲を超えることを防止するため、y座標が指定数値を超えた時点でその数値を代入し続けるようにした。

```python:main.py
                        #移動座標セット
                        move_degree_x = now_degree_x - (img_x-160)*0.06
                        move_degree_y = now_degree_y - (img_y-50)*0.06
                        #可動範囲越え防止
                        if move_degree_y > 650:
                            move_degree_y = 650
                        print('deg: ', move_degree_x , move_degree_y)
                        pwm.set_pwm(15, 0, int(move_degree_x))
                        pwm.set_pwm(14, 0, int(move_degree_y))
                        #移動後座標セット
                        now_degree_x = move_degree_x
                        now_degree_y = move_degree_y
```

サーボモータの制御にはAdafruit_PCA9685を用いた。[^2]
これで複数のサーボモータを制御できる。

最終的には以下のようになる。

```python:main.py
#/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import numpy as np
import time
import Adafruit_PCA9685
#arucoライブラリ
aruco = cv2.aruco 
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)

class MarkerCtl():

    def arReader(self,Sb):
        # initial   
        pwm = Adafruit_PCA9685.PCA9685()
        pwm.set_pwm_freq(60)
        pwm.set_pwm(14, 0, 600)
        time.sleep(1)
        pwm.set_pwm(15, 0, 450)
        time.sleep(1)

        #ビデオキャプチャの開始
        cap = cv2.VideoCapture(0) 
        cap.set(3, 320)
        cap.set(4, 240)
        color = (255, 255, 255)

        #現在のサーボ状態セット
        now_degree_x, now_degree_y, move_degree_x, move_degree_y = 450, 600, 0, 0
        while True:
            #ビデオキャプチャから画像を取得
            ret, frame = cap.read()
            #sizeを取得
            Height, Width = frame.shape[:2]
            # resize
            img = cv2.resize(frame,(int(Width),int(Height)))
            #マーカを検出
            corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) 

            if len(corners) > 0:
                #print(ids[0])
                #マーカ01検出時
                if ids[0] == 1:
                    square_points = np.reshape(np.array(corners), (4, -1))
                    G = np.mean(square_points, axis = 0)
                    x = G[0]
                    y = G[1]
                    #合わせたい座標ポイント指定
                    markerrect = [[x,y-40]]
                    print(markerrect)
                    for rect in markerrect:
                        img_x = rect[0]
                        img_y = rect[1]
                        #print('img: ',img_x, img_y)
                        #移動座標セット
                        move_degree_x = now_degree_x - (img_x-160)*0.06
                        move_degree_y = now_degree_y - (img_y-50)*0.06
                        #可動範囲越え防止
                        if move_degree_y > 650:
                            move_degree_y = 650
                        print('deg: ', move_degree_x , move_degree_y)
                        pwm.set_pwm(15, 0, int(move_degree_x))
                        pwm.set_pwm(14, 0, int(move_degree_y))
                        #移動後座標セット
                        now_degree_x = move_degree_x
                        now_degree_y = move_degree_y

        #終了処理
        cap.release()
        cv2.destroyAllWindows()
        
```


[^1]: 「【Pythonでプログラミング】arucoライブラリを使ってARマーカを認識してみた」 http://okatenari.com/2017/11/28/python-ar/ 
[^2]: 「Adafruit PCA9685 16-Channel Servo Driver」 https://learn.adafruit.com/16-channel-pwm-servo-driver/python-circuitpython