【Unitree Go2】Mid360 on Go2でFAST-LIOを使う

初めに
環境
１．MID-360によるFAST-LIOの実行
２．go2 imuの配信
３．実行
おわりに

初めに

本記事では、【Unitree Go2】Mid360でSLAMを実行するの続きになります。
前回の記事では犬を少し歩かせると、地図が崩れてしまいました。
本記事ではGo2内蔵のIMUを用いて、安定的なSLAMを実現します。

環境

環境は、次のようなものを想定しています。

機種: Go2 R&D+(Edu+)（AIR/PROではないもの）
付属LiDAR：Mid-360

※FAST-LIOはかなり重いので、Orin NXで行うことを前提とします。

１．MID-360によるFAST-LIOの実行

本記事では下記の記事の続きになります。
MID360単体によるFAST-LIOを実行できるところまでは、できているとします。
【Unitree Go2】Mid360でSLAMを実行する

２．go2 imuの配信

Unitree Go2の内部に搭載されたIMUは、Unitree社独自開発のIMUですが、Go2の高性能な運動を支えるものであり、かなり高性能になっています。
本記事ではこのIMUを用いて、FAST-LIOを実行させるように修正を加えます。

2-1. Docker Fileの修正

まず、go2 IMUを取得するために、unitree_ros2を使えるようにする必要があります。
そのためにDocker Fileを修正します。下記の内容を画像のあたりに差し込みます。

# for unitree_ros2
RUN apt-get update -q && \
apt-get install -y \
ros-humble-rosidl-generator-dds-idl \
ros-humble-rmw-cyclonedds-cpp
# Download unitree_ros2
WORKDIR /root/
RUN git clone https://github.com/unitreerobotics/unitree_ros2

2-2. dockerイメージの作成

その後、dockerイメージを作成します。
なお、前の記事で作ったdockerコンテナは削除しておきます。

$ docker rm fast-lio-ros2
$ cd /path/to/FAST_LIO_ROS2/docker
$ docker build -t fast-lio-ros2:latest .

※途中までは同じなので、さほど時間はかかりません

2-4. dockerコンテナの作成

前の記事と同様、コンテナを作成する際に、実行している様子の表示をNoMachine上で行えるようにするために、この時点でNoMachineから接続しGUI上のterminalからコンテナの作成を行います。
参考：【Unitree Go2】NoMachineでDocking StationのGUIを使う

$ cd /path/to/FAST_LIO_ROS2/docker
$ ./container_run.sh fast-lio-ros2 fast-lio-ros2:latest

これでdockerコンテナが作成されました。

2-5. livox_ros_driver2とFAST-LIOのコンパイル

ここでも前の記事と同様に、コンパイルします。

# cd ~/ros2_ws/src/livox_ros_driver2
# ./build.sh humble

2-6. unitree_ros2の環境設定

unitree_ros2の環境を設定します。
（参照：【Unitree Go2】SDK or ROSを使う方法の最後の方を詳しくはご覧ください）

# cd ~/unitree_ros2/cyclonedds_ws
# colcon build

2-7. Go2 IMUの配信パッケージのダウンロード＆コンパイル

今回、Go2 IMUのpublisher nodeについては弊社で作成しました。（imu_publisher）

これをダウンロードし、コンパイルします。
なお、コンパイルするときにunitree_ros2を参照する必要があります。

# cd ~/ros2_ws/src
# git clone -b imu_publisher https://github.com/TechShare-inc/go2_unitree_ros2.git imu_publisher
# source /opt/ros/humble/setup.bash
# source ~/unitree_ros2/cyclonedds_ws/install/setup.bash
# cd ~/ros2_ws
# colcon build --packages-select go2_demo

３．実行

3-1. livox_ros_driver2の実行とFAST-LIOの実行

まずは、前の記事と同様のことが行えるか確認します。

下記のように行うことで、MID360の点群取得をdocker/humble環境で行えます。

$ docker exec -it fast-lio-ros2 bash
# . install/setup.bash
# ros2 launch livox_ros_driver2 rviz_MID360_launch.py

さらに、新しいterminalを開きFAST-LIOを実行します。

$ docker exec -it fast-lio-ros2 bash
# . install/setup.bash
# ros2 launch fast_lio mapping_mid360.launch.py

すると、マップが生成されますが、少し歩かせているとマップが吹き飛んでいくという同じような状況になることが確認できます。

3-2. FAST-LIOの参照IMUの変更

FAST-LIO/config/mid360.yamlについて、/livox/imuを/go2/imuに変更します。

その後、変更を反映させるためにコンパイルします。

# # cd ~/ros2_ws
# colcon build --packages-select fast_lio

3-3. Go2IMUとMID360を用いたFAST-LIOを実行する

まず、/go2/imuを配信します。

terminal 1: imu_publisher

$ docker exec -it fast-lio-ros2 bash
# export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
# . install/setup.bash
# ros2 run go2_demo imu_publisher

次に、livox_ros_driver2とFAST-LIOを実行します。

terminal 2: livox_ros_driver2

$ docker exec -it fast-lio-ros2 bash
# . install/setup.bash
# ros2 launch livox_ros_driver2 rviz_MID360_launch.py

terminal 3: FAST-LIO

$ docker exec -it fast-lio-ros2 bash
# . install/setup.bash
# ros2 launch fast_lio mapping_mid360.launch.py

Go2を歩かせても地図がぶっ飛ぶことはなく、正しい自己位置推定・マッピングができている。

おわりに

本記事ではGo2内蔵のIMUとMid360を用いたGo2 SLAMサンプルの実行方法を見てきました。
この記事が少しでも役に立てたのなら幸いです。

初めに

環境