Lチカで始めるテスト自動化(10)6DoFロボットアームの制御スクリプトの保守性向上

##1. はじめに
Lチカで始めるテスト自動化シリーズ第十弾です。

[第九弾](https://qiita.com/pbjpkas/items/7a4cb4c59886b1fd2cf9)で組込んだロボットアームの制御スクリプトを簡潔に記述できるようテストランナーを改修します。
併せて、テストスクリプトや結果ファイルのファイル名が決め打ちだったのを引数で指定できるようにします。

これまでの記事はこちらをご覧ください。

1. [Lチカで始めるテスト自動化](https://qiita.com/pbjpkas/items/fd4bd2689deda2d793aa)
2. [Lチカで始めるテスト自動化(2)テストスクリプトの保守性向上](https://qiita.com/pbjpkas/items/337bce392281ee5f0526)
3. [Lチカで始めるテスト自動化(3)オシロスコープの組込み](https://qiita.com/pbjpkas/items/b80b7e199a5aed4688a4)
4. [Lチカで始めるテスト自動化(4)テストスクリプトの保守性向上(2)](https://qiita.com/pbjpkas/items/a02c4128baf5a3f2a46c)
5. [Lチカで始めるテスト自動化(5)WebカメラおよびOCRの組込み](https://qiita.com/pbjpkas/items/9af9e6441e1e20c5909e)
6. [Lチカで始めるテスト自動化(6)AI(機械学習)を用いたPass/Fail判定](https://qiita.com/pbjpkas/items/405d55f809da1221b867)
7. [Lチカで始めるテスト自動化(7)タイムスタンプの保存](https://qiita.com/pbjpkas/items/8e3c4330f4a81d30bd53)
8. [Lチカで始めるテスト自動化(8)HDMIビデオキャプチャデバイスの組込み](https://qiita.com/pbjpkas/items/58f84c347e44fb9d5635)
9. [Lチカで始めるテスト自動化(9)6DoFロボットアームの組込み](https://qiita.com/pbjpkas/items/7a4cb4c59886b1fd2cf9)

##2. Arduinoファームウェアの改修
以下に[Arduino6DoFArm.ino](https://qiita.com/pbjpkas/items/7a4cb4c59886b1fd2cf9#42-ソース)の改修箇所を示します。
cmd_rx_data()関数内で、echobackがoffのときに改行コードを送っていたのを送らないようにします。

```cpp:Arduino6DoFArm.ino
void cmd_rx_data(void)
{
（略）
            else if( (buf[i] == '\r') or (buf[i] == '\n') )
            {
                if(echoback) Serial1.print( F("\r\n") );     // <---- echobackがoffのときは改行コードを送らないようにする
                buf[i] = '\0';
                return_val = cmd_execute(&echoback, &buf[0]);
                for(i=0; i<CMD_BUF_LENGTH; i++) buf[i] = '\0';
                i=0;

                if(return_val == ERR_OK)
                {
                    Serial1.print(F("OK\r\n"));
                }
            }
（略）
}
```

##3. テストランナーの改修
###3.1 ロボットアームの制御スクリプト改善
set_servoコマンドでサーボのIDと設定したい角度を与えると、自動でArduino治具へservoreadコマンドを発行して現在のサーボの角度を取得し、目標の角度になるまで1°ずつ値を変えながらservoコマンドを所定の間隔で発行します（所定の間隔はひとまず0.2秒としています）。
NUM_OF_SERVOはサーボの個数を指定します。

|コマンド |引数             |機能
|---------+-----------------+----
|set_servo|\<id\> \<rotate angle\>|サーボの回転角度制御、id:0～5、rotate angle:0～180

```python:test-runnner.py
（略）
NUM_OF_SERVO = 6
（略）
def set_servo(uart, servo_id, servo_pos):
    if servo_id < 0 or servo_id >= NUM_OF_SERVO:
        print("Invalid Servo ID")
        return False
    
    if servo_pos < 0 or servo_pos > 180:
        print("Invalid Servo Position")
        return False
    
    if uart != UNINITIALIZED:
        serial_write(uart, "servoread")
        
        servo_position = uart.readline().strip().decode('utf-8')
        print(servo_position)
        
        # discard "OK"
        devnull = uart.readline().strip().decode('utf-8')
        
        current_pos = int(servo_position.split(' ')[servo_id])
        #print(servo_id, current_pos)
        
        if current_pos < servo_pos:
            start = current_pos +1
            stop  = servo_pos +1
            step  = 1
        else:
            start = current_pos -1
            stop  = servo_pos -1
            step  = -1
            
        for i in range(start, stop, step):
            command = "servo " + str(servo_id) + " " + str(i)
            print(command)
            serial_write(uart, command)
            # discard "OK"
            devnull = uart.readline().strip().decode('utf-8')
            sleep(0.2) # sec.
            
        return True
    else:
        print("UART Not Initialized.")
        return False

def main():
（略）
                elif cmd[0] == "set_servo":
                    ret = set_servo(uart, int(cmd[1]), int(cmd[2]))
                    if ret == False:
                        is_passed = False
（略）
```

###3.2 テストスクリプトや結果ファイルのファイル名を引数で指定できるようにする
sys.argvの引数が2個のときはテストスクリプトや結果ファイルのファイル名を以下のルールで設定します。
テストスクリプト：引数.csv
結果ファイル：引数_result.csv

それ以外の時は従来通りそれぞれscript.csvおよびresult.csvとします。

```python:test-runnner.py
（略）
def main():
（略）
    if len(sys.argv) == 2:
        script_file_name = sys.argv[1] + ".csv"
        result_file_name = sys.argv[1] + "_result.csv"
    else:
        script_file_name = "script.csv"
        result_file_name = "result.csv"

    with codecs.open(script_file_name, 'r', 'utf-8') as file:
        script = csv.reader(file, delimiter=',', lineterminator='\r\n', quotechar='"')

        with codecs.open(result_file_name, 'w', 'utf-8') as file:
            result = csv.writer(file, delimiter=',', lineterminator='\r\n', quotechar='"')
（略）
```

##4. テストスクリプト
###4.1 記述のポイント
Arduino治具へsendコマンドで"echo 0"を発行した後、治具の応答を読み捨てるためrcvdコマンドを3回発行します。その後、ロボットアームの制御コマンドを記述します。

```sh.script180.csv
#
# Arduino Leonardo, dsrdtr=0
open_uart,COM8,0
#
# echo off and discard response
send,echo 0
rcvd,
rcvd,
rcvd,
#
# set_servo
set_servo,5,170
```

###4.2 実行例(1)
引数として"script180"を、set_servoコマンドでサーボID=5のサーボを180°に設定する例を以下に示します。

servoreadコマンドをArduino治具へ発行してサーボID=5の現在の角度が170°であることを取得し、180°になるまで1°ずつ値を変えながらservoコマンドを発行します。

```sh
>python test-runner.py script180
['#']
['2020/08/20 00:20:32', '#', 'OK']
['# Arduino Leonardo', ' dsrdtr=0']
['2020/08/20 00:20:32', '# Arduino Leonardo', ' dsrdtr=0', 'OK']
['open_uart', 'COM8', '0']
['2020/08/20 00:20:32', 'open_uart', 'COM8', '0', 'OK']
['#']
['2020/08/20 00:20:32', '#', 'OK']
['# echo off']
['2020/08/20 00:20:32', '# echo off', 'OK']
['send', 'echo 0']
['2020/08/20 00:20:32', 'send', 'echo 0', 'OK']
['rcvd', '']
['2020/08/20 00:20:32', 'rcvd', '', 'echo 0', 'OK']
['rcvd', '']
['2020/08/20 00:20:32', 'rcvd', '', '', 'OK']
['rcvd', '']
['2020/08/20 00:20:32', 'rcvd', '', 'OK', 'OK']
['#']
['2020/08/20 00:20:32', '#', 'OK']
['# set_servo']
['2020/08/20 00:20:32', '# set_servo', 'OK']
['set_servo', '5', ' 180']
85 90 90 90 90 170
servo 5 171
servo 5 172
servo 5 173
servo 5 174
servo 5 175
servo 5 176
servo 5 177
servo 5 178
servo 5 179
servo 5 180
['2020/08/20 00:20:32', 'set_servo', '5', ' 180', 'OK']
PASS
```
###4.3 実行例(2)
こちらの動画（消しゴムをつかんで移動し、元の場所に戻します）のテストスクリプトを以下に示します。
角度を1°ずつ変えながらservoコマンドを発行する間隔を0.2秒ではなく0.025秒に変えています。

<blockquote class="twitter-tweet"><p lang="ja" dir="ltr">第9弾で61行あったサーボ制御のスクリプトが第10弾で22行に減って動きも滑らかに。<br>記事で0.2秒になっているインターバルは0.025秒に変更しています。 <a href="https://t.co/lYBbVf3XPM">pic.twitter.com/lYBbVf3XPM</a></p>&mdash; ka’s (@pbjpkas) <a href="https://twitter.com/pbjpkas/status/1297083063340474368?ref_src=twsrc%5Etfw">August 22, 2020</a></blockquote> <script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

```sh:script.csv
#
# Arduino Leonardo, dsrdtr=0
open_uart,COM8,0
#
# echo off and discard response
send,echo 0
rcvd,
rcvd,
rcvd,
#
# set_servo
set_servo,5,140
set_servo,1,95
set_servo,3,60
set_servo,2,110
set_servo,1,87
set_servo,5,170
set_servo,1,90
set_servo,0,70
set_servo,1,87
set_servo,5,140
set_servo,1,90
sleep,1
set_servo,1,87
set_servo,5,170
set_servo,1,90
set_servo,0,90
set_servo,1,87
set_servo,5,140
set_servo,1,90
set_servo,2,90
set_servo,3,90
set_servo,5,170
```

##5. おわりに
- [servoコマンドとsleepコマンドを駆使してサーボを制御していた](https://qiita.com/pbjpkas/items/7a4cb4c59886b1fd2cf9#5-テストスクリプト)のと比べると保守性を向上できました。
- また、1°ずつサーボを動かすことで動きが滑らかになりました。
- テストスクリプトに任意のファイル名を付けられるようになり、テストスクリプトの再利用がしやすくなったように思います。


##A. 付録 test-runner.pyのソース
```python:test-runner.py
#!/usr/bin/python3

#
# This software includes the work that is distributed
# in the Apache License 2.0
#

from time import sleep
import sys
import codecs
import csv
import datetime
import serial
import visa
import cv2
from PIL import Image
import pyocr
import pyocr.builders
import platform
import subprocess

UNINITIALIZED = 0xdeadbeef
NUM_OF_SERVO = 6

def serial_write(h, string):
    if h != UNINITIALIZED:
        string = string + '\n'
        string = str.encode(string)
        h.write(string)
        return True
    else:
        print("UART Not Initialized.")
        return False

def close_uart(h):
    if h != UNINITIALIZED:
        h.close()
    else:
        #print("UART Not Initialized.")
        pass

def open_dso():
    rm = visa.ResourceManager()
    resources = rm.list_resources()
    #print(resources)
    for resource in resources:
        #print(resource)
        try:
            dso = rm.open_resource(resource)
        except:
            print(resource, "Not Found.")
        else:
            print(resource, "Detected.")
            return dso

    #Throw an error to caller if none succeed.
    return dso

def open_cam(camera_number, width, height):
    h = cv2.VideoCapture(camera_number)
    h.set(cv2.CAP_PROP_FRAME_WIDTH, width)
    h.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
    return h

def close_cam(cam):
    if cam != UNINITIALIZED:
        cam.release()

def capture_cam(cam, filename):
    if cam != UNINITIALIZED:
        _, img = cam.read()
        cv2.imwrite(filename, img)
        return True
    else:
        print("CAM Not Ready.")
        return False

def crop_img(filename_in, v, h, filename_out):
    img = cv2.imread(filename_in, cv2.IMREAD_COLOR)
    v0 = int(v.split(':')[0])
    v1 = int(v.split(':')[1])
    h0 = int(h.split(':')[0])
    h1 = int(h.split(':')[1])
    img2 = img[v0:v1, h0:h1]
    cv2.imwrite(filename_out, img2)
    return True

def open_ocr():
    ocr = pyocr.get_available_tools()
    if len(ocr) != 0:
        ocr = ocr[0]
    else:
        ocr = UNINITIALIZED
        print("OCR Not Ready.")
    return ocr

def exec_ocr(ocr, filename):
    try:
        txt = ocr.image_to_string(
            Image.open(filename),
            lang = "eng",
            builder = pyocr.builders.TextBuilder()
        )
    except:
        print("OCR Fail.")
    else:
        return txt

def exec_labelimg(filename, label_string):
    if platform.system() == "Windows" :
        python = "python"
        grep = "findstr"
    else:
        python = "python3"
        grep = "grep"

    cmd = python + \
          " label_image.py \
          --graph=c:\\tmp\\output_graph.pb \
          --labels=c:\\tmp\\output_labels.txt \
          --input_layer=Placeholder \
          --output_layer=final_result \
          --image=" + filename \
          + "|" + grep + " " + label_string
    print(cmd)
    log = subprocess.run(cmd, stdout=subprocess.PIPE, shell=True)
    ret = log.stdout.strip().decode("utf-8").split(" ")[1]
    return ret

def set_servo(uart, servo_id, servo_pos):
    if servo_id < 0 or servo_id >= NUM_OF_SERVO:
        print("Invalid Servo ID")
        return False
    
    if servo_pos < 0 or servo_pos > 180:
        print("Invalid Servo Position")
        return False
    
    if uart != UNINITIALIZED:
        serial_write(uart, "servoread")
        
        servo_position = uart.readline().strip().decode('utf-8')
        print(servo_position)
        
        # discard "OK"
        devnull = uart.readline().strip().decode('utf-8')
        
        current_pos = int(servo_position.split(' ')[servo_id])
        #print(servo_id, current_pos)
        
        if current_pos < servo_pos:
            start = current_pos +1
            stop  = servo_pos +1
            step  = 1
        else:
            start = current_pos -1
            stop  = servo_pos -1
            step  = -1
            
        for i in range(start, stop, step):
            command = "servo " + str(servo_id) + " " + str(i)
            print(command)
            serial_write(uart, command)
            # discard "OK"
            devnull = uart.readline().strip().decode('utf-8')
            sleep(0.2) # sec.
            
        return True
    else:
        print("UART Not Initialized.")
        return False

def main():
    is_passed = True
    val = str(UNINITIALIZED)
    uart = UNINITIALIZED
    dso = UNINITIALIZED
    cam = UNINITIALIZED
    ocr = UNINITIALIZED

    if len(sys.argv) == 2:
        script_file_name = sys.argv[1] + ".csv"
        result_file_name = sys.argv[1] + "_result.csv"
    else:
        script_file_name = "script.csv"
        result_file_name = "result.csv"

    with codecs.open(script_file_name, 'r', 'utf-8') as file:
        script = csv.reader(file, delimiter=',', lineterminator='\r\n', quotechar='"')

        with codecs.open(result_file_name, 'w', 'utf-8') as file:
            result = csv.writer(file, delimiter=',', lineterminator='\r\n', quotechar='"')

            for cmd in script:
                timestamp = datetime.datetime.now().strftime("%Y/%m/%d %H:%M:%S")
                print(cmd)

                if "#" in cmd[0]:
                    pass

                elif cmd[0] == "sleep":
                    sleep(float(cmd[1]))

                elif cmd[0] == "open_uart":
                    if len(cmd) == 2:
                        dsrdtr_val = 1
                    else:
                        dsrdtr_val = int(cmd[2])
                    try:
                        uart = serial.Serial(cmd[1], 115200, timeout=1.0, dsrdtr=dsrdtr_val)
                    except:
                        is_passed = False

                elif cmd[0] == "send":
                    ret = serial_write(uart, cmd[1])
                    if ret == False:
                        is_passed = False

                elif cmd[0] == "rcvd":
                    try:
                        val = uart.readline().strip().decode('utf-8')
                        cmd.append(val)
                    except:
                        is_passed = False

                elif cmd[0] == "open_dso":
                    try:
                        dso = open_dso()
                    except:
                        is_passed = False

                elif cmd[0] == "dso":
                    try:
                        if "?" in cmd[1]:
                            val = dso.query(cmd[1]).rstrip().replace(",", "-")
                            cmd.append(val)
                        else:
                            dso.write(cmd[1])
                    except:
                        is_passed = False

                elif cmd[0] == "open_cam":
                    if len(cmd) == 2:
                        cam = open_cam(int(cmd[1]), 640, 480)
                    else:
                        cam = open_cam(int(cmd[1]), int(cmd[2]), int(cmd[3]))

                elif cmd[0] == "close_cam":
                    close_cam(cam)
                    cam = UNINITIALIZED

                elif cmd[0] == "capture_cam":
                    ret = capture_cam(cam, cmd[1])
                    if ret == False:
                        is_passed = False

                elif cmd[0] == "crop_img":
                    crop_img(cmd[1], cmd[2], cmd[3], cmd[4])

                elif cmd[0] == "open_ocr":
                    ocr = open_ocr()
                    if ocr == UNINITIALIZED:
                        is_passed = False

                elif cmd[0] == "exec_ocr":
                    try:
                        val = exec_ocr(ocr, cmd[1])
                    except:
                        is_passed = False
                    else:
                        cmd.append(str(val))

                elif cmd[0] == "exec_labelimg":
                    try:
                        val = exec_labelimg(cmd[1], cmd[2])
                    except:
                        is_passed = False
                    else:
                        cmd.append(str(val))

                elif cmd[0] == "set_servo":
                    ret = set_servo(uart, int(cmd[1]), int(cmd[2]))
                    if ret == False:
                        is_passed = False

                elif cmd[0] == "eval_str_eq":
                    if str(val) != str(cmd[1]):
                        is_passed = False

                elif cmd[0] == "eval_int_eq":
                    if int(val) != int(cmd[1]):
                        is_passed = False

                elif cmd[0] == "eval_int_gt":
                    if int(val) < int(cmd[1]):
                        is_passed = False

                elif cmd[0] == "eval_int_lt":
                    if int(val) > int(cmd[1]):
                        is_passed = False

                elif cmd[0] == "eval_dbl_eq":
                    if float(val) != float(cmd[1]):
                        is_passed = False

                elif cmd[0] == "eval_dbl_gt":
                    if float(val) < float(cmd[1]):
                        is_passed = False

                elif cmd[0] == "eval_dbl_lt":
                    if float(val) > float(cmd[1]):
                        is_passed = False

                else:
                    cmd.append("#")

                if is_passed == True:
                    cmd.append("OK")
                    cmd.insert(0,timestamp)
                    print(cmd)
                    result.writerow(cmd)
                else:
                    cmd.append("NG")
                    cmd.insert(0,timestamp)
                    print(cmd)
                    result.writerow(cmd)
                    close_uart(uart)
                    close_cam(cam)
                    print("FAIL")
                    sys.exit(1)

    if is_passed == True:
        close_uart(uart)
        close_cam(cam)
        print("PASS")
        sys.exit(0)

main()
```