編集履歴一覧に戻る
airpocketのアイコン画像

airpocket が 2022年11月16日17時56分10秒 に編集

コメント無し

本文の変更

# はじめに 前回の記事で、SipeedのデプスセンサMS-A010をWindows PCに接続してデプス情報を可視化してみました。 今回はMS-A010をRaspberry Piに接続してOpenCVで可視化してみたいと思います。 [MS-A010のクラファンサイト](https://www.indiegogo.com/projects/metasense-rgbd-tof-3d-camera-for-mcu-ros-robot) [MS-A010の公式サイト](https://wiki.sipeed.com/hardware/zh/maixsense/maixsense-a010/maixsense-a010.html) [ハードウェア情報](https://dl.sipeed.com/shareURL/MaixSense/MaixSense_A010) [開発者用サイト](https://wiki.sipeed.com/hardware/en/maixsense/maixsense-a010/at_command_en.html) # できたこと デプスデータをopencvの画像配列に変換、連結してmp4として出力しました。 もちろん、ロボット制御などに使用する事もできます。

-

@[twitter](https://twitter.com/AirpocketRobot/status/1592799862520950784)

+

@[twitter](https://twitter.com/AirpocketRobot/status/1592803585309691905)

# 環境 都合によりテスト環境はbusterです。大した依存関係はないはずなので、pyserial と opencv-pythonさえ入っていればbulseyeでも問題なく動くと思います。 pyserialはVer.3.4、opencv-pythonはVer.4.6.0.66でした。 ``` $ lsb_release -a No LSB modules are available. Distributor ID: Raspbian Description: Raspbian GNU/Linux 10 (buster) Release: 10 Codename: buster $ pip3 list | grep pyserial pyserial 3.4 $ pip3 list | grep opencv opencv-python 4.6.0.66 ``` # デバイス接続 接続する前に、現在接続されているUSBデバイスを確認しておきます。 ``` $ ls -al /dev | grep ttyUSB ``` 何も接続されていない場合は、何も表辞されません。 続いてMS-A010とRaspberry PiをUSBケーブルで接続します。MS-A010はType C 、Raspberry Pi側はType Aです。 接続できたら、もう一度接続されているUSBデバイスを確認します。 ``` $ ls -al /dev | grep ttyUSB crw-rw----+ 1 root dialout 188, 0 11月 15 23:02 ttyUSB0 crw-rw----+ 1 root dialout 188, 1 11月 15 23:02 ttyUSB1 ``` 私の環境ではttyUSB0とttyUSB1というポートが新たに検出されました。 このうち通信に使用するのは番号の若い方です。この場合はttyUSB0です。/devフォルダに格納されているため、絶対アドレスでは/dev/ttyUSB0となります。 # 可視化用コード このコードではpyserialとopencv-pythonのパッケージを使用しますので事前にインストールしてください。 実行するとカレントディレクトリにoutput.mp4というムービーファイルが記録されます。 ``` import serial import cv2 import numpy as np from struct import unpack rawData = b'' w = 200 h = 200 fps = 2 #USBポートを指定 port = "/dev/ttyUSB0" #デプスデータのヘッダと終了点 FRAME_HEAD = b"\x00\xFF" FRAME_TAIL = b"\xCC" #Serial接続のインスタンス ser = serial.Serial(port,115200) #データ読み込み def uart_readBytes(): return ser.read() #受信データの有無を確認 def uart_hasData(): return ser.in_waiting #セットしたATコマンド送信 def uart_sendCmd(cmd): ser.write(cmd) #解像度設定。変更したときは、一度立ち上げ直さないと反映されない。 #1:100x100 2:50x50 3:25x25 #uart_sendCmd(b"AT+BINN=1\r") #データの出力先設定 #0:all off 1:lcd 2:usb 3:lcd,usb 4:uart #5:lcd,usb 6:usb,uart 7:lcd,usb,uart uart_sendCmd(b"AT+DISP=7\r") #FPS設定。通信速度の方ボトルネックになりやすい? #1-19 uart_sendCmd(b"AT+FPS=5\r") #ヒートマップ用色設定(もとの設定を反転させて使用 jetcolorsR = [ (128, 0, 0), (132, 0, 0), (136, 0, 0), (140, 0, 0), (144, 0, 0), (148, 0, 0), (152, 0, 0), (156, 0, 0), (160, 0, 0), (164, 0, 0), (168, 0, 0), (172, 0, 0), (176, 0, 0), (180, 0, 0), (184, 0, 0), (188, 0, 0), (192, 0, 0), (196, 0, 0), (200, 0, 0), (204, 0, 0), (208, 0, 0), (212, 0, 0), (216, 0, 0), (220, 0, 0), (224, 0, 0), (228, 0, 0), (232, 0, 0), (236, 0, 0), (240, 0, 0), (244, 0, 0), (248, 0, 0), (252, 0, 0), (255, 0, 0), (255, 4, 0), (255, 8, 0), (255, 12, 0), (255, 16, 0), (255, 20, 0), (255, 24, 0), (255, 28, 0), (255, 32, 0), (255, 36, 0), (255, 40, 0), (255, 44, 0), (255, 48, 0), (255, 52, 0), (255, 56, 0), (255, 60, 0), (255, 64, 0), (255, 68, 0), (255, 72, 0), (255, 76, 0), (255, 80, 0), (255, 84, 0), (255, 88, 0), (255, 92, 0), (255, 96, 0), (255, 100, 0), (255, 104, 0), (255, 108, 0), (255, 112, 0), (255, 116, 0), (255, 120, 0), (255, 124, 0), (255, 128, 0), (255, 132, 0), (255, 136, 0), (255, 140, 0), (255, 144, 0), (255, 148, 0), (255, 152, 0), (255, 156, 0), (255, 160, 0), (255, 164, 0), (255, 168, 0), (255, 172, 0), (255, 176, 0), (255, 180, 0), (255, 184, 0), (255, 188, 0), (255, 192, 0), (255, 196, 0), (255, 200, 0), (255, 204, 0), (255, 208, 0), (255, 212, 0), (255, 216, 0), (255, 220, 0), (255, 224, 0), (255, 228, 0), (255, 232, 0), (255, 236, 0), (255, 240, 0), (255, 244, 0), (255, 248, 0), (255, 252, 0), (254, 255, 1), (250, 255, 6), (246, 255, 10), (242, 255, 14), (238, 255, 18), (234, 255, 22), (230, 255, 26), (226, 255, 30), (222, 255, 34), (218, 255, 38), (214, 255, 42), (210, 255, 46), (206, 255, 50), (202, 255, 54), (198, 255, 58), (194, 255, 62), (190, 255, 66), (186, 255, 70), (182, 255, 74), (178, 255, 78), (174, 255, 82), (170, 255, 86), (166, 255, 90), (162, 255, 94), (158, 255, 98), (154, 255, 102), (150, 255, 106), (146, 255, 110), (142, 255, 114), (138, 255, 118), (134, 255, 122), (130, 255, 126), (126, 255, 130), (122, 255, 134), (118, 255, 138), (114, 255, 142), (110, 255, 146), (106, 255, 150), (102, 255, 154), (98, 255, 158), (94, 255, 162), (90, 255, 166), (86, 255, 170), (82, 255, 174), (78, 255, 178), (74, 255, 182), (70, 255, 186), (66, 255, 190), (62, 255, 194), (58, 255, 198), (54, 255, 202), (50, 255, 206), (46, 255, 210), (42, 255, 214), (38, 255, 218), (34, 255, 222), (30, 255, 226), (26, 255, 230), (22, 255, 234), (18, 255, 238), (14, 255, 242), (10, 255, 246), (6, 255, 250), (2, 255, 254), (0, 252, 255), (0, 248, 255), (0, 244, 255), (0, 240, 255), (0, 236, 255), (0, 232, 255), (0, 228, 255), (0, 224, 255), (0, 220, 255), (0, 216, 255), (0, 212, 255), (0, 208, 255), (0, 204, 255), (0, 200, 255), (0, 196, 255), (0, 192, 255), (0, 188, 255), (0, 184, 255), (0, 180, 255), (0, 176, 255), (0, 172, 255), (0, 168, 255), (0, 164, 255), (0, 160, 255), (0, 156, 255), (0, 152, 255), (0, 148, 255), (0, 144, 255), (0, 140, 255), (0, 136, 255), (0, 132, 255), (0, 128, 255), (0, 124, 255), (0, 120, 255), (0, 116, 255), (0, 112, 255), (0, 108, 255), (0, 104, 255), (0, 100, 255), (0, 96, 255), (0, 92, 255), (0, 88, 255), (0, 84, 255), (0, 80, 255), (0, 76, 255), (0, 72, 255), (0, 68, 255), (0, 64, 255), (0, 60, 255), (0, 56, 255), (0, 52, 255), (0, 48, 255), (0, 44, 255), (0, 40, 255), (0, 36, 255), (0, 32, 255), (0, 28, 255), (0, 24, 255), (0, 20, 255), (0, 16, 255), (0, 12, 255), (0, 8, 255), (0, 4, 255), (0, 0, 255), (0, 0, 252), (0, 0, 248), (0, 0, 244), (0, 0, 240), (0, 0, 236), (0, 0, 232), (0, 0, 228), (0, 0, 224), (0, 0, 220), (0, 0, 216), (0, 0, 212), (0, 0, 208), (0, 0, 204), (0, 0, 200), (0, 0, 196), (0, 0, 192), (0, 0, 188), (0, 0, 184), (0, 0, 180), (0, 0, 176), (0, 0, 172), (0, 0, 168), (0, 0, 164), (0, 0, 160), (0, 0, 156), (0, 0, 152), (0, 0, 148), (0, 0, 144), (0, 0, 140), (0, 0, 136), (0, 0, 132), (0, 0, 128) ] jetcolors = tuple(reversed(jetcolorsR)) #デプス画像表示 def imgTransform(frame): resR = unpack("B", frame[14:15])[0] resC = unpack("B", frame[15:16])[0] res = (resR, resC) frameData = [unpack("B", frame[20+i:21+i])[0] for i in range(0, frameDataLen, 1)] resR = res[0] resC = res[1] depthImage = [] for y in range(resR): col = [] for x in range(resC): pixel_cmap_rgb= jetcolors[frameData[y*resR + x]] col.append(pixel_cmap_rgb) depthImage.append(col) npDepthImage = np.array(depthImage,dtype=np.uint8) return npDepthImage fourcc = cv2.VideoWriter_fourcc('a','v','c','1') out = cv2.VideoWriter('output.mp4', fourcc, fps, (w, h)) while True: movie = [] #データが届いているかチェック if not uart_hasData(): continue #データ読み取り rawData += uart_readBytes() #フレームのヘッダを探してヘッダの前のデータを排除 idx = rawData.find(FRAME_HEAD) if idx < 0: continue rawData = rawData[idx:] #データが規定の長さになるまでデータを取り込み try: dataLen = unpack("H", rawData[2: 4])[0] except: continue frameLen = len(FRAME_HEAD) + 2 + dataLen + 2 frameDataLen = dataLen - 16 if len(rawData) < frameLen: continue #フレームデータを切り出し frame = rawData[:frameLen] rawData = rawData[frameLen:] #check sum _sum = frame[-2] frameTail = frame[-1] if frameTail != 0xdd and _sum != sum(frame[:frameLen - 2]) % 256: continue #フレームデータをopencv形式に変換 DImage = imgTransform(frame) DImage = cv2.resize(DImage, (w, h)) cv2.imshow("frame",DImage) out.write(DImage) if cv2.waitKey(1) & 0xFF == ord('q'): break cv2.destroyAllWindows() out.release() ser.close() ``` # まとめ 実行するとかなり処理が遅く、fpsは1を少し超えるくらい。遅延も1秒以上感じます。 処理量はそれほど重くないはずなので、どの処理がボトルネックになっているか調査のうえ改良が必要です。