激安LiDAR(Camsense X1)を使ってみる

#はじめに
AliExpressを徘徊してると、なんと$19(送料込み)でLiDARが売ってました。((追記)もう売ってないみたいです。)
https://ja.aliexpress.com/item/4000632680397.html

今回、その激安LiDARを買ってみました。

公式には通信仕様やライブラリ等は公開されていないようです。

(追記)工場テスト品なるものが$5.93(+送料$4.07)で売られているのも見つけました。((追記)こちらももう売ってないみたいです。)
https://www.aliexpress.com/item/4001253880158.html

#激安LiDARの正体
AliExpressのページにはメーカー等も書かれていませんが、
画像中に「Camsense X1」というシールがあったのでこれがセンサの型式のようです。
Camsense X1を検索すると、Camsenseというブランドが2017年に発売した製品のようです。
http://en.camsense.cn/index.php?c=article&id=116
#仕様
Camsenseのページには以下のように簡単な仕様が書かれています。

仕様項目	仕様値
測距範囲	0.08 ～ 8m
繰り返し精度	0.5%未満
絶対精度	2%未満
測定周波数	2080Hz超

なお、測定周波数というのは回転周波数ではなく、1秒間に距離データを何ステップ取れるかという意味です。
その他使ってみて分かってきた仕様は以下の通りです。

仕様項目	仕様値
電圧	5V
消費電流	0.4A以下
角度分解能	0.75度くらい
回転周期	0.2秒くらい
通信	UART通信(115200bps)
UARTレベル	3.3V

角度分解能は一定ではなく、手で触ったりしてモータ回転を遅くしてやると角度分解能が0.5度とかに細かくなったります。
データの取得周期は回転とは同期しておらず、時間で決まっているようです。

#接続
外部と接続する線はVCC、GND、TXの3つです。コネクタ等はついてなかったので頑張ってつなぎました。

PCで通信するのであれば、UART通信は直接できないのでUSB-UART変換機が必要です。
私はこんなのを使いました。(だいぶ前に買ったものでどこで買ったのかは忘れましたが)

Raspberry Piなら3.3VのUARTを持ってるのでそのままで使えるはずです。

CamsenseX1とPCとの接続はこうします。

Camsense X1側	PC側
VCC	5.0V
GND	GND
TX	RX

#注意
Windowsの場合、シリアルポートから連続的にデータが来る場合、シリアルマウスとして認識されてしまい、マウスカーソルがあっちこっちに行ってやっちゃいけない操作がされてしまったりします。
そのため、Windowsの場合はシリアルマウスとして認識する機能を切っておくのがいいです。
シリアルマウスというのはUSBやPS/2以前に使われていたすっごい古いマウスで、現在のマウスが使えなくなるわけではありません。
やり方としては簡単で、レジストリエディタで
HKEY_LOCAL_MACHINE\System\CurrentControlSet\Services\sermouse
のStartキーの値が3になってると思うので、それを4にするだけです。
https://stackoverflow.com/questions/9226082/device-misdetected-as-serial-mouse

#通信プロトコル
通信プロトコルについて解析されている方がgithubで説明してくれていたので参考にさせていただきました。
https://github.com/Vidicon/camsense-X1

以下のような距離と強度データが8ステップずつ入った36バイトのパケットになっていて、
それが延々と出力されています。

項目	バイト数	備考
ヘッダ	4	0x55, 0xAA, 0x03, 0x08 固定
回転速度	2(リトルエンディアン)	value / 64 = rpm
開始角度	2(リトルエンディアン)	(value - 40960) / 64 = degree
距離0	2(リトルエンディアン)	mm
強度0	1	単位は不明
距離1	2(リトルエンディアン)	mm
強度1	1	単位は不明
...	...	...
距離7	2(リトルエンディアン)	mm
強度7	1	単位は不明
終了角度	2(リトルエンディアン)	(value - 40960) / 64 = degree
CRC？	2	?

最後の2バイトはCRCらしいですが、いくつかの生成多項式で計算してみたり、計算範囲からヘッダを抜いてみたりしてみましたが合うものは見つかりませんでした。

#データ取得プログラム(Python)
PythonでCamsense X1のデータを取得して描画するコードを書いてみました。
githubにも置いてます。
https://github.com/junp007/CamsenseX1

角度分解能が回転速度によって変化するので、1周のデータ数が決まらないのですが、安定時の角度分解能の1周分のデータ数を確保しています。
動作にはpyserialとmatplotlibが必要です。

動作確認環境

ソフト	バージョン
python	3.8.2
pyserial	3.4
matplotlib	3.2.2

camsenseX1.py

import matplotlib.pyplot as plt
import math
import serial
import sys
import struct
import time
import threading

