YDLidar X4ProをRDK X3 Piで動かす

2024年9月23日 16:32

さあ今日も人類の何人が必要としているかわからないが地球のどこかには必ず必要としている人がいるであろう情報を書いておくよ!主に想定読者は未来の自分。

さて、RDK X3を買ったはいいが、今度こそ何かに使わないとバチが当たる。
ロボットを作ろうと決めたのはいいとして、ロボットといえばLidarがないと話にならない。まあなくてもいいんだけど画像認識だけあってLidarがないとか中途半端じゃないですか。

サンフランシスコで乗って感動した自動運転車も頭にでっかいLidarがついてる。21世紀のロボットはLidarがなければ作れないのだ。

頭についてるのがLidar。前後左右のバンパーにも多分Lidarっぽいのがついてるんじゃないかな
イメージセンサーかもしれないけどそれにしてはデカすぎる

というわけで、秋月に行ってYDLidar X4 Proを買ってみた。在庫があったのと、最大10メートルくらいまで見れるため。

探知範囲10メートルもいらないと思うかもしれないが、技研スタシジオは10メートルくらいあるかもしれないし、あってそんなに困るというわけではない。30メートルは流石にいらないと思うが5メートルは狭すぎる。そんな感じ。

さて、YDLidarシリーズはUSB変換基盤がついてくるのでUARTを直接読まなくてもいいようになっている。嬉しい。

YDLidarをUSB経由でRDK X3に繋ぐといきなり頭がグルグル回転し始めてやばい。こんな勢いで回転するんかい!

こんな駆動部がぐるぐる回る機械とかできるだけ触りたくないのだがこれがメカトロニクスという奴なのか。まあ受け入れるしかない。いつかもっとスマートな量子センサーベースのLidar風のものが登場するを待たなければならない。あと半世紀くらいか。

さて、こいつを買ってきたはいいが使い方がわからん。
いや、わからんでもないんだけど世の中にはROS(Robot OS)ベースのものしかないので俺の役に立たない。

ROS使ってまでなんかしたいわけじゃないのよ。ちょっとした障害物を避けられればそれでいい。

Pythonからお手軽にアホみたいな感覚で使いたいのよ。
ところがROSを使わないLidarの使い方の情報がほぼ皆無。地球上にはLidar使いはROSを使うような大学院生しかいないのか。俺のような日曜工作研究者にも気軽に触らせろ!

もちろんSDKは用意されている。

SDKの言うがままに使ったら地獄を見た。
とにかくエラー。

いずれ地球上の誰かが同じエラーに直面して涙で枕を濡らすだろうから検索しやすいようにコピペしておく

__   ______  _     ___ ____    _    ____  
\ \ / /  _ \| |   |_ _|  _ \  / \  |  _ \ 
 \ V /| | | | |    | || | | |/ _ \ | |_) | 
  | | | |_| | |___ | || |_| / ___ \|  _ <  
  |_| |____/|_____|___|____/_/   \_\_| \_\ 

[0] ydlidar /dev/ttyS3
[1] ydlidar1.3 /dev/ttyUSB0
Please select the lidar port:1
[YDLIDAR] SDK initializing
[YDLIDAR] SDK has been initialized
[YDLIDAR] SDK Version: 1.2.6
[YDLIDAR] Lidar successfully connected [/dev/ttyUSB0:230400]
[YDLIDAR] Error, cannot retrieve YDLidar health code: ffffffff
[YDLIDAR] Fail to get baseplate device information
[YDLIDAR] Lidar init success, Elapsed time 2661 ms
[YDLIDAR] Failed to start scan mode: ffffffff

とにかく腹立つのがこの「[YDLIDAR] Error, cannot retrieve YDLidar health code: ffffffff」と言うエラー。接続できてるっぽいのにヘルスがFFFFFFFFってどう言うことだよ。むしろ健康的じゃないか。それともこれは-1と言う意味か?(二進数だから)

まあとにかくこれに二日くらい悩まされて、検索してもROSのパラメータファイルをあれこれしろ、みたいな寝言めいた助言しか出てこない。お呼びじゃないっつの。

色々調べた結果、まあ簡単に言うと、YDLidar X4 Proは、シングルチャンネルで動いてると言うこと。違うかもしれないけどシングルチャンネルにしたらあらゆるサンプルコードが正常に動いた。

$ sudo ./tri_test 
__   ______  _     ___ ____    _    ____  
\ \ / /  _ \| |   |_ _|  _ \  / \  |  _ \ 
 \ V /| | | | |    | || | | |/ _ \ | |_) | 
  | | | |_| | |___ | || |_| / ___ \|  _ <  
  |_| |____/|_____|___|____/_/   \_\_| \_\ 

