安価なToFセンサ MaixSense-A010

MaixSense-A010 (または MetaSense A010 ) という3Dデプスセンサ(ToFセンサ)がSipeedから発売されていたので使用してみました。
100x100の解像度で、2.5mまでの距離を8bit精度で、最大19psで取得できるという性能を持っています。
接続はUSB Type-Cとシリアルを利用できます (後述しますが、シリアル接続だと100x100 最大fpsでの利用は厳しめです)。
下記写真のようにLCDを搭載したモデルもあり、これだと他にマイコン等を使うことなく手軽に動作や視野の確認ができて便利です。

下記はAmazon.co.jpのリンクですが、慣れている人はSeeed studioやAliExpress等で購入した方が安く(おそらく5000円前後くらいで)購入できます。

Sipeed MaixSense A010

使い方

PC用のツールで画像取得でき、こちらのWikiに方法がまとまっています。
が、今回はこちらは使わず、プログラムからデータを取得してみました。

USB接続すると、2つのシリアルポート(FTDI)として認識されます。そのうち若い番号のほうでコマンド送信やデータの受信をします。なおこのUSBシリアルでは通信速度設定などは無視されます。

$ lsusb
Bus 001 Device 004: ID 0403:6001 Future Technology Devices International, Ltd FT232 Serial (UART) IC
Bus 001 Device 011: ID 0403:6010 Future Technology Devices International, Ltd FT2232C/D/H Dual UART/FIFO IC

ATコマンドを送って、転送開始や解像度・fps設定などを指示をします。
最も単純には、データ送信先としてUSBを指定し、ISP(画像信号処理)を開始させる2つのコマンドを送ればOKです。

MaixSense-A010 Development - Sipeed Wiki

この時注意点として、データ送信先としてシリアルを指定すると、1fps程度でしかデータが取得できなくなります。これはシリアルの通信速度がデフォルトだと115200bpsであり、これでは100x100x8bitのデータはせいぜい1秒に1枚程度を送るのが限界だからで、USBやLCDもそれに引きづられるためです。シリアルの速度を AT+BAUD=コマンドで上げればfpsも上がりますが、ESP32マイコンのHardwareSerialで受信してみた限りでは230400bpsが限界で、それ以上にしてしまうと同期が取れずに正常にデータが受信できませんでした。このためシリアルしか接続手段のないマイコン等では解像度またはfpsを下げないと厳しいと思われます。

USBであれば通信速度の問題はありませんので、マイコンを使うならRaspberryPi等USBが使えるものが良いでしょう。ただしちょくちょくdmesgを見ているとパケットロストのようなエラーが出たりハングしたりするので、下記プログラムではエラーチェックやリトライは厳しめにしています。

データフォーマットも上記にWikiにある通りですが、ざっくり下記のようなフォーマットで流れてきます。

<ff> <00> <size (little endian 2byte)> <header (16B)> <data (10000B)> <checksum (1B)> <0xdd>

これをパースして、WebSocketに流すプログラムをRubyで書いてみました。

#!/usr/bin/ruby
require 'serialport'
require 'em-websocket'
require 'timeout'

def open_serial_port(path)
  sp = SerialPort.new(path, 115200, 8, 1, 0)
  sp.read_timeout = 2000
  sp
end

def wait_ok(sp)
  loop do
    resp = sp.readline.chomp
    puts resp
    break if resp == "OK"
  end
end

def stop(sp)
  sp.puts("AT+ISP=0\r")
  exit
end

raise "specify serial port path\n" unless ARGV[0]
sp = open_serial_port(ARGV[0])
sp.puts("AT+ISP=0\r")
wait_ok(sp)
sp.puts("AT+DISP=3\r")
wait_ok(sp)
sp.puts("AT+ISP=1\r")
wait_ok(sp)

[:INT, :TERM, :HUP, :PIPE].each do |signal|
  Signal.trap(signal) { stop(sp) }
end

EM.run do
  @channel = EM::Channel.new
  @last_update = Time.now

  EM::WebSocket.run(:host => '0.0.0.0', :port => 8080) do |ws|
    ws.onopen do
      sid = @channel.subscribe { |msg| ws.send(msg) }
      ws.onclose { @channel.unsubscribe(sid) }
    end
  end

  EM.defer do
    loop do
      b = sp.read(1)
      if b&.ord == 0x00 && sp.read(1)&.ord == 0xff
        @last_update = Time.now
        chksum = 0xff
        s1 = sp.read(1).ord
        s2 = sp.read(1).ord
        chksum += s1 + s2
        size = s1 + s2 * 256
        if size == 10016
          packet = sp.read(size)
          if packet.size == size
            chksum += packet.bytes.sum
            b = sp.read(1).ord
            if b == chksum & 0xff
              @channel.push(packet.unpack('H*')[0])
            else
              warn "** invalid chksum: #{b} != #{chksum & 0xff}"
            end
            unless sp.read(1) != 0xff
              warn "** invalid end of packet"
            end
          else
            warn "** couldn't read packet: #{packet.size} != #{size}"
          end
        else
          warn "** invalid packet length: #{size} **"
        end
      end
    rescue IOError => e
      warn "IOError: #{e}"
      sleep 0.1
    end
  end

  EventMachine::PeriodicTimer.new(2) do
    if @last_update < Time.now - 2
      warn "no data for 2sec. reopen ..."
      sp.close
      sp = open_serial_port(ARGV[0])
    end
  end
end


これをシリアルボートのパス ( /dev/ttyUSBx 等 ) をつけて起動させ、下記HTMLファイルをブラウザで開くとWebSocket接続させて取得データを描画させることができます。

<html>
  <body>
  <canvas id="data" width="400" height="400" style="border: solid 1px black;"></canvas>
    <script>
      var socket = new WebSocket('ws://xxx.xxx.xxx.xxx:8080'); // ← ここに上記サーバーのアドレスを指定する
      socket.addEventListener('message', (event) => {
        var canvas = document.createElement('canvas');
        var ctx = canvas.getContext('2d');
        var bmp = new ImageData(100, 100);
        for (i = 0; i < 10000; i++) {
          var data = parseInt(event.data[i*2 + 32] + event.data[i*2 + 33], 16);
          bmp.data[i * 4 + 0] = data;
          bmp.data[i * 4 + 1] = data;
          bmp.data[i * 4 + 2] = data;
          bmp.data[i * 4 + 3] = 255;
        }
        ctx.putImageData(bmp, 0, 0, 0, 0, 400, 400);
        var canvas4 = document.getElementById('data');
        var ctx4 = canvas4.getContext('2d');
        ctx4.drawImage(canvas, 0, 0, 100, 100, 0, 0, 400, 400);
      });
    </script>
  </body>
</html>

下記のような感じでリアルタイムに深度画像がグレイスケール表示されます。これはMacBook Proを持ちながらセンサーに手をかざしてみているところですが、Appleロゴは鏡面状になっているので白く飛んでいるのが面白いですね。ToFセンサなので、もちろん真っ暗な状況でも距離画像が撮れます。