class Capture:
    def __init__(self, serialPort, dataSize = 460, isInvert = True):
        self.theta = [0] * dataSize
        self.distance = [0] * dataSize
        self.intensity = [0] * dataSize
        self.writePos = 0
        self.serial = serial.Serial(port = serialPort, baudrate = 115200)
        self.dataSize = dataSize
        self.thread = threading.Thread(target = self.getData)
        self.lock = threading.Lock()
        self.isInvert = isInvert
        self.dataObtained = False
        self.rpm = 0

    def getDataUnit(self):
        # まずはヘッダまで読み捨てる
        header = b"\x55\xAA\x03\x08"
        headerPos = 0
        while True:
            tmp = self.serial.read(1)
            if tmp[0] == header[headerPos]:
                headerPos += 1
                if headerPos == len(header):
                    break
            else:
                headerPos = 0

        tmp = self.serial.read(4)
        # "<" : リトルエンディアン, "H" : 2バイト符号なしデータ, "B" : 1バイト符号なしデータ
        (rotationSpeedTmp, startAngleTmp) = struct.unpack_from("<2H", tmp)
        self.rpm = rotationSpeedTmp / 64
        startAngle = (startAngleTmp - 0xa000) / 64
        # 距離、強度データを格納する配列を用意
        distanceTmp = [0] * 8
        intensityTmp = [0] * 8
        for i in range(8):
            tmp = self.serial.read(3)
            (distanceTmp[i], intensityTmp[i]) = struct.unpack_from("<HB", tmp)
        tmp = self.serial.read(4)
        (endAngleTmp, crc) = struct.unpack_from("<2H", tmp)
        endAngle = (endAngleTmp - 0xa000) / 64

        return (distanceTmp, intensityTmp, startAngle, endAngle)

    def getData(self):
        preStartAngle = 0
        while True:
            (distanceTmp, intensityTmp, startAngle, endAngle) = self.getDataUnit()

            # 0度付近の場合は開始角度と終了角度の大小関係が逆になることがあるので、終了角度に360度足して大小関係を維持する
            if endAngle < startAngle:
                endAngle += 360

            # 開始角度が小さくなったら0度の場所なのでデータ更新フラグを立てる
            if (startAngle - preStartAngle < 0):
                self.dataObtained = True
            preStartAngle = startAngle

            # 角度をラジアンに変換
            startAngleRad = startAngle * math.pi / 180 * (-1 if self.isInvert else 1)
            endAngleRad = endAngle * math.pi / 180 * (-1 if self.isInvert else 1)
            # 1ステップ当たりの角度を計算
            angleIncrement = (endAngleRad - startAngleRad) / len(distanceTmp)
            # 排他制御開始
            self.lock.acquire()
            for i in range(len(distanceTmp)):
                self.theta[self.writePos] = startAngleRad + angleIncrement * i
                self.distance[self.writePos] = distanceTmp[i]
                self.intensity[self.writePos] = intensityTmp[i]
                self.writePos += 1
                if self.writePos >= self.dataSize:
                    self.writePos = 0
            # 排他制御終了
            self.lock.release()

    def run(self):
        self.thread.start()

    def stop(self):
        self.thread.stop()

if __name__ == "__main__":
    if len(sys.argv) == 1:
        print("You must specify serial port! ex) " + sys.argv[0] + " COM2")
        quit()

    # 極座標のグラフを生成
    dist = plt.subplot(111, polar = True)
    # 表示する最大距離の初期値を2000に設定
    rmax = 2000

    capture = Capture(sys.argv[1], dataSize = 480, isInvert = True)
    capture.run()
    preTime = time.time()

    while True:
        if capture.dataObtained:
            # 描画を初期化
            plt.cla()

            # 排他制御開始
            capture.lock.acquire()
            # 距離データをプロット(散布図として描画)
            dist.scatter(list(capture.theta), list(capture.distance), c = "blue", s = 5)
            # 強度データをプロット(線でつなげて描画)
            dist.plot(list(capture.theta), list(capture.intensity), c = "orange", linewidth = 1)
            # データの描画をしたのでデータ取得フラグを下ろす
            capture.dataObtained = False
            # 排他制御終了
            capture.lock.release()

            # 画面の上を0度にする
            dist.set_theta_offset(math.pi / 2)
            # 表示する距離値の最大値を設定
            dist.set_rmax(rmax)
            # 描画処理
            plt.pause(0.01)
            # 現在設定されている表示最大距離値を取得
            rmax = dist.get_rmax()

        else:
            time.sleep(0.01)

データを描画している様子はこんな感じです。
青い点で距離データを描画していて、オレンジの線が強度データを表しています。

#その他公開されているプログラム
ROS1用のノード : https://github.com/Vidicon/camsense_driver Made by Vidicon (Bram Fenijn)
ROS2用のノード : https://github.com/rossihwang/ros2_camsense_x1 Made by rossihwang
STM32用プログラム：https://github.com/anhui1995/Camsense_X1

#おわりに
この性能のLiDARが$19で購入できるのは驚きです。
Ethernet接続のLiDARでは安価なマイコンで処理するのはなかなか大変ですが、
Camsense X1であればUARTなので安価なマイコンでも簡単に処理できます。
回転周期が結構遅いですし、取得できる距離も短いので素早く動くロボット等に使うには厳しいと思いますが、
趣味で使う分には十分な性能だと思います