OpenMVをどうにかC#で制御したい

OSSのいい所を味わっていきましょう。

事情
しらべて実装
知見
まとめ

1. 事情

つい先日Indiegogoで出資していたuArm Swiftというロボットが届きました。

このロボットには画像処理もサポートできるようにとOpen MVカメラが付属していたのですが、正直持て余していました。

何もしないのも持て余したままでつまらないので、とりあえず例によって、C#で制御できるか試しました。ロボット本体はC#用に色々整備しており、とくにUnityで制御できるアテもついている(この記事とか参照)ので、カメラもひとます様子を見ておこう、という魂胆です。

2. しらべて実装

やりたい事は大まかにいうとOpenMV IDEの真似です。

ボードのファームウェアバージョンがとれる(=基本的な通信ができる)状態にしたい
LEDをチカチカしたり、ボードを制御したい
画像処理プログラムを実行/停止したい
現在とっている画像をPC側にも持ってきたい

いずれも、答えはOpenMVのGitHubを読んでればわかります。とくに見るべきソースは以下2つ。

ボード側のI/F定義: usr/openmv.py
PC側GUIでI/F使っている処理: usr/openmv-ide.py

要点は以下の通りです。

シリアル通信をボーレート921600または12000000で接続するとIDE用の専用モードで接続したことになる
- ボーレートが低い(115200とか)接続で普通にやってるとMicroPython REPLが動く
基本的に、アスキーコード0x30から始まるデータを渡してやりとりする
画像処理用のMicroPythonスクリプトを実行したい場合、スクリプトの内容全体を文字列として投げつける

で、動作環境は.NETなら割と何でもいいんですが、ここでは最終的に絵を出したいのでUnityを使った例で行きます。以下にスクリプトの実装例(JpgToTexture.cs)を示します。本来.NETのシリアル受信処理はそのまま使わない方がいいらしい¹のですが、面倒なので普通にやっちゃってます。

長いので折りたたみ。

[expand title="JpgToTexture.cs(クリックで展開)"]

using System;
using System.IO;
using System.Text;
using System.IO.Ports;
using System.Threading.Tasks;
using UnityEngine;
using OpenMV;

public class JpgToTexture : MonoBehaviour {

    public string serialName;

    private byte[] _newJpg = null;
    private SerialPort _serial = null;
    private Texture2D _targetTexture;

    private int _blockTimeMillisec = 200;

    private void Start()
    {
        if (_targetTexture == null)
        {
            _targetTexture = GetComponent<Renderer>()?.material?.mainTexture as Texture2D;
        }

        if (_targetTexture == null)
        {
            Debug.Log("no texture2d, create new");
            _targetTexture = new Texture2D(640, 480);
            GetComponent<Renderer>().material.mainTexture = _targetTexture;
            bool resizeRes = _targetTexture.Resize(640, 480, TextureFormat.ARGB32, false);
            Debug.Log("resize res=" + resizeRes.ToString());
        }

        //シリアル通信開始
        Task.Run(() => DoSerialCommunication());
    }
    
    private void Update()
    {
        if (_targetTexture == null)
        {
            return;
        }

        //シリアル側のスレッドから最新データが置かれたら拾って適用
        if (_newJpg != null)
        {
            byte[] jpgBin = _newJpg;
            _newJpg = null;

            _targetTexture.LoadImage(jpgBin);
            _targetTexture.Apply();
        }
    }

    private void OnDestroy()
    {
        if (_serial != null || _serial.IsOpen)
        {
            //TODO: この方法では複数スレッドからシリアル出力に書き込む事になるのであまりマナー良くない
            Write(_serial, Protocol.CommandStopScript);
        }
        _serial?.Close();
        _serial = null;
    }

