[PYTHON] Follow the AR marker with a 2-axis servo

This time, when I made a demo of the system to follow the teaching materials, I was forced to follow the AR marker, so a memo for myself at that time.

Implementation flow

• Recognize AR markers
• Get the center coordinates of the AR marker
• Control the servo motor based on the acquired coordinates

With OpneCV's arconv, you can identify AR markers and take their coordinates and angles from images. [^ 1]

For the time being, generate an AR marker.

make.py

#/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import sys
#aruco library
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():

If you run this file, you should have 30 AR marker images.

This time we will use ar1.png.

Next, let's look at the actual program. First, the image is acquired, and the frame size is acquired and resized.

main.py

while True:
            #Get images from video capture
            ret, frame = cap.read()
            #Get size
            Height, Width = frame.shape[:2]
            # resize
            img = cv2.resize(frame,(int(Width),int(Height)))

Next, the marker is detected and identified, and the center coordinates are obtained.

main.py

            #Detect markers
            corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) 

            if len(corners) > 0:
                #print(ids[0])
                #When marker 01 is detected
                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]
                    #Specify the coordinate point you want to match
                    markerrect = [[x,y-40]]
                    print(markerrect)
                    for rect in markerrect:
                        img_x = rect[0]
                        img_y = rect[1]

At that time, the moving coordinates are calculated. It is necessary to change the value depending on the angle at which the servo is installed. Adjust as appropriate. In addition, since it was necessary to limit the upward movement, in order to prevent the operation range from being exceeded, the value was continuously substituted when the y-coordinate exceeded the specified value.

main.py

                        #Moving coordinate set
                        move_degree_x = now_degree_x - (img_x-160)*0.06
                        move_degree_y = now_degree_y - (img_y-50)*0.06
                        #Prevention of exceeding the movable range
                        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))
                        #Post-move coordinate set
                        now_degree_x = move_degree_x
                        now_degree_y = move_degree_y

Adafruit_PCA9685 was used to control the servo motor. [^ 2] With this, a plurality of servo motors can be controlled.

The final result is as follows.

main.py

#/usr/bin/env python3
# -*- coding: utf-8 -*-

import cv2
import numpy as np
import time
import Adafruit_PCA9685
#aruco library
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)

        #Start video capture
        cap = cv2.VideoCapture(0) 
        cap.set(3, 320)
        cap.set(4, 240)
        color = (255, 255, 255)

        #Current servo state set
        now_degree_x, now_degree_y, move_degree_x, move_degree_y = 450, 600, 0, 0
        while True:
            #Get images from video capture
            ret, frame = cap.read()
            #Get size
            Height, Width = frame.shape[:2]
            # resize
            img = cv2.resize(frame,(int(Width),int(Height)))
            #Detect marker
            corners, ids, rejectedImgPoints = aruco.detectMarkers(img, dictionary) 

            if len(corners) > 0:
                #print(ids[0])
                #When marker 01 is detected
                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]
                    #Specify the coordinate point you want to match
                    markerrect = [[x,y-40]]
                    print(markerrect)
                    for rect in markerrect:
                        img_x = rect[0]
                        img_y = rect[1]
                        #print('img: ',img_x, img_y)
                        #Moving coordinate set
                        move_degree_x = now_degree_x - (img_x-160)*0.06
                        move_degree_y = now_degree_y - (img_y-50)*0.06
                        #Prevention of exceeding the movable range
                        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))
                        #Post-move coordinate set
                        now_degree_x = move_degree_x
                        now_degree_y = move_degree_y

        #End processing
        cap.release()
        cv2.destroyAllWindows()
        

[^ 1]: "[Programming with Python] I tried to recognize the AR marker using the aruco library" http://okatenari.com/2017/11/28/python-ar/