ラズパイとAI（AutoML）で映像からオブジェクトを検出してレゴEV3のモーターを制御する

├─chainer-ev3
│  │      
│  ├─workspace
│  │ DeepChick.ipynb
│  │  │
│  │  ├──models
│  │  │  lego_figure_OD_dict.txt  
│  │  │  lego_figure_OD_model.tflite  
│  │  │  lego_figure_OD_tflite_metadata.json  
│  │  │

# -*- coding: utf-8 -*-
"""
Created on Sun Mar  8 11:18:55 2020
@author: Souichirou Kikuchi
"""

from time import sleep
import os
import io
import sys
sys.path.append('/home/pi/.local/lib/python3.7/site-packages') # Pathを明示的に指定
from concurrent.futures import ThreadPoolExecutor # スレッド処理
from concurrent.futures import ProcessPoolExecutor # プロセス処理
from picamera import PiCamera
from PIL import Image # Python Image Library
from lib.ev3 import EV3
from tflite_runtime.interpreter import Interpreter, load_delegate
import numpy as np

MODEL = './models/lego_figure_OD_model.tflite'

# 読み込むカメラの解像度
CAMERA_WIDTH = 640
CAMERA_HEIGHT = 480
THRESHOLD = 0.4 # 最低予測スコア

class  EyeCameraClass: # カメラのクラス
    def __init__(self): # コンストラクタ
        self.camera = PiCamera(resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), framerate=30)
        self.camera.start_preview()

    def detect_pi_camera(self): # カメラ撮影は別スレッドで実行する
        print('camera theread start')
        executor = ThreadPoolExecutor(max_workers=2, thread_name_prefix="thread")
        executor.submit(self.camera_theread)
        print('camera theread end')
 
    def camera_theread(self): # PiCameraのストリームからjpegを取り出して物体検出
        _, input_height, input_width, _ = interpreter.get_input_details()[0]['shape']
        stream = io.BytesIO()
        for _ in self.camera.capture_continuous(stream, format='jpeg', use_video_port=True):
            stream.seek(0)
            image = Image.open(stream).convert('RGB').resize((input_width, input_height), Image.ANTIALIAS)
            self.objs = self.detect_objects(interpreter, image, THRESHOLD)
            stream.seek(0)
            stream.truncate()
    
    def set_input_tensor(self, interpreter, image):
        tensor_index = interpreter.get_input_details()[0]['index']
        input_tensor = interpreter.tensor(tensor_index)()[0]
        input_tensor[:, :] = image

    def get_objs(self): # objsを返す関数（先頭のclass_idを返す)
        if len(self.objs) == 0: # Objectが見つからない時
            res = 99
        else:
            res = self.objs[0]['class_id']
        return res

    def get_output_tensor(self, interpreter, index):
        # indexで指定されたtensorを出力する
        # 配列の順番はlego_figure_OD_tflite_metadata.jsonのoutputTensorsに記述されている
        output_details = interpreter.get_output_details()[index]
        tensor = np.squeeze(interpreter.get_tensor(output_details['index']))
        return tensor

    def detect_objects(self, interpreter, image, threshold):
        # threshold以上の予測スコアの検出結果を複数返す
        self.set_input_tensor(interpreter, image)
        interpreter.invoke()
        # 検出結果明細を（予測スコア以下も含めて）全て出力する
        boxes = self.get_output_tensor(interpreter, 0)
        classes = self.get_output_tensor(interpreter, 1)
        scores = self.get_output_tensor(interpreter, 2)
        count = int(self.get_output_tensor(interpreter, 3))
        results = []
        for i in range(count):
            if scores[i] >= threshold: # 予測スコア以上のモノだけをresultに追加する
                result = {
                        'bounding_box': boxes[i],
                        'class_id': classes[i],
                        'score': scores[i]
                        }
                results.append(result)
        return results

    def close_pi_camera(self):
        self.camera.stop_preview()
        self.camera.close()

#main
try:
    if __name__ == "__main__":
        os.chdir(os.path.dirname(os.path.abspath("__file__"))) # カレントディレクトリをプログラムのあるディレクトリに移動する
        # センサーとモーターの通信ポートの定義.
        touch_port = EV3.PORT_2
        lmotor_port = EV3.PORT_B
        rmotor_port = EV3.PORT_C
        # センサーとモーターの初期設定.
        ev3 = EV3()
        ev3.motor_config(lmotor_port, EV3.LARGE_MOTOR)
        ev3.motor_config(rmotor_port, EV3.LARGE_MOTOR)
        ev3.sensor_config(touch_port, EV3.TOUCH_SENSOR)

        # Tensorインタープリター
        interpreter = Interpreter(MODEL, experimental_delegates=[load_delegate('libedgetpu.so.1.0')]) 
        interpreter.allocate_tensors()
        eye_camara = EyeCameraClass() # カメラコンストラクタ
        res = eye_camara.detect_pi_camera() # 物体検出
        while True:

            print("追いかけるフィギュアを認識します。タッチセンサーを押してください")
            while not ev3.touch_sensor_is_pressed(touch_port): # タッチセンサーが押されて離されるまで待つ
                pass
            while ev3.touch_sensor_is_pressed(touch_port):
                pass
            target_class = eye_camara.get_objs() # 追いかけるターゲットのクラスIDを取得する
            if target_class == 0: # class_idによって振り分け
                ev3.lcd_draw_string('Woman', 1)
                speed = 10 # 前進
                steer = 0
            elif target_class == 1:
                ev3.lcd_draw_string('Olivia', 1)
                speed = -10 # 後退
                steer = 0
            elif target_class == 2:
                ev3.lcd_draw_string('Rapunzel', 1)
                speed = 10 # 左
                steer = -50
            elif target_class == 3:
                ev3.lcd_draw_string('Stephanie', 1)
                speed = 10 # 右
                steer = 50
            elif target_class == 99:
                ev3.lcd_draw_string('not found', 1)
                speed = 0 # 停止
                steer = 0
            print("もう一度タッチセンサーを押すとWoman：前進、Olivia：後退、Rapunzel：左、Stephanie：右に動きます")
            while not ev3.touch_sensor_is_pressed(touch_port): # タッチセンサーが押されて離されるまで待つ
                pass
            while ev3.touch_sensor_is_pressed(touch_port):
                pass
            ev3.motor_steer(lmotor_port, rmotor_port, speed, steer)
            sleep(1)
            speed = 0 # 停止
            steer = 0
            ev3.motor_steer(lmotor_port, rmotor_port, speed, steer)
except KeyboardInterrupt:
    pass
finally:
    ev3.close()
    eye_camara.close_pi_camera()
    print('program end')

    def detect_pi_camera(self): # カメラ撮影は別スレッドで実行する
        print('camera theread start')
        executor = ThreadPoolExecutor(max_workers=2, thread_name_prefix="thread")
        executor.submit(self.camera_theread)
        print('camera theread end')

    def camera_theread(self): # PiCameraのストリームからjpegを取り出して物体検出
        _, input_height, input_width, _ = interpreter.get_input_details()[0]['shape']
        stream = io.BytesIO()
        for _ in self.camera.capture_continuous(stream, format='jpeg', use_video_port=True):
            stream.seek(0)
            image = Image.open(stream).convert('RGB').resize((input_width, input_height), Image.ANTIALIAS)
            self.objs = self.detect_objects(interpreter, image, THRESHOLD)
            stream.seek(0)
            stream.truncate()