    private async void DoSerialCommunication()
    {
        _serial = new SerialPort(serialName, 921600);
        _serial.Open();

        //簡単なテスト: ファームウェアバージョン取ってみる
        Write(_serial, Protocol.CommandGetFirmwareVersion);
        var recv = new byte[12];
        _serial.Read(recv, 0, recv.Length);
        Debug.Log("Recv 1:" + string.Join(",", recv));

        //実験した感じだとこのイネーブルがなくても大丈夫っぽい(そう見える)のと、これやると逆に不安定説もあるのでやらない
        //ダメそうならコメントアウト外す
        //Write(_serial, Protocol.CommandFrameBufferEnable(true));

        //適当にスクリプト始動させる例: デスクトップ直下に"test_openmv.py"を置いておき、その内容を投げつけて実行
        //NOTE: 
        //  この例ならFile.ReadAllBytesも使えるが、
        //  本来は文字エンコードする流れの方が正しいので、雰囲気重視でこうしてる
        byte[] binScript = Encoding.UTF8.GetBytes(
            File.ReadAllText(
                Path.Combine(
                    Environment.GetFolderPath(Environment.SpecialFolder.Desktop),
                    "test_openmv.py"
                    )
                )
            );
        byte[] binHeader = Protocol.CommandScriptExecHeader(binScript.Length);

        Write(_serial, binHeader);
        Write(_serial, binScript);
        Debug.Log("Script started");

        //カメラのリセット処理時間とフレームスキップ処理が終わるまで待機
        await Task.Delay(3000);

        //ひたすらカメラ画像とりに行く
        while (_serial != null && _serial.IsOpen)
        {
            //正常系では待機処理しない: シリアル受信がボトルネックになるからWait的なものがなくても速度が頭打ちになるのでは、という想定
            bool isBufferUpdated = TryUpdateFrameBuffer();
            if(!isBufferUpdated)
            {
                //処理失敗した場合は時間間隔を持たせる(通信がギチギチなせいで処理にコケ続ける可能性あるので避ける)
                await Task.Delay(_blockTimeMillisec);
            }
        }

        Debug.Log("loop ended");
    }

    //カメラ画像の取得を試みる
    private bool TryUpdateFrameBuffer()
    {
        _serial.DiscardInBuffer();
        Write(_serial, Protocol.CommandGetFrameBufferSize);
        byte[] sizeRes = new byte[12];
        _serial.Read(sizeRes, 0, sizeRes.Length);
        int width = BitConverter.ToInt32(sizeRes, 0);
        int height = BitConverter.ToInt32(sizeRes, 4);
        int imgSize = BitConverter.ToInt32(sizeRes, 8);

        //以下はすべて不審データなのでデータとらないでおく
        // - ゼロ以下の値がまじってる
        // - imgSizeがでかすぎる
        if (width <= 0 || height <= 0 || imgSize <= 2 || imgSize >= 40000)
        {
            Debug.Log($"FB Size w={width}, h={height}, size={imgSize}");
            return false;
        }

        Write(_serial, Protocol.CommandGetFrameBuffer(imgSize));
        //指定したバイト数ぶんのデータが戻ってくるはずなので、それをまとめる
        //TODO: データ途中で切れた場合とか
        var frameBuffer = new byte[imgSize];
        int resCount = 0;
        while (resCount < frameBuffer.Length)
        {
            resCount += _serial.Read(frameBuffer, resCount, frameBuffer.Length - resCount);
        }
        _newJpg = frameBuffer;

        return true;
    }

    static void Write(SerialPort serial, byte[] data)
        => serial.Write(data, 0, data.Length);
}

[/expand]

さらに通信に使うProtocolというのも別ファイルで定義します。上のスクリプトとは異なり、Unity特有のMonoBehaviorとかが関係ないので、こちらは本格的に普通のC#コードとなります。こちらも一応たたんでおきます。

[expand title="Protocol.cs(クリックで展開)"]

using System;

namespace OpenMV
{
    internal static class Protocol
    {
        public static readonly byte DBG_CMD = 48;
        public static readonly byte FRAME_SIZE_RESPONSE_SIZE = 12;

        public static readonly byte USBDBG_FW_VERSION = 0x80;
        public static readonly byte USBDBG_FRAME_SIZE = 0x81;
        public static readonly byte USBDBG_FRAME_DUMP = 0x82;
        public static readonly byte USBDBG_ARCH_STR = 0x83;
        public static readonly byte USBDBG_SCRIPT_EXEC = 0x05;
        public static readonly byte USBDBG_SCRIPT_STOP = 0x06;
        public static readonly byte USBDBG_SCRIPT_SAVE = 0x07;
        public static readonly byte USBDBG_SCRIPT_RUNNING = 0x87;
        public static readonly byte USBDBG_TEMPLATE_SAVE = 0x08;
        public static readonly byte USBDBG_DESCRIPTOR_SAVE = 0x09;
        public static readonly byte USBDBG_ATTR_READ = 0x8A;
        public static readonly byte USBDBG_ATTR_WRITE = 0x0B;
        public static readonly byte USBDBG_SYS_RESET = 0x0C;
        public static readonly byte USBDBG_FB_ENABLE = 0x0D;
        public static readonly byte USBDBG_TX_BUF_LEN = 0x8E;
        public static readonly byte USBDBG_TX_BUF = 0x8F;


