elchika elchika
ログイン/登録
airpocketのアイコン画像
airpocket 2022年11月16日作成 (2022年11月16日更新)
セットアップや使用方法 セットアップや使用方法 閲覧数 1275
airpocket 2022年11月16日作成 (2022年11月16日更新) セットアップや使用方法 セットアップや使用方法 閲覧数 1275

デプスセンサ Sipeed MS-A010をRaspberry PiでOpenCVする

デプスセンサ Sipeed MS-A010をRaspberry PiでOpenCVする

はじめに

前回の記事で、SipeedのデプスセンサMS-A010をWindows PCに接続してデプス情報を可視化してみました。
今回はMS-A010をRaspberry Piに接続してOpenCVで可視化してみたいと思います。

MS-A010のクラファンサイト
MS-A010の公式サイト
ハードウェア情報
開発者用サイト

できたこと

デプスデータをopencvの画像配列に変換、連結してmp4として出力しました。
もちろん、ロボット制御などに使用する事もできます。

環境

都合によりテスト環境は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秒以上感じます。
処理量はそれほど重くないはずなので、どの処理がボトルネックになっているか調査のうえ改良が必要です。

airpocketのアイコン画像
airpocket
電子工作、プログラミング、AI、DIY、XR、IoT M5Stack / Raspberry Pi / Arduino / spresense / K210 / ESP32 / Maix / maicro:bit / oculus / Jetson Nano / minipupper etc
ログインしてコメントを投稿する

投稿者の人気記事