北の国から

忘れたことを思い出す為の記録

cppでdepthaiのxLinkやSPIOutを試す

ruru-log.hatenablog.com

上記のcpp版です。

#include <vector>
#include "depthai/depthai.hpp"
int main(int argc, char *argv[]){

    dai::Pipeline pipeline = createPipeline();
    dai::Device device(pipeline);

    std::shared_ptr<dai::DataInputQueue> xInQueue = device.getInputQueue("communication");
    std::shared_ptr<dai::DataOutputQueue> xOutQueue = device.getOutputQueue("communication",5,false);
    
    uint16_t angle = 0;
    uint16_t reachTime = 0;
    while(true){
        dai::Buffer srv;
        std::vector<uint8_t> data{
            0x55,
            0x55,
            0x01,
            0x07,
            0x01,
            0x00FF & angle,
            0x00FF & angle>>8,
            0x00FF & reachTime,
            0x00FF & reachTime>>8,
            };
        data.push_back(~std::accumulate(data.begin(),data.end(),0));
        srv.setData(data);
        xInQueue->send(srv);
        auto get_data = xOutQueue->tryGet();
        if(get_data != nullptr){
            std::for_each(get_data->getRaw()->data.begin(),get_data->getRaw()->data.end(),
            [](const int& n){std::cout<<n<<";";});
            std::cout<<std::endl;
        }
    }
    return 0;
}

角度とかは0固定にしてるけど。

rosから動かすのも問題なく書けそうです。
c++もっと使って勉強しないとなぁ。

dataの宣言のとこ、もっと賢くできるんじゃないかなぁと思っている。

depthai周りはこのリポジトリで作業します。

github.com

ruru

depthaiのSPIでサーボを操作する

Introduction

この記事は前回の続きです。

ruru-log.hatenablog.com

前回でdepthaiを通して任意のデータを送信できました。

今回はESP32へのSPIによるデータ送信とESP側での受信処理をやります。
構成として、depthai ---< SPI >--- ESP32 ---< UART >--- PCとしてdepthaiで送ったデータがちゃんと流れていることを確認します。

Host Script (python)

import depthai as dai

def main():
    pipeline = dai.Pipeline()

    xIn = pipeline.createXLinkIn()
    xIn.setStreamName("input_stream")

    xSpi = pipeline.createSPIOut()
    xSpi.setStreamName("servo")
    xSpi.setBusId(0)
    xSpi.input.setBlocking(False)

    xOut = pipeline.createXLinkOut()
    xOut.setStreamName("servo")

    xIn.out.link(xOut.input)
    xIn.out.link(xSpi.input)

    with dai.Device(pipeline) as device:
        xIn_queue = device.getInputQueue("input_stream")
        xOut_queue = device.getOutputQueue("servo",maxSize=5,blocking=False)
        
        while True:
            data_out = dai.RawBuffer()
            data_out.data = [0x61,0x62]

            xIn_queue.send(data_out)

            data_in = xOut_queue.tryGet()
            if data_in is not None:
                print(data_in.getRaw().data)

if __name__ == "__main__":
    main()

Host側は前回とほぼ変化なしですね。
SPIで送信するにはcreateSPIOutでSPIノードを立てます。
BusIdはSPIバスの番号ですが,すけまティックを見る限り"0"しか接続されてないので0固定です。
input.setBlockingはSPI受信のときにブロッキング(読み取れるまで待つ)するか否かのフラグ設定だと思います。これもほとんどFalse固定でいいと思います。

重要なポイントとして、出力したいデータのQueueと同じ名前のstreamNameを設定する必要があります。
この場合、servoの出力と同じデータを吐き出したいのでservoにします。
送信に関して気にすることは無く、xOutの出力をそのままSPIに出力してくれます。

ESP32 Script

