このブログは、株式会社フィックスターズのエンジニアが、あらゆるテーマについて自由に書いているブログです。
先日、Livox社から新たにMid-360という40mレンジの3D LiDARが発売されました。
一式、おおむね10万円程度で購入可能です。(2023年1月現在)
視野角も360° x 59°(仰角)と特徴的であり、Velodyne等と差別化した運用ができると期待しています。
弊社でも、早速入手してROS1/ROS2で動かしてみました。
弊社所有Mid-360と自作ケーブル
※ 弊社自作ケーブルはMid-360クイックスタートガイド内のピン配置を参考に作成しました。
動作確認に用いた環境は以下の通りです。
(Mid-360クイックスタートガイド p.23, 2023/01/24)
Livox Mid-360は、動的IPアドレスモードと静的IPアドレスモードの2つのIPモードに対応しています。すべての Livox Mid-360 LiDAR センサーは、初期設定で静的 IP アドレスモード(IP アドレス 192.168.1.1XX)に設定されます。(XX は、Livox Mid-360 LiDAR センサーのシリアル番号の末尾 2 桁です)。 Livox Mid-360LiDAR センサーの初期設定時のサブネットマスクはすべて 255.255.255.0、デフォルトゲートウェイは192.168.1.1 です。初めて使用する場合は Livox Mid-360 をパソコンに直接接続します。
こちらから入手可能な公式クイックスタートガイドより、Mid-360はシリアル番号の末尾2桁に応じて192.168.1.1XX/24
のいずれかに設定されます。弊社のMid-360のシリアル番号末尾2桁は02でしたので、192.168.1.102/24
となります。
本記事では、ホストPC側の設定は192.168.1.200/24
としております。この状態で192.168.1.102
にpingを飛ばして応答があることを確認します。
$ ip a
≪中略≫
3: enx9096f34afb12: <BROADCAST,MULTICAST,UP,LOWER_UP> mtu 1500 qdisc fq_codel state UP group default qlen 1000
link/ether 90:96:f3:4a:fb:12 brd ff:ff:ff:ff:ff:ff
inet 192.168.1.200/24 brd 192.168.1.255 scope global noprefixroute enx9096f34afb12
valid_lft forever preferred_lft forever
inet6 fe80::1b9b:8384:3f4c:46c9/64 scope link noprefixroute
valid_lft forever preferred_lft forever
≪中略≫
$ ping 192.168.1.102
PING 192.168.1.102 (192.168.1.102) 56(84) bytes of data.
64 bytes from 192.168.1.102: icmp_seq=1 ttl=255 time=3.05 ms
64 bytes from 192.168.1.102: icmp_seq=2 ttl=255 time=1.88 ms
64 bytes from 192.168.1.102: icmp_seq=3 ttl=255 time=0.642 ms
Mid-360を起動するために、まずLivox LiDAR用のSDK(Livox_SDK2)の導入し、動作確認を行います。
その後、ROS用ドライバーであるlivox_ros_driver2を導入してROS1/ROS2での点群可視化を行います。
Livox SDK2をREADMEに従ってインストールします。
$ cd $HOME
$ git clone https://github.com/Livox-SDK/Livox-SDK2 -b 1.0.1
$ cd $HOME/Livox-SDK2/ && mkdir build && cd build
$ cmake .. && make -j
$ sudo make install
サンプルプログラムlivox_lidar_quick_start
でLivox_SDK2を介してMid-360と通信可能できるかを確認します。
デフォルトの設定ファイルではホストPCのIPアドレスが192.168.1.5
となっています。
その為、自環境に合わせて設定ファイルを書き換えます。
$ sed -i "s/192.168.1.5/192.168.1.200/g" $HOME/Livox-SDK2/samples/livox_lidar_quick_start/mid360_config.json
diff --git a/samples/livox_lidar_quick_start/mid360_config.json b/samples/livox_lidar_quick_start/mid360_config.json
index e203497..07620db 100644--- a/samples/livox_lidar_quick_start/mid360_config.json
+++ b/samples/livox_lidar_quick_start/mid360_config.json
@@ -8,15 +8,15 @@
"log_data_port": 56500
},
"host_net_info" : {- "cmd_data_ip" : "192.168.1.5",
+ "cmd_data_ip" : "192.168.1.200",
"cmd_data_port": 56101,- "push_msg_ip": "192.168.1.5",
+ "push_msg_ip": "192.168.1.200",
"push_msg_port": 56201,- "point_data_ip": "192.168.1.5",
+ "point_data_ip": "192.168.1.200",
"point_data_port": 56301,- "imu_data_ip" : "192.168.1.5",
+ "imu_data_ip" : "192.168.1.200",
"imu_data_port": 56401,- "log_data_ip" : "192.168.1.5",
+ "log_data_ip" : "192.168.1.200",
"log_data_port": 56501
} }
設定完了後、以下のコマンドを実行してください。
Mid-360との接続に成功し、点群情報が取得できていれば問題ありません。
$ cd $HOME/Livox-SDK2/build/samples/livox_lidar_quick_start/
$ ./livox_lidar_quick_start ../../../samples/livox_lidar_quick_start/mid360_config.json
[2023-01-20 12:02:04.520] [console] [info] Livox lidar logger disable. [parse_cfg_file.cpp] [Parse] [84]
[2023-01-20 12:02:04.520] [console] [info] Data Handler Init Succ. [data_handler.cpp] [Init] [49]
[2023-01-20 12:02:04.521] [console] [info] Init livox lidars succ. [device_manager.cpp] [Init] [153]
[2023-01-20 12:02:04.523] [console] [info] Handle detection data, handle:1711384768, dev_type:9, sn:47MDKBV0010102, cmd_port:56100 [general_command_handler.cpp] [HandleDetectionData] [345]
point cloud handle: 1711384768, data_num: 96, data_type: 1, length: 1380, frame_counter: 0
point cloud handle: 1711384768, data_num: 96, data_type: 1, length: 1380, frame_counter: 0
point cloud handle: 1711384768, data_num: 96, data_type: 1, length: 1380, frame_counter: 0
point cloud handle: 1711384768, data_num: 96, data_type: 1, length: 1380, frame_counter: 0
[2023-01-20 12:02:04.525] [console] [info] Receive Ack: Id 257 Seq 3 [mid360_command_handler.cpp] [Handle] [73]
[2023-01-20 12:02:04.525] [console] [info] Query Fw type succ, the fw_type:1 [general_command_handler.cpp] [QueryFwTypeCallback] [489]
point cloud handle: 1711384768, data_num: 96, data_type: 1, length: 1380, frame_counter: 0
≪中略≫
livox_ros_driver2のREADMEより、パッケージ内にあるbuild.shを使用してビルドを行います。
livox_ros_driver2はROS1/ROS2で兼用となっており、各環境ごとに細かい仕様が異なり、build.sh
を使用せずにROSワークスペースディレクトリ直下でcatkin_make
やcolcon build
を行ってもビルドに失敗するので注意が必要です。
ROS1では、以下のコマンドでビルドが可能です。
$ mkdir $HOME/livox_ws/src -p && cd $HOME/livox_ws/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver2 -b 1.1.1
$ cd livox_ros_driver2
$ ./build.sh ROS1
ROS1でのlivox_ros_driver2のデフォルトビルドコマンドは次のようになっています。
catkin_make -DROS_EDITION=${VERSION_ROS1}
ROS_EDITION
はCMakeLists.txt内でROS1とROS2の処理を分けるために使用しています。
ビルドモードの指定はありませんが、CMakeLists.txt内のここでCMAKE_BUILD_TYPEにRelease
をセットしており、自動的にRelaseビルドになっています。
ROS2 Humbleでは、以下のコマンドでビルドが可能です。
$ mkdir $HOME/livox_ws/src -p && cd $HOME/livox_ws/src
$ git clone https://github.com/Livox-SDK/livox_ros_driver2 -b 1.1.1
$ cd livox_ros_driver2
$ ./build.sh humble
READMEより、ROS2 Humbleより古いdistroを使用する場合は./build.sh ROS2
を実行してください。
ROS2 Humbleからrosidl_typesupport_cpp
の仕様変更によりカスタムメッセージ作成の処理を内部で分岐させているようです。
ROS2でのlivox_ros_driver2のデフォルトビルドコマンドは次のようになっています。
colcon build --cmake-args -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}
ROS1のビルドコマンドと同様にROS_EDITION
・HUMBLE_ROS
はCMakeLists.txt内での処理分岐に使用しています。
一方、ROS1と異なり、CMakeLists.txt内部でビルドモードを設定する処理が存在しないため、Debugビルドとなっています。
また、--symlink-install
オプションが付与されておらず、パッケージ内のconfigやlaunchを書き換える度に./build.sh
の再実行が必要なります。
その為、少なくともlivox_ros_driver2 v1.1.1では以下のようにビルドコマンドを書き換えておくことを推奨します。
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}
Livox_SDK2動作確認
と同様に起動時の設定ファイルを自環境に更新します。
livox_ros_driver2/config/MID360_config.json
内のIPアドレスを以下のように更新してください。
# ホストPC IPアドレス更新
$ sed -i "s/192.168.1.5/192.168.1.200/g" $HOME/livox_ws/src/livox_ros_driver2/config/MID360_config.json
# Mid-360 IPアドレス更新
$ sed -i "s/192.168.1.12/192.168.1.102/g" $HOME/livox_ws/src/livox_ros_driver2/config/MID360_config.json
diff --git a/config/MID360_config.json b/config/MID360_config.json
index e3df5d1..2218f3e 100644--- a/config/MID360_config.json
+++ b/config/MID360_config.json
@@ -11,13 +11,13 @@
"log_data_port": 56500
},
"host_net_info" : {- "cmd_data_ip" : "192.168.1.5",
+ "cmd_data_ip" : "192.168.1.200",
"cmd_data_port": 56101,- "push_msg_ip": "192.168.1.5",
+ "push_msg_ip": "192.168.1.200",
"push_msg_port": 56201,- "point_data_ip": "192.168.1.5",
+ "point_data_ip": "192.168.1.200",
"point_data_port": 56301,- "imu_data_ip" : "192.168.1.5",
+ "imu_data_ip" : "192.168.1.200",
"imu_data_port": 56401,
"log_data_ip" : "",
"log_data_port": 56501@@ -25,7 +25,7 @@
},
"lidar_configs" : [
{- "ip" : "192.168.1.12",
+ "ip" : "192.168.1.102",
"pcl_data_type" : 1,
※ ROS2 Humbleで--symlink-install
でのビルドを行っていない場合は、書き換え後再度./build.sh humble
を実行する必要があります。
ROS1・ROS2それぞれ以下のコマンドでMid-360の起動ができます。
ROS1
$ source $HOME/livox_ws/devel/setup.bash
$ roslaunch livox_ros_driver2 rviz_MID360.launch
ROS2
$ source $HOME/livox_ws/install/setup.bash
$ ros2 launch livox_ros_driver2 rviz_MID360_launch.py
コマンド実行後に起動するrviz上でMid-360の点群可視化ができました。
机等も細かく認識しており、全体的に精度が良さそうです。
ROS2上でノード情報を確認したところ/livox/lidar
トピックが点群情報(sensor_msgs/msg/PointCloud2
)・/livox/imu
がIMU情報(sensor_msgs/msg/Imu
)のようです。
それぞれ10[hz] (デフォルト)・200[hz]で配信されていました。
※点群形式や点群の配信周波数は変更可能です。詳細は後述のAppendixを参照ください。
$ ros2 node info /livox_lidar_publisher
/livox_lidar_publisher
Subscribers:
/parameter_events: rcl_interfaces/msg/ParameterEvent
Publishers:
/livox/imu: sensor_msgs/msg/Imu
/livox/lidar: sensor_msgs/msg/PointCloud2
/parameter_events: rcl_interfaces/msg/ParameterEvent
/rosout: rcl_interfaces/msg/Log
Service Servers:
/livox_lidar_publisher/describe_parameters: rcl_interfaces/srv/DescribeParameters
/livox_lidar_publisher/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
/livox_lidar_publisher/get_parameters: rcl_interfaces/srv/GetParameters
/livox_lidar_publisher/list_parameters: rcl_interfaces/srv/ListParameters
/livox_lidar_publisher/set_parameters: rcl_interfaces/srv/SetParameters
/livox_lidar_publisher/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
Service Clients:
Action Servers:
Action Clients:
$ ros2 topic hz /livox/lidar
average rate: 10.075
min: 0.092s max: 0.109s std dev: 0.00472s window: 12
average rate: 10.037
min: 0.092s max: 0.109s std dev: 0.00471s window: 22
$ ros2 topic hz /livox/imu
average rate: 200.061
min: 0.004s max: 0.006s std dev: 0.00056s window: 202
average rate: 200.019
min: 0.004s max: 0.006s std dev: 0.00057s window: 402
今後は、Mid-360を用いてSLAMを行ってみる予定です。
ノード実行時に以下のように、liblivox_lidar_sdk_shared.so
が見つからずエラーになる場合があります。
[livox_ros_driver2_node-1] terminate called after throwing an instance of 'class_loader::LibraryLoadException'
[livox_ros_driver2_node-1] what(): Could not load library dlopen error: liblivox_lidar_sdk_shared.so: cannot open shared object file: No such file or directory, at ./src/shared_library.c:99
その際は、ldconfigでライブラリキャッシュを更新することで解決します。
$ sudo ldconfig
Mid-360はlivox_ros_driver2/msg/CustomMsg
という独自形式の点群情報の配信も可能です。
README#32-livox-ros-driver-2-internal-main-parameter-configuration-instructions より、各launch内のパラメータxfer_format
を変更することで配信メッセージの変更ができます。
参考:ROS2の場合は以下のように変更します。
diff --git a/launch_ROS2/rviz_MID360_launch.py b/launch_ROS2/rviz_MID360_launch.py
index 31d3480..3e85a55 100644--- a/launch_ROS2/rviz_MID360_launch.py
+++ b/launch_ROS2/rviz_MID360_launch.py
@@ -5,7 +5,7 @@ from launch_ros.actions import Node
import launch
################### user configure parameters for ros2 start ###################-xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
+xfer_format = 1 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
data_src = 0 # 0-lidar, others-Invalid data src publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
$ ros2 topic info /livox/lidar
Type: livox_ros_driver2/msg/CustomMsg
Publisher count: 1
Subscription count: 0
スペック表によると、Mid-360の配信周波数は10 Hz (typical)
となっています。
しかし、こちらもREADME#32-livox-ros-driver-2-internal-main-parameter-configuration-instructions を見ると、各launch内のパラメータpublish_freq
を変更することで最大100[hz]まで設定できそうです。
参考:ROS2の場合は以下のように変更します。
diff --git a/launch_ROS2/rviz_MID360_launch.py b/launch_ROS2/rviz_MID360_launch.py
index 31d3480..f8567e3 100644--- a/launch_ROS2/rviz_MID360_launch.py
+++ b/launch_ROS2/rviz_MID360_launch.py
@@ -8,7 +8,7 @@ import launch
xfer_format = 0 # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format
multi_topic = 0 # 0-All LiDARs share the same topic, 1-One LiDAR one topic
data_src = 0 # 0-lidar, others-Invalid data src-publish_freq = 10.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
+publish_freq = 100.0 # freqency of publish, 5.0, 10.0, 20.0, 50.0, etc.
output_type = 0
frame_id = 'livox_frame' lvx_file_path = '/home/livox/livox_test.lvx'
$ ros2 topic hz /livox/lidar
average rate: 100.017
min: 0.009s max: 0.011s std dev: 0.00027s window: 102
average rate: 100.018
min: 0.009s max: 0.011s std dev: 0.00027s window: 203
average rate: 100.019
min: 0.009s max: 0.011s std dev: 0.00027s window: 304
This issue was sorted.
Hey guys myself deiva from India currently i am working in this Livox MID360 and eager to knwo whether you have done the SLAM with this Lidar…
If so kindly share the tech blog of that too.
Hi. This is an interesting blog. I attempted to integrate MID 360 LiDAR with ROS 2 humble and it failed. Could you please provide any solution for this issue? https://github.com/Livox-SDK/Livox-SDK2/issues/55 I would appreciate your support.