【Unitree Go2】XT16 on Go2でD-LIOを使う【SLAM】

初めに
環境
１．xt16のip設定
２．D-LIOの導入
３．実行
- - 3-1. D-LIOの実行
  - 3-2. 可視化
おわりに

初めに

本記事では、Unitree Go2 XT16版を用いたSLAMの方法を案内します。
SLAMのアルゴリズムにはいろいろな種類がありますが、今回はD-LIOというものを使っていきます。

本記事のゴール（SLAMの様子）：

↓詳しくは下記動画
本記事のゴール（XT16のSLAM走行の動画）

環境

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

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

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

１．xt16のip設定

【Unitree Go2】XT16を動かしてみる
の１の項を見て、接続設定をします。

本記事では、下記のような設定として進めますが、IPアドレスが異なる場合は適宜置き換えて読み進めてくだされば問題ありません。

XT16 IP：192.168.123.20
Docking Staion IP：192.168.123.18

２．D-LIOの導入

本記事では、D-LIOというSLAMアルゴリズムを用いた方法を解説します。
Docking Station(Orin NX)上で行うことを前提とします。

2-1. ネット接続

Docking Stationをネット接続します。
参考記事；【Unitree Go2】Docking Station のインターネット接続方法

2-2. 本記事で使用するpkgのダウンロード

本記事で使用するパッケージをまとめたものをダウンロードします。

$ git clone https://github.com/TechShare-inc/faq_go2_xt16.git

使用するものは、

unitree ros2 : go2をROS2で扱うため
imu_publisher : go2に搭載されたimuの情報をpublishするもの。弊社作成
Hesai LiDAR SDK : xt16を使うためのSDK。弊社でかなり手を加えたもの。timestampやフォーマットの関係上公式のSDKをそのままで使用できないため、修正している
dlio : 今回使用するSLAMアルゴリズム。オープンソースのROS2ブランチを使用。今回に記事用にtopic名などのパラメーターを修正している

$ cd humble_ws/src
$ git clone https://github.com/TechShare-inc/go2_unitree_ros2.git -b imu_publisher
$ git clone https://github.com/TechShare-inc/HesaiLidar_ROS2_techshare.git
$ git clone https://github.com/unitreerobotics/unitree_ros2.git
$ git clone https://github.com/TechShare-inc/direct_lidar_inertial_odometry.git

弊社で実機で行った時の様子：

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

本記事はros2 humble環境で実装するため、dockerイメージを作成します。

$ cd docker
$ docker build -t go2-humble:latest .

※かなり時間がかかります

なお、dockerはデフォルトでは入っていないため、例えば下記のコマンドでdockerを入れます。

$ sudo apt-get update
$ sudo apt-get install apt-transport-https ca-certificates curl software-properties-common
$ curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo gpg --dearmor -o /usr/share/keyrings/docker-archive-keyring.gpg
$ echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/docker-archive-keyring.gpg] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null
$ sudo apt-get update
$ sudo apt-get install docker-ce docker-ce-cli containerd.io
$ sudo usermod -aG docker $USER
$ newgrp docker
$ distribution=$(. /etc/os-release;echo $ID$VERSION_ID) \
    && curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add - \
    && curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
$ sudo apt-get update
$ sudo apt-get install -y nvidia-docker2
$ sudo systemctl restart docker

弊社で実機で行った時の様子：

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

$ cd humble_ws
$ bash ../docker/humble.sh

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

ここでもし、「nvidiaがunknown」とエラーが出る場合は、2-3節のdockerのインストールコマンドを実行する必要があります。（イメージのbuildはもう一度やる必要はありません）

2-5. コンパイル

前項でコンテナを作成し、コンテナの中にいる状態で、下記のようにコンパイルします。

# cd /external
# colcon build --symlink-install

2-6. unitree_ros2の環境設定

unitree_ros2の環境を設定します。
下記のように、unitree_ros2/setup.shを修正します。

本記事では、rvizを無線接続された外部PCで実行するので、無線インターフェイスwlan0も含めて指定しています。
こうすることでDockingStationから配信されるtopicはwlan0と同じネットワーク上のコンピューターにも配信されます。

#!/bin/bash
echo "Setup unitree ros2 environment"
#source /opt/ros/foxy/setup.bash
source unitree_ros2/cyclonedds_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
                            <NetworkInterface name="eth0" priority="1" multicast="default" />
                            <NetworkInterface name="wlan0" priority="2" multicast="default" />
                       </Interfaces></General></Domain></CycloneDDS>'

３．実行

3-1. D-LIOの実行

下記のように行うことで、実行できます。

# cd /external/src
# catmux_create_session test_catmux.yaml

正しく実行できていれば下記のような画面になります。

imu_publisher

HesaiLiDARSDK

dlio

3-2. 可視化

Docking Station(Orin NX)上で描画すると、かなりメモリが使われてしまうため、外部PCで描写を行います。

外部PCを同じ無線ネットワークに接続し、外部PCで下記のようなsetupを行うと、Docking Station(Orin NX)で配信されているトピックを見ることができます。（環境によってはwlan0ではない可能性があります）

#!/bin/bash
echo "Setup unitree ros2 environment"
#source /opt/ros/foxy/setup.bash
source $HOME/unitree_ros2/cyclonedds_ws/install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
                            <NetworkInterface name="wlan0" priority="default" multicast="default" />
                       </Interfaces></General></Domain></CycloneDDS>'

dlio pkgにあるdlio.rvizを外部PCにコピーしておきます。
その後、その設定でRvizを起動します。

$ rviz2 -d direct_lidar_inertial_odometry/launch/dlio.rviz

起動直後のRvizの画面

少し犬を歩かせると、うまくいっていれば下記のように点群が現れ、さらに歩かせると軌跡が表示される。

このように動けば成功です。
なお、伏せたり立たせたりすると自己位置が吹っ飛びやすいので、立たせた状態からSLAMをすることを推奨します。

↓結果の動画
本記事のゴール（XT16のSLAM走行の動画）

おわりに

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

初めに

環境