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側とのコミュニケーションを実装します。