[0] ydlidar /dev/ttyS3
[1] ydlidar1.3 /dev/ttyUSB0
Please select the lidar port:1
Baudrate:
[0] 115200
[1] 128000
[2] 150000
[3] 153600
[4] 230400
[5] 460800
[6] 512000
Please select the lidar baudrate:1
Whether the Lidar is one-way communication [yes/no]:yes
[YDLIDAR] SDK initializing
[YDLIDAR] SDK has been initialized
[YDLIDAR] SDK Version: 1.2.6
[YDLIDAR] Lidar successfully connected [/dev/ttyUSB0:128000]
[YDLIDAR] Lidar running correctly! The health status: good
[YDLIDAR] Fail to get baseplate device information
[YDLIDAR] Lidar init success, Elapsed time 622 ms
[YDLIDAR] Start to getting intensity flag
[YDLIDAR] Auto set intensity 0
[YDLIDAR] End to getting intensity flag
[YDLIDAR] Create thread 0xA73D71C0
[YDLIDAR] Successed to start scan mode, Elapsed time 2505 ms
[YDLIDAR] Module device info
Firmware version: 3.1
Hardware version: 3
Model: S2PRO
Serial: 2021102600030461
[YDLIDAR] Single Fixed Size: 1400
[YDLIDAR] Sample Rate: 5.00K
[YDLIDAR] Successed to check the lidar, Elapsed time 2341 ms
[2024-09-23 14:58:45][info] [YDLIDAR] Now lidar is scanning...
Fail to get baseplate device info
Scan received [1400] points scanFreq [3.50]
Time consuming [5757] from initialization to parsing to point cloud data
Scan received [1395] points scanFreq [3.50]
Scan received [1394] points scanFreq [3.50]
Scan received [1394] points scanFreq [3.50]
Scan received [1393] points scanFreq [3.50]
Scan received [1399] points scanFreq [3.50]
Scan received [1408] points scanFreq [3.50]
Scan received [1403] points scanFreq [3.50]
Scan received [1403] points scanFreq [3.50]
Scan received [1397] points scanFreq [3.50]

ただ、「シングルチャンネル」と言うのが、別のサンプルコードでは「one way communication」と言う表現になっていたと言うのがこの話のややこしさを物語る。あのな。用語は統一しろ。中国の人々よ。ある場面ではシングルチャンネルで別の場面では片方向通信とバラバラな呼称をするのは混乱の元だぞ。

そしてX4 Proは128000bps(baudrate)じゃないと正く通信できない。
したがって、python/examplesにあるtof_test.pyを以下のように変更する。

import os
import ydlidar
import time
import sys

if __name__ == "__main__":
    ydlidar.os_init();
    ports = ydlidar.lidarPortList();
    port = "/dev/ttyUSB0";
    for key, value in ports.items():
        port = value;
    laser = ydlidar.CYdLidar();
    laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
    laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000); #ここ
    laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF);
    laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
    laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
    laser.setlidaropt(ydlidar.LidarPropSampleRate, 5);
    laser.setlidaropt(ydlidar.LidarPropSingleChannel, True); #ここ

    ret = laser.initialize();
    print(ret)
    if ret:
        ret = laser.turnOn();
        scan = ydlidar.LaserScan()
        print(scan)
        while ret and ydlidar.os_isOk() :
            r = laser.doProcessSimple(scan);
            if r and scan.config.scan_time>0:
                print("Scan received[",scan.stamp,"]:",scan.points.size(),"ranges is [",1.0/scan.config.scan_time,"]Hz");
            else :
                print("Failed to get Lidar Data.")
            time.sleep(0.05);
        laser.turnOff();
    laser.disconnecting();

これで無事動作するようになる。
ついでに、plot_tof_test.pyも変更する。

import os
import ydlidar
import time
import sys
from matplotlib.patches import Arc
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np

RMAX = 32.0


fig = plt.figure()
#fig.canvas.set_window_title('YDLidar LIDAR Monitor') #ここ
lidar_polar = plt.subplot(polar=True)
lidar_polar.autoscale_view(True,True,True)
lidar_polar.set_rmax(RMAX)
lidar_polar.grid(True)
ports = ydlidar.lidarPortList();
port = "/dev/ydlidar";
for key, value in ports.items():
    port = value;
    
laser = ydlidar.CYdLidar();
laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000) #ここ
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF);
laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
laser.setlidaropt(ydlidar.LidarPropSampleRate, 20);
laser.setlidaropt(ydlidar.LidarPropSingleChannel, True); #ここ
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0);
laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01);
scan = ydlidar.LaserScan()

def animate(num):
    
    r = laser.doProcessSimple(scan);
    if r:
        angle = []
        ran = []
        intensity = []
        for point in scan.points:
            angle.append(point.angle);
            ran.append(point.range);
            intensity.append(point.intensity);
        lidar_polar.clear()
        lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', alpha=0.95)

ret = laser.initialize();
if ret:
    ret = laser.turnOn();
    if ret:
        ani = animation.FuncAnimation(fig, animate, interval=50)
        plt.show()
    laser.turnOff();
laser.disconnecting();
plt.close();

これで動作したぜ。バカみたいなPythonコードでLidarが動作だぜ!

これで自動運転に近づいた。
あとはクローラーを動かすためのモータードライバーとの接続だ!
秋月で12ボルトバッテリーを買ってきたのはいいが、下手すりゃ爆発するぜ!怖いぜ!