Livox Lidar
最近、Livoxという会社が作っているLidarを入手しました。
https://www.livoxtech.com/
中国？の会社のようで、とにかく低価格で実用的な性能を備えたLidarを作るということを得意としているようです。
私が入手したLidarは、Livox製品の中でも特に低価格なMid-70というものです。
https://www.livoxtech.com/mid-70
このMid-70は、なんと約10万円という破格の値段で売られています。
海外製品ですが、日本国内のいくつかの会社でも販売を行っていて、入手も比較的容易でした。
さらに、安かろう悪かろうではなく、自動車やロボットの自動走行に使うためのものとして開発しているようで、性能としてはかなり実用的のようです。
このLivox製のLidarは、Livoxから提供されているLivox Viewerというソフトを使えば、開封後すぐに点群データを取得できます。
https://www.livoxtech.com/jp/downloads
ただし、サポートしているOSが、Windows、または、Ubuntu-16.04（古くないか？）のみなうえ、GUIでの操作のみです。
なので、例えば、1時間おきに自動で点群データを観測したいとかはできないですし、野外で歩きながら観測したいとか考えるとノートPCを持ちながら…のようになり、あまり現実的ではないです。
今回は、Linuxのコマンドライン上でMid-70で点群データを保存するまでを備忘録を兼ねてまとめていきたいと思います。
これができれば、Raspberry Piや超小型PCでも点群データを保存できるようになるので、一気に未来が明るくなりそうです。
用意した環境
今回使ったものは、
- Lidar本体（Mid-70）
- PC（サバ太郎）
- Ubuntu-20.04（Server版）
のみです。サバ太郎は、小型・軽量なので安定動作するマイクロサーバーで気に入って使っていますが、別にRaspberry PiやノートPCでもよくある汎用PCならなんでもよいと思います。
ただし、仮想環境はMid-70がネットワーク経由での接続になる都合上、ネットワーク周りの設定に仮想環境の種類によっては手こずるかもしれません。
なお、Singularityは普通に動作しました。
OSは、今回はUbuntu を用意しましたが、この後説明するLivox-SDKを使った点群の保存までのみでよい場合は、C/C++コンパイラーが使える環境であれば、おそらくなんでもよいです。その先の、ROSを使った方法を行う場合は、ROSのサポートするOSでないとだめなため、Ubuntu 一択となります。
時刻同期（PTP）の設定（オプション）
いきなりオプションからのスタートですが、時刻同期を設定しない場合は、得られる点群データのタイムスタンプは、Lidarの電源投入からの経過時間となり、今後ほかのデータと点群を組み合わせて何かやりたいとなったときに困ることになると思います。
サポートされている時刻同期の方法は、PTP、GPS、PPSの３つのようですが、今回は、追加のハードウェアが一切不要なPTPによる時刻同期の設定を行っておこうと思います。
https://github.com/Livox-SDK/Livox-SDK/wiki/livox-device-time-synchronization-manual
方法は、Livox-SDKのWikiに詳しく書かれているのですが、結論からいうと、うまくいきませんでした。原因はさっぱりわからないのですが、用意したハードウェアが悪いのかもしれません。
アドホックにはなりますが、今回は、
https://medium.com/geekculture/livox-ptp-time-sync-on-raspberry-pi-4-bb31b78db838
こちらの方がやっている方法で設定しようと思います。
方法は、いたって簡単で、下記のコマンドで行けると思います。
sudo apt install ptpd
sudo ptpd -M -i enp3s0 -C
引数の意味は、
-
-Mマスターモードで動作させる。 -
-i enp3s0ネットワークインターフェースenp3s0を使う。 -
-Cforegraundで動作させる。
になります。ネットワークインターフェースは、Lidarを接続するのと同じLANポート（または、Lidarを接続するネットワークにつながるLANポート）のものを指定します。インターフェース名はifconfigコマンドなどで調べられます。
-Cオプションを消せばバックグラウンド実行になるので、Lidarに接続する前にあらかじめ実行しておくのが良いと思います。
これにて、PCの時刻をPTPマスタークロックに指定できるので、接続されるLidarも自動的にPCの時刻と同期された状態で稼働することになるそうです。
当たり前ですが、すでにPTPグラウンドマスタークロックをLidarと同じネットワーク内にお持ちであれば、このSectionは不要です。
ネットワークの設定
Lidarの取説にも書かれていますが、出荷状態では、ネットワークの設定は下記の通りになっています。
- LidarのIP:
192.168.1.1XX/24（XXは、シリアルナンバーの下2桁） - PCのIP：
192.168.1.50/24 - Gateway:
192.168.1.1
そのため、予めPCのIPアドレスを192.168.1.50/24に合わせておく必要があります。
Ubuntuの場合は、/etc/netplan/XX-hoge.yamlを作成することで設定できます。XXは、任意の整数で、この番号の大きいファイルが優先されます。
network:
ethernets:
enp3s0:
dhcp4: false
dhcp6: false
addresses: [192.168.1.50/24]
gateway4: 192.168.1.1
version: 2
yamlファイルを保存したら、設定を反映すればOKです。
sudo netplan apply
なお、Livox Viewerを使えばDHCPに変更することもできるようです（試していませんが）。
Livox-SDKのみを使った方法
Livox-SDKのgithubのReadmeで説明されている通りにインストール作業を行うことで問題なくLivox-SDKをインストールできました。
まずは、依存関係にあるライブラリのインストールです。といってもgccやcmakeくらいですが…。Ubuntu Serverにgccは初めから入っていたので、cmakeのみのインストールです。
sudo apt install cmake
あとは、Livox-SDKのコンパイルです。
git clone https://github.com/Livox-SDK/Livox-SDK.git
cd Livox-SDK
cd build && cmake ..
make
sudo make install
cmakeなので、-DCMAKE_INSTALL_PREFIXを指定すれば、任意のディレクトリをインストール先にすることができます。
さて、このようにしてインストールすると、Livox-SDK/build/sample/lidar_lvx_filesに実行ファイルlidar_lvx_sampleが作成されていると思います。
接続するLidarが一台のみの場合は、これを実行するだけで、Lidarから点群データを取得できます。
それでは、早速、Mid-70をPCに接続して試してみます。Mid-70の電源は、電源ケーブルを接続するだけで勝手に起動します。
cd Livox-SDK/build/sample/lidar_lvx_files
./lidar_lvx_sample
そうすると、おそらく、次のようなメッセージがコンソールに表示されると思います。
Livox SDK initializing.
Livox SDK has been initialized.
Livox SDK version 2.3.0 .
Start discovering device.
[2022-08-18 10:50:50.929] [console] [info] Broadcast broadcast code: xxxxxxxxxxxxxxx [device_discovery.cpp] [OnBroadcast] [150]
Receive Broadcast Code xxxxxxxxxxxxxxx
[2022-08-18 10:50:50.930] [console] [info] Broadcast code : xxxxxxxxxxxxxxx not add to connect [device_discovery.cpp] [OnBroadcast] [163]
[2022-08-18 10:50:51.930] [console] [info] Broadcast broadcast code: xxxxxxxxxxxxxxx [device_discovery.cpp] [OnBroadcast] [150]
Receive Broadcast Code xxxxxxxxxxxxxxx
[2022-08-18 10:50:51.930] [console] [info] Broadcast code : xxxxxxxxxxxxxxx not add to connect [device_discovery.cpp] [OnBroadcast] [163]
[2022-08-18 10:50:52.931] [console] [info] Broadcast broadcast code: xxxxxxxxxxxxxxx [device_discovery.cpp] [OnBroadcast] [150]
Receive Broadcast Code xxxxxxxxxxxxxxx
[2022-08-18 10:50:52.932] [console] [info] LocalIP: 192.168.1.50 [device_discovery.cpp] [OnBroadcast] [201]
[2022-08-18 10:50:52.932] [console] [info] DeviceIP: 192.168.1.194 [device_discovery.cpp] [OnBroadcast] [202]
[2022-08-18 10:50:52.932] [console] [info] Command Port: 55501 [device_discovery.cpp] [OnBroadcast] [203]
[2022-08-18 10:50:52.932] [console] [info] Data Port: 56001 [device_discovery.cpp] [OnBroadcast] [204]
[2022-08-18 10:50:52.933] [console] [info] New Device [device_discovery.cpp] [OnData] [99]
[2022-08-18 10:50:52.933] [console] [info] Handle: 0 [device_discovery.cpp] [OnData] [100]
[2022-08-18 10:50:52.933] [console] [info] Broadcast Code: xxxxxxxxxxxxxxx [device_discovery.cpp] [OnData] [101]
[2022-08-18 10:50:52.933] [console] [info] Type: 6 [device_discovery.cpp] [OnData] [102]
[2022-08-18 10:50:52.933] [console] [info] IP: 192.168.1.194 [device_discovery.cpp] [OnData] [103]
[2022-08-18 10:50:52.933] [console] [info] Command Port: 55501 [device_discovery.cpp] [OnData] [104]
[2022-08-18 10:50:52.933] [console] [info] Data Port: 56001 [device_discovery.cpp] [OnData] [105]
[2022-08-18 10:50:52.934] [console] [info] Send Command: Set 0 Id 2 Seq 3 [command_channel.cpp] [Send] [243]
[2022-08-18 10:50:52.935] [console] [info] Recieve Ack: Set 0 Id 2 Seq 3 [command_handler.cpp] [OnCommand] [118]
OnDeviceChange broadcast code xxxxxxxxxxxxxxx update type 0
[WARNING] Lidar sn: [xxxxxxxxxxxxxxx] Connect!!!
Device Working State 5
Device State Error Code 0X00000000
Device feature 0
[2022-08-18 10:50:52.935] [console] [info] Send Command: Set 0 Id 2 Seq 4 [command_channel.cpp] [Send] [243]
[2022-08-18 10:50:52.937] [console] [info] Recieve Ack: Set 0 Id 2 Seq 4 [command_handler.cpp] [OnCommand] [118]
firm ver: 10.7.0.0
[2022-08-18 10:50:52.989] [console] [info] Update State to 1, device connect true [device_manager.cpp] [UpdateDeviceState] [281]
OnDeviceChange broadcast code xxxxxxxxxxxxxxx update type 2
[WARNING] Lidar sn: [xxxxxxxxxxxxxxx] StateChange!!!
Device Working State 1
Device State Error Code 0X00000000
Device feature 0
[2022-08-18 10:50:52.989] [console] [info] Send Command: Set 1 Id 2 Seq 6 [command_channel.cpp] [Send] [243]
[2022-08-18 10:50:52.990] [console] [info] Send Command: Set 0 Id 4 Seq 7 [command_channel.cpp] [Send] [243]
[2022-08-18 10:50:52.992] [console] [info] Recieve Ack: Set 1 Id 2 Seq 6 [command_handler.cpp] [OnCommand] [118]
OnGetLidarExtrinsicParameter statue 0 handle 0 response 0
Start initialize lvx file.
[2022-08-18 10:50:52.992] [console] [info] Recieve Ack: Set 0 Id 4 Seq 7 [command_handler.cpp] [OnCommand] [118]
OnSampleCallback statues 0 handle 0 response 0
Finish save 0 frame to lvx file.
Finish save 1 frame to lvx file.
Finish save 2 frame to lvx file.
Finish save 3 frame to lvx file.
Finish save 4 frame to lvx file.
Finish save 5 frame to lvx file.
Finish save 6 frame to lvx file.
Finish save 7 frame to lvx file.
Finish save 8 frame to lvx file.
Finish save 9 frame to lvx file.
(同様に続く)
デフォルトでは、保存するフレーム数は、lvx_file_save_time(=10)×FRAME_RATE(=20)となっていて、すべて保存するとプログラムは終了します。ようするに、10秒間だけ記録するという設定になっています。
保存する秒数は、-tオプションで指定できます。
# 例）120秒間保存する場合
./lidar_lvx_sample -t 120
これにて、Livox-SDK/build/sample/lidar_lvx_filesに、YYYY-mm-dd_HH-MM-SS.lvxというファイルが作成されていると思います。この.lvxファイルが点群データになります。保存形式がLivox独自の形式のため、そのままでは様々なソフトウェアで読み込むことができないため、データ形式の変換が必要です。
変換方法は、様々ですが、ROSを使わない場合は、Livox Viewerを使う方法が最も簡単と思います。
保存したlvxファイルをWindowsPCなどにコピーして、Livox Viewerで開き、Tools->File Converterとすれば、Cloud Compareなどで読み込める形式のファイルに変換できます。なお、lvxファイルは時系列データとして点群が保存されますが、変換するlasファイルには、すべてのタイムステップにおける点群を積算したものを格納したファイルとなります。
複数台のLidarを保有していないので、試してはいませんが、もし複数のLidarが同一LAN内に接続されている場合は、デフォルトでは、すべてのLidarから点群を取得する動作となるそうです。もし、特定のLidarからの点群のみを取得したい場合は、-cオプションで、Broadcast Codeというものを指定します。このBroadcast Codeは、14桁のシリアルナンバー＋1桁の識別コードから成ります。詳しくは、Livox-SDK Wikiに書かれているので、そちらを確認してみてください。
（ちなみに、Mid-100とHub以外の機種の場合は、識別コードは1になると思います。）
timestampの確認
デフォルトのサンプルプログラムですと、timestampがきちんとPCと同期されているのかどうか確認することができないので、プログラムを少しだけ書き換えてきちんと同期できているかどうかを確かめてみます。
書き換えるプログラムは、Livox-SDK/sample/lidar_lvx_file/main.cppで、次のような変更を加えました。
--- a/sample/lidar_lvx_file/main.cpp
+++ b/sample/lidar_lvx_file/main.cpp
@@ -382,8 +382,12 @@ int main(int argc, const char *argv[]) {
printf("Point cloud packet is empty.\n");
break;
}
-
- printf("Finish save %d frame to lvx file.\n", i);
+
+ uint8_t timestamp_type = point_packet_list_temp.begin()->timestamp_type;
+ uint64_t cur_timestamp = *((uint64_t *)(point_packet_list_temp.begin()->timestamp));
+ double cur_seconds = cur_timestamp * 1e-9;
+
+ printf("Finish save %d frame to lvx file. type=%u t=%20.9lf\n", i, timestamp_type, cur_seconds);
lvx_file_handler.SaveFrameToLvxFile(point_packet_list_temp);
}
1パケットのデータが来るたびに、先頭のデータのタイムスタンプを表示するようにしただけです。
変更後は、次のようにコンパイルすればよいです。
cd Livox-SDK/build/sample/lidar_lvx_files
make
確認のために、まずは、ptpdを起動しない状態で./lidar_lvx_sampleを実行してみると、
Finish save 0 frame to lvx file. type=0 t= 39.423563850
Finish save 1 frame to lvx file. type=0 t= 39.472523850
Finish save 2 frame to lvx file. type=0 t= 39.522443850
のようなメッセージが得られました。type=0は、時刻同期していないという意味になります。（https://github.com/Livox-SDK/Livox-SDK/wiki/Livox-SDK-Communication-Protocol#32-time-stamp-timestamp）
そのため、タイムスタンプ(t)は電源投入からの経過秒数を示しています。（この場合は、電源投入から39.5秒。実際そのくらいだったと思います。）
そして、別のターミナルを開き、次のコマンドでptpdを起動して、
sudo ptpd -M -i enp3s0 -C
その後、./lidar_lvx_sampleを実行してみると、
Finish save 0 frame to lvx file. type=0 t= 40.428683850
Finish save 1 frame to lvx file. type=1 t=1660801587.738910675
Finish save 2 frame to lvx file. type=1 t=1660801587.789790630
のようなメッセージに変わりました。
（なぜか0 frame だけ同期していない。。。）
type=1は、PTPとの時刻同期を表していて狙い通りです（https://github.com/Livox-SDK/Livox-SDK/wiki/Livox-SDK-Communication-Protocol#32-time-stamp-timestamp)）。
また、タイムスタンプ（t）は、UNIX時刻を表しているので、例えばt=1660801587.738910675は、2022-08-18 14:46:27:738911を表します。正確に時刻同期できていそうです。
ROSを使った方法
いずれはSLAMによるマッピングをするとかそういうことを考えている場合は、ROSを使った方法をとることになります。
私は、ROSについては全くの素人ではありますが、ひとまず、点群データを保存するまではやってみたので、その記録をのこしておきます。
何はともあれROSのインストールです。Ubuntu を使っている場合は、インストールはaptでできるのでいたって簡単です。（逆に、Ubuntu以外の場合は、インストールはできないわけではないが非常に困難らしい？）
インストール自体は、公式ページの案内の通りにやればすんなりと行けました。注意点としては、Ubuntu のバージョンによってインストールするROSのバージョンも変わる点です。今回は、Ubuntu-20.04を使用しているので、ROS Noeticをインストールします。
http://wiki.ros.org/noetic/Installation/Ubuntu
sudo apt-add-repository restricted
sudo apt-add-repository universe
sudo apt-add-repository multiverse
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt install curl # if you haven't already installed curl
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo apt update
sudo apt install ros-noetic-desktop-full
echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
sudo apt install python3-rosdep python3-rosinstall python3-rosinstall-generator python3-wstool build-essential
sudo apt install python3-rosdep
sudo rosdep init
rosdep update
次に、livox_ros_driverをビルドします。
git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src
cd ws_livox
catkin_make
ビルドが完了したら、
source ./devel/setup.sh
で必要な環境変数等を読み込みます。この処理は、ターミナルを新しく立ち上げるたびに必要です。
これにて準備は完了です。LidarをPCに接続して点群データを取得してみます。
roslaunch livox_ros_driver livox_lidar.launch rosbag_enable:="true"
プログラムを止めるにはctl-Cです。これにて、~/.rosにYYY-mm-dd-HH-MM-SS.bagというファイルができると思います。Livox-SDKでは.lvxファイルでしたが、ROSを使う場合は、rosbagファイルが時系列の点群データになります。
rosbagファイルをPCDファイルとかに変換する方法は、こちらに書かれていました。（試してはいません。）
時刻の確認
roslaunchしたときのコンソールに表示される中のメッセージにtimestamp_type[1]のようなログが表示されるので、この番号から時刻同期されているかどうかわかりますが、せっかくなので、出力されているメッセージに含まれるタイムスタンプを確認してみます。
やり方は簡単で、ターミナルを二つ立ち上げて、一方のターミナルで
roslaunch livox_ros_driver livox_lidar.launch xfer_format:=1
を実行し、もう一方のターミナルで、
rostopic echo --noarr /livox/lidar
を実行します。そうすると、次のようなメッセージが見れると思います。
header:
seq: 130
stamp:
secs: 1660810986
nsecs: 598693609
frame_id: "livox_frame"
timebase: 1660810986598693585
point_num: 9984
lidar_id: 0
rsvd: "<array type: uint8[3, length: 3>"
points: "<array type: livox_ros_driver/CustomPoint, length: 9984>"
このメッセージにあるtimebaseがタイムスタンプです。UNIX時刻をナノ秒で表示しています。よって、1660810986598693585は、2022-08-18 17:23:6:598694を表しており、きちんと時刻同期されていそうです。
リアルタイムで点群を見る
ROSを使うと、リアルタイムで点群を眺めて楽しむこともできます。
X-serverを使うので、初めに、X11をインストールしておきます。
あとは、
roslaunch livox_ros_driver livox_lidar_rviz.launch
とすると、RVizというソフトの窓が飛んでくると思います。