Unitree L2 LiDARを使用し、GLIMで点群MAPを作成する

はじめに

ROS2 TOPICやrosbagデータからSLAMを行って点群MAPの作成までできるGLIMに、Unitree LiDAR L2のデータを処理させます。

1. GLIMをDockerコンテナで実行

1.1. Dockerコンテナの構築

GLIM Docker imagesをベースに実行します。

GPU(cuda)の有無で使用できる設定やコマンドが少し変わるので注意しましょう。

bash

# GLIMのコンフィグファイルを取得し、ホームディレクトリの /glim/config ディレクトリにコピーする
git clone https://github.com/koide3/glim.git /tmp/glim
cp -R /tmp/glim/config ./glim/config

# GPUが使えない場合は下記項目を編集する(gpu から cpu に変更する)
nano config/config.json
# "config_odometry" : "config_odometry_cpu.json"
# "config_sub_mapping" : "config_sub_mapping_cpu.json"
# "config_global_mapping" : "config_global_mapping_cpu.json"

# docker hubからGLIMのDockerイメージを取得する
# GPUありの場合
docker pull koide3/glim_ros2:humble_cuda12.2

# GPUなしの場合
docker pull koide3/glim_ros2:humble

以下、Unitree L2 LiDAR用の設定

config/config_ros.json

    "imu_topic": "/unilidar/imu",       // Unitree L2 LiDAR IMU topic
    "points_topic": "/unilidar/cloud",  // Unitree L2 LiDAR points topic

    // acc_scale はデフォルトの1.0のままでいいっぽい

config/config_sensors.json

    "T_lidar_imu": [  // Unitree L2 LiDAR IMU transformation
      0.007698,
      0.014655,
      -0.00667,
      0.0,
      0.0,
      0.0,
      1.0
    ],

必要に応じてGLIM Important parametersに記載の項目も調整します。

1.2. SLAMデータの保存

必要なファイルの準備ができたら、GLIMのDockerイメージをGUI表示付きで立ち上げます。
長ったらしいコマンドなので、.shファイルに記載して一発で実行できるようにすると良いかもしれません。
-vオプションでホストマシンをDockerコンテナにマウントしていることに注意です。

GPUあり

docker run \
  -it \
  --rm \
  --net=host \
  --ipc=host \
  --pid=host \
  --gpus all \
  -e DISPLAY=$DISPLAY \
  -e ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0} \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  -v "$HOME/glim/config":/glim/config \
  -v "$HOME/glim/output":/tmp/dump \
  koide3/glim_ros2:humble_cuda12.2 \
  ros2 run glim_ros glim_rosnode --ros-args -p config_path:=/glim/config

GPUなし

# glim_ros2:humble のDockerイメージをGUI表示付きで立ち上げる
# -vオプションでホストマシンをDockerコンテナにマウントしていることに注意
docker run \
  -it \
  --rm \
  --net=host \
  --ipc=host \
  --pid=host \
  -e=DISPLAY \
  -e ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-0} \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  -v "$HOME/glim/config":/glim/config \
  -v "$HOME/glim/output":/tmp/dump \
  koide3/glim_ros2:humble \
  ros2 run glim_ros glim_rosnode --ros-args -p config_path:=/glim/config

点群MAP化したい場所を一通り歩き回った後にCtrl+cでDockerコンテナを終了すると、ホームディレクトリの/glim/output/にSLAMしたデータができています。

ただこの状態ではMAPになっていないので、次にこのデータを可視化します。

1.3. 点群MAP化

ターミナルに以下のコマンドを入力し、GLIMをオフラインビューアーモードで立ち上げます。

GPUあり

docker run \
  -it \
  --rm \
  --net=host \
  --ipc=host \
  --gpus all \
  -e=DISPLAY \
  -e=NVIDIA_VISIBLE_DEVICES=all \
  -e=NVIDIA_DRIVER_CAPABILITIES=compute,utility,graphics \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  -v $HOME/glim/config:/glim/config \
  -v $HOME/glim/output:/tmp/dump \
  koide3/glim_ros2:humble_cuda12.2 \
  ros2 run glim_ros offline_viewer

GPUなし

docker run \
  -it \
  --rm \
  --net=host \
  --ipc=host \
  -e=DISPLAY \
  -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
  -v "$HOME/glim/config":/glim/config \
  -v "$HOME/glim/output":/tmp/dump \
  koide3/glim_ros2:humble \
  ros2 run glim_ros offline_viewer

GLIMのGUIが立ち上がったら、左上のタブからFile->readを選択し、/tmp/dump/の構成データを指定します。

GLIMはDocker環境で立ち上げているので、ローカル環境の$HOME/glim/outputではなくDocker環境内の/tmp/dump/でないと構成データが見つからないことに注意です。

点群が3DのMAPになっていれば、File -> Save -> Save Mapと選択して.ply形式で/tmp/dump/に保存すれば、ローカル環境の$HOME/glim/outputで点群MAPファイルが入手できます。

1.4. シェルスクリプトで実行、屋内・屋外向けconfigファイル(githubにて公開)

各GLIMの立ち上げコマンドは正直面倒なので、下記のgithubリポジトリにてシェルスクリプトを公開しています。
（1.2. 、1.3. のコマンドを覚える必要がなくなります）

また、屋内・屋外向けに最適化したconfigファイルと、GPU/CPU向けのシェルスクリプトも公開しています。
L2 LiDARはMid-360などと比べて点群が粗いのですが、ちゃんと設定を作り込めば屋内なら部屋のインテリア、屋外なら植栽などがわかる程度の点群MAPができます。

2. .ply形式の点群MAPファイルを.pcdに変換する

.ply形式の点群MAPデータは、Cloudcompareで点群マッチングでは一般的な.pcd形式に変換できますが、Ubuntuでは下記の記事にあるように

bash

sudo snap install cloudcompare

でないと.pcdの入出力ができないので注意です。

参考