FAST_LIOで地図作成マニュアル

0. FAST_LIOについて

FAST-LIO（Fast LiDAR-Inertial Odometry）は、計算効率が高くて頑強なLiDAR-慣性オドメトリパッケージです。

LiDARの特徴点とIMUデータを緊密に結合し、高速動作、ノイズの多い環境、または雑多な環境での頑強なナビゲーションを可能にするために、反復拡張カルマンフィルタを使用している。

オリジナル:

使用するリポジトリ:

1. 前提条件

PCL >= 1.8 (テスト環境: 1.12)
Eigen >= 3.3.4 (テスト環境: 3.4.0)

2. pcl_rosのインストール

sudo apt install -y ros-$ROS_DISTRO-pcl-*

3. Eigenのインストール

sudo apt install -y libeigen3-dev

4. LIVOXドライバのインストール

LIVOX MID-360の環境構築

LIVOX MID-360 環境構築が全て済んでいる場合は飛ばしてください

Warning

FAST_LIOとは別のワークスペースを分ける

4.1 新規ワークスペースの作成

mkdir $HOME/livox_ws/src -p && cd $HOME/livox_ws/src

4.2 ドライバのclone

git clone https://github.com/Livox-SDK/Livox-SDK2
git clone https://github.com/Livox-SDK/livox_ros_driver2

4.3 build.sh:

sed -i 's/colcon build --cmake-args -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}/colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}/' $HOME/livox_ws/src/livox_ros_driver2/build.sh

-    colcon build --cmake-args -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}
+    colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DROS_EDITION=${VERSION_ROS2} -DHUMBLE_ROS=${ROS_HUMBLE}

4.4 hostpcとLIVOXのIPアドレスの設定

sed -i "s/192.168.1.5/192.168.1.50/g" $HOME/livox_ws/src/livox_ros_driver2/config/MID360_config.json
sed -i "s/192.168.1.12/192.168.1.119/g" $HOME/livox_ws/src/livox_ros_driver2/config/MID360_config.json

Example

192.168.1.50: 有線のプロファイルで設定したIPアドレス

192.168.1.119: シリアル番号から取得したLIVOXのIPアドレス

4.5 build

cd $HOME/livox_ws/src/Livox-SDK2/ && mkdir build && cd build
cmake .. && make -j
sudo make install
cd $HOME/livox_ws/src/livox_ros_driver2/
./build.sh humble
source /opt/ros/humble/setup.sh
source $HOME/livox_ws/install/setup.bash

4.6 実行

ros2 launch livox_ros_driver2 rviz_MID360_launch.py
ros2 topic list -t

Success

$ ros2 topic list -t の出力

/clicked_point [geometry_msgs/msg/PointStamped]
/initialpose [geometry_msgs/msg/PoseWithCovarianceStamped]
/livox/imu [sensor_msgs/msg/Imu]
/livox/lidar [sensor_msgs/msg/PointCloud2]
/move_base_simple/goal [geometry_msgs/msg/PoseStamped]
/parameter_events [rcl_interfaces/msg/ParameterEvent]
/rosout [rcl_interfaces/msg/Log]
/tf [tf2_msgs/msg/TFMessage]
/tf_static [tf2_msgs/msg/TFMessage]

5. FAST_LIOのインストール

mkdir -p $HOME/ros2_ws/src && cd $HOME/ros2_ws/src/
git clone https://github.com/Ericsii/FAST_LIO.git
cd FAST_LIO
git submodule update --init
cd $HOME/ros2_ws/
source $HOME/livox_ws/install/setup.bash
colcon build --symlink-install --packages-select fast_lio
source $HOME/ros2_ws/install/setup.bash

Tip

ROS 2ワークスペースのパスは自分の環境に合わせて適宜変更してください

6. ビルドに失敗した場合

CMakelists.txtのADD_COMPILE_OPTIONSを修正

9 10	`-ADD_COMPILE_OPTIONS(-std=c++14) +ADD_COMPILE_OPTIONS(-std=c++17)`

7. 動作確認

ros2 launch livox_ros_driver2 msg_MID360_launch.py
ros2 launch fast_lio mapping.launch.py
ros2 service call /map_save std_srvs/srv/Trigger

Note

ros2 service call /map_save std_srvs/srv/Triggerを実行したディレクトリにtest.pcdという名前のファイルが保存される

Tip

ファイルパスを変更したい場合:

config/mid360.yamlのmap_file_path:を変更する

3 4	`- map_file_path: "./test.pcd" + map_file_path: "<path_to_your_map_file>"`

8. rosbagの取得

rosbag record by launch filerosbag record by ros2 command

ros2 launch livox_ros_driver2 msg_MID360_launch.py
ros2 launch raspicat_bringup rosbag_record.launch.py rosbag_path:=$HOME/rosbag_mapping

Info

topicを指定したい場合

ros2 launch raspicat_bringup rosbag_record.launch.py \
    rosbag_path:=$HOME/rosbag_mapping \
    topics:=/odom \
            /point_raw \
            /pointcloud_map \
            /ekf_pose_with_covariance

ros2 launch livox_ros_driver2 msg_MID360_launch.py
ros2 bag record -o ${HOME}/rosbag_mapping --all

Info

topicを指定したい場合

ros2 bag record -o ${HOME}/rosbag_mapping \
    /odom \
    /point_raw \
    /pointcloud_map \
    /ekf_pose_with_covariance

9. rosbagを再生して地図作成

Tip

/livox/lidarのtopicの型がsensor_msgs/msg/PointCloud2で地図作成したい場合:

config/mid360.yamlのlidar_type:を変更する

        preprocess:
-           lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for any other pointcloud input
+           lidar_type: 4                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for any other pointcloud input

lidar_type: 1だと、/livox/lidarのtopicの型がlivox_ros_driver2/msg/CustomMsgとなる

ros2 launch fast_lio mapping.launch.py config_path:=$HOME/ros2_ws/src/FAST_LIO/config/mid360.yaml
ros2 bag play -r 1 --clock 100 $HOME/rosbag_mapping
# rosbag再生終了後、地図を取得
ros2 service call /map_save std_srvs/srv/Trigger