void run_demo(){
    uint8_t req_success = 0;

    dai::SpiApi mySpiApi;
    mySpiApi.set_send_spi_impl(&esp32_send_spi);
    mySpiApi.set_recv_spi_impl(&esp32_recv_spi);

    bool receivedAnyMessage = false;
    while(1) {
        dai::Message servoDataMsg;
        if(mySpiApi.req_message(&servoDataMsg, "servo")){

            uart_write_bytes(ECHO_UART_PORT_NUM, servoDataMsg.raw_data.data, servoDataMsg.raw_data.size);
            printf("received! m:%s d:%d t:%d\n",servoDataMsg.raw_data.data,servoDataMsg.raw_data.size,servoDataMsg.type==dai::DatatypeEnum::Buffer);

            mySpiApi.free_message(&servoDataMsg);
            mySpiApi.spi_pop_message("servo");
            receivedAnyMessage = true;
        }

        if(!receivedAnyMessage){
            // Delay pooling of messages
            usleep(1000);
        }
    }
}

//Main application
void app_main()
{
    init_esp32_spi();

    /* Configure parameters of an UART driver,
     * communication pins and install the driver */
    uart_config_t uart_config = {
        .baud_rate = ECHO_UART_BAUD_RATE,
        .data_bits = UART_DATA_8_BITS,
        .parity    = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_APB,
    };
    int intr_alloc_flags = 0;

#if CONFIG_UART_ISR_IN_IRAM
    intr_alloc_flags = ESP_INTR_FLAG_IRAM;
#endif

    ESP_ERROR_CHECK(uart_driver_install(ECHO_UART_PORT_NUM, BUF_SIZE * 2, 0, 0, NULL, intr_alloc_flags));
    ESP_ERROR_CHECK(uart_param_config(ECHO_UART_PORT_NUM, &uart_config));
    ESP_ERROR_CHECK(uart_set_pin(ECHO_UART_PORT_NUM, ECHO_TEST_TXD, ECHO_TEST_RXD, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));

    run_demo();

    deinit_esp32_spi();
}

ESP32側に関しては、uart_echo(esp-idf公式)とtwo_streams(luxonis)のサンプルを参考に引用しつつ組んでいます。

app_mainの最初でinit_esp32_spi();を呼んでspiを初期化します。 この関数でセマフォ周りも初期化してくれていて、割り込み処理を(ほぼ)気にせずにSPIを実装できます。

    dai::SpiApi mySpiApi;
    mySpiApi.set_send_spi_impl(&esp32_send_spi);
    mySpiApi.set_recv_spi_impl(&esp32_recv_spi);

その後上記でSpiを初期化します。特に意識せずにサンプルを引っ張ってきています。

mySpiApi.req_message(&servoDataMsg, "servo")

上記でservoストリームのデータをリクエストし、servoDataMsgに格納します。 このservoDataMsgは

struct Message {
    Data raw_data;
    Metadata raw_meta;
    dai::DatatypeEnum type;
};

で、送信データがraw_data、種類がtyoeに格納されます。
また、raw_metaはパット見、大きめのデータ(depthのデータとか)が入るやつでRawBufferを使う際には使わなさそうです。
Nodeなどを自作する際には便利なのかな。

処理が終わったらdataは解放します。

uart部分はほとんどuart_echoのパクリです。

Result

Host側から送信しているデータは"0x61","0x62"でUTF-8で"a","b"にあたります。
受信側のCOMポートを見ると、uartで適切にデータを送れていることがわかります。

f:id:r_u__r_u:20211114222827p:plain
受信結果

Conclusion

一方向ですがdepthaiからサーボを動かす準備ができたので、次はサーボ動作用のプロトコルを組んでみます。
また、送信側がpythonなのが少し癪なのでCppで組めるようにしときます。
サーボ用電源が5Vしかない(定格6-8V)のがちょっと不安ですが、まぁなんとかなると思います。

追記

送信データを次のようにすることでちゃんとサーボ動かせました。
あとはID書き込むノードとか準備して2軸動かしますね。

            TxData = [
                    0x55,
                    0x55,
                    0x01,
                    0x07,
                    0x01,
                    0x00FF & (Angle),
                    0x00FF & (Angle>>8),
                    0x00FF & (reach_time),
                    0x00FF & (reach_time>>8)
                    ]
            TxData.append(~np.sum(TxData[2:]))
            data_out.data = TxData

動作の様子

ruru

depthaiのxLinkを通して任意のデータをやり取りする方法

Introduction

depthai はLuxonisが作っている(?)VPUやカメラなどをひとまとめにしたデバイス(OAK-Dなど)を扱う際に必要となるライブラリです。

