cppでdepthaiのxLinkやSPIOutを試す
上記の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周りはこのリポジトリで作業します。
depthaiのSPIでサーボを操作する
Introduction
この記事は前回の続きです。
前回で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で適切にデータを送れていることがわかります。
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
動作の様子
動いたけど電源が弱い pic.twitter.com/1E1hBK1S3c
— ruru (@r_u__r_u) 2021年11月14日
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_queue
のsend
メソッドでデータを送信できます。RawBuffer
のdata
変数にlist
を入れると好きにデータを遅れます。
また、データの取得はtryGet
メソッドでデータがあるかを確認してから、getData
でデータを取得します。
今回はtimeを入れて試してみました。
送受信できたdataは基本的に整数の1列のリストか同じ長さのリストのリストでした。[[0], [1,2]]
のようなデータは送れないようです。
また、リストのリストは1列に圧縮されます。( [[0,1],[2,3]]を送ると[0,1,2,3] )
どこでそうなるでかはわかっていませんが。
Conclusion
ひとまずやりたいこととして任意のデータ送信ができました。
目的はOAK上のESPによるサーボ制御なので整数型の一列listが送れれば十分です。
次回はSPIOutによるESP側とのコミュニケーションを実装します。