        public static byte[] CommandGetFirmwareVersion
            => new byte[] { DBG_CMD, USBDBG_FW_VERSION, 12, 0, 0, 0 };

        public static byte[] CommandScriptExecHeader(int contentLength)
        {
            var result = new byte[6];
            result[0] = DBG_CMD;
            result[1] = USBDBG_SCRIPT_EXEC;
            WriteLittleEndianInt(contentLength, result, 2);

            return result;
        }

        public static byte[] CommandStopScript
            => new byte[] { DBG_CMD, USBDBG_SCRIPT_STOP, 0, 0, 0, 0 };

        public static byte[] CommandGetFrameBufferSize
            => new byte[] { DBG_CMD, USBDBG_FRAME_SIZE, FRAME_SIZE_RESPONSE_SIZE, 0, 0, 0 };

        public static byte[] CommandGetFrameBuffer(int imgSize)
        {
            var result = new byte[6];
            result[0] = DBG_CMD;
            result[1] = USBDBG_FRAME_DUMP;
            WriteLittleEndianInt(imgSize, result, 2);

            return result;
        }

        public static byte[] CommandFrameBufferEnable(bool enabled)
            => enabled ?
            new byte[] { DBG_CMD, USBDBG_FB_ENABLE, 0, 0, 0, 0, 1, 0 } :
            new byte[] { DBG_CMD, USBDBG_FB_ENABLE, 0, 0, 0, 0, 0, 0 };

        private static void WriteLittleEndianInt(int value, byte[] dest, int index)
        {
            byte[] data = BitConverter.GetBytes(value);
            if (BitConverter.IsLittleEndian)
            {
                Array.Copy(data, 0, dest, index, data.Length);
            }
            else
            {
                for (int i = 0; i < data.Length; i++)
                {
                    dest[index + i] = data[data.Length - 1 - i];
                }
            }
        }
    }
}

[/expand]

また上記の例では実行時デスクトップ直下にtest_openmv.pyというファイルが無いと動かないので、それも用意します。内容としては、例えばエッジ検出の簡単なコードを置きたければ以下のようなものを。

import sensor, image, time

sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_gainceiling(8)

while(True):
    img = sensor.snapshot()
    img.find_edges(image.EDGE_CANNY, threshold=(50, 80))

要するに、ふだんOpenMV IDEで書いてるようなのと同じものならOKです。

以上の用意が出来たら、JpgToTextureを適当なPlaneなどにアタッチし、シリアル通信ポートの名前を正しく設定して動かします。以下が動かした例です。

https://twitter.com/baku_dreameater/status/896282554919133184

エッジ検出するプログラムを読み込んで実行し、結果の画像をUnity側に引っ張りこめています。

3. 知見

上記のプログラムだとOpenMV IDEに比べてFPSが出ない
- ちょくちょくフレームバッファのサイズ取得に失敗しています。運にも依存します。詳細はよくわかってません。
PCに持ち込める画像はJPG圧縮されてるので粗い
- 単純に通信量を削る(FPSを上げる)ためにそうなっているようです。PC側でbmp貰って画像処理しようとするのは基本NG、ということです。
この方法を使っている場合、カメラ画像以外の情報(物体認識の有無とか位置とか)はシリアル通信で取得できない(ように見える)
- 両立したければ大人しくSPIとかUARTとかのピンを使いましょう、という話っぽいです。

4. まとめ

完璧ではないですが、C#でPythonを真似したコードでほぼOpenMVボードが制御できる事が分かりました。ここではUnityに突っ込む方向性で紹介しましたが、あくまで画像表示に使ってただけなので、通常のWindows向けコードでも同じような事は出来ます。

If you must use .NET System.IO.Ports.SerialPort ↩

Bakulog

獏の夢日記的な何か。

OpenMVをどうにかC#で制御したい

1. 事情

2. しらべて実装

3. 知見

4. まとめ