OAK-D-WIFI(製品として出てるのはOAK-D-IoT)にはESP32が搭載されているのでセンサのデータをやり取りしたり、ESPを通してモータを制御したりなどいろいろやれることがあります。

OAK-D-WIFIはSPIのピンもそのままコネクタとして引き出されているのでVPU側からSPIでセンサデータを直接取り出すこともできそうです。

Motivation

公式のサンプルは各機能の扱い方よりは「カメラ周りですでにライブラリ化されたものたち(NNやDepthなど)でこんなことができますよ」みたいな側面を強く感じたので、今回はホストーデバイス間でのデータのやり取りの基礎となるxLinkプロトコル部分について、コールバックするスクリプトの作成を通して学んでいきます。

Script

import depthai as dai
import time

def main():
    pipeline = dai.Pipeline()

    xIn = pipeline.createXLinkIn()
    xIn.setStreamName("stream_0")

    xOut = pipeline.createXLinkOut()
    xOut.setStreamName("stream_1")

    xIn.out.link(xOut.input)

    with dai.Device(pipeline) as device:
        start = time.time()
        xIn_queue = device.getInputQueue("stream_0")
        xOut_queue = device.getOutputQueue("stream_1",maxSize=5,blocking=False)
        
        while True:
            t = time.time() - start

            data_out = dai.RawBuffer()
            data_out.data = [t,t]

            xIn_queue.send(data_out)

            data_in = xOut_queue.tryGet()
            if data_in is not None:
                print(data_in.getData())

if __name__ == "__main__":
    main()

Explanation

import depthai as dai
import time

depthaiは大抵のサンプルでdaiエイリアスされているので私もそうします。
timeは結果を用意するために使います。

pipeline = dai.Pipeline()

xIn = pipeline.createXLinkIn()
xIn.setStreamName("stream_0")

xOut = pipeline.createXLinkOut()
xOut.setStreamName("stream_1")

xIn.out.link(xOut.input)

まず,depthaiでデバイスを扱うにはpipelineを用意する必要があるためdai.Pipeline()インスタンスを生成します。
createXLinkIn()createXLinkOut()メソッドによってパイプライン上にストリームを生成し、setStreamName命名します。
これらのcreateなんちゃらはcreateメソッドのノードごとの固有ラッパみたいなのでノードを自作する場合などはcreateメソッドから指定してあげる必要があります。(ノード自作のやり方や必要性はまだ理解していないです)

それぞれ名称設定等の初期設定が終わったら、出力の.out.linkメソッドに入力の.input変数を与えてストリームをつなぐ先を指定します。
今回の場合、stream_0がホストからのデータ取得、stream_1がホストへのデータ吐き出しなのでVPUデバイス内では stream_0 -> stream_1 とつないでいます。

  with dai.Device(pipeline) as device:
        start = time.time()
        xIn_queue = device.getInputQueue("stream_0")
        xOut_queue = device.getOutputQueue("stream_1",maxSize=5,blocking=False)
        
        while True:
            t = time.time() - start

            data_in = dai.RawBuffer()
            data_in.data = [t]

            xIn_queue.send(data_in)

            data_out = xOut_queue.tryGet()
            if data_out is not None:
                print(data_out.getData())

バイスーホスト間のデータのやり取りは``Queue```で行われます。
In/Outの関係性ですが、depthaiのAPIは基本的にデバイス視点で設定されているようです(In関数群がデータ取得、Out関数群データ吐き出し)

xIn_queuesendメソッドでデータを送信できます。RawBufferdata変数にlistを入れると好きにデータを遅れます。

また、データの取得はtryGetメソッドでデータがあるかを確認してから、getDataでデータを取得します。

今回はtimeを入れて試してみました。

f:id:r_u__r_u:20211114165156p:plain

送受信できたdataは基本的に整数の1列のリストか同じ長さのリストのリストでした。[[0], [1,2]]のようなデータは送れないようです。
また、リストのリストは1列に圧縮されます。( [[0,1],[2,3]]を送ると[0,1,2,3] )
どこでそうなるでかはわかっていませんが。

Conclusion

ひとまずやりたいこととして任意のデータ送信ができました。
目的はOAK上のESPによるサーボ制御なので整数型の一列listが送れれば十分です。
次回はSPIOutによるESP側とのコミュニケーションを実装します。

ruru