YDLIDAR T-mini ProをROS2(Humble)上で使う

SLAMで地図作成するための準備として、YDLIDAR T-mini ProをROS2(Humble)上で使えるようにします。

# 使うもの

+ YDLIDAR T-mini Pro
  + https://www.switch-science.com/products/8614

  + 測距距離：0.02 ～ 12 m
  + 測距周期：最大 4000 Hz
  + スキャニング周期：6～12 Hz
  + データシート
    + https://www.ydlidar.com/Public/upload/files/2022-09-06/YDLIDAR%20T-mini%20Pro%20Data%20Sheet_V0.2.0%20(220831).pdf
  + ユーザマニュアル
    + https://www.ydlidar.com/Public/upload/files/2022-11-11/YDLIDAR%20T-mini%20Pro%20User%20Manual_V0.1.0%20(221024).pdf
 
+ Ubuntu PC
  + OS : Ubuntu 22.04.1 LTS (arm64)
  + ROS : ROS2 Humble

# 参考手順
+ https://zenn.dev/tasada038/articles/c4fff08deae9dd

# 事前準備

+ Ubuntu22.04環境をセットアップ

+ ROS2 Humbleをインストール
  + https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html

# 準備1 : YDLidar-SDKをインストール

+ 公式手順
  + https://github.com/YDLIDAR/YDLidar-SDK/blob/master/doc/howto/how_to_build_and_install.md

+ YDLidar-SDKをビルド＆インストール

```bash
sudo apt install cmake pkg-config
sudo apt-get install python3 swig
sudo apt-get install python3-pip

git clone https://github.com/YDLIDAR/YDLidar-SDK.git
mkdir YDLidar-SDK/build
cd YDLidar-SDK/build
cmake ..
make
sudo make install

cd ~/YDLidar-SDK/
pip3 install .
```

YDLidar-SDKのインストールが終了したら、ydlidarをインポートできるか確認しておきます。
```bash
python3
>>> import ydlidar
>>>
```

+ 実行権限の追加
```bash
sudo gpasswd --add $USER dialout
sudo reboot
```

正しく追加できたかは、`id`コマンドで確認します。

```bash
ubuntu@ubuntu-desktop:~$ id
uid=1000(ubuntu) gid=1000(ubuntu) groups=1000(ubuntu),4(adm),20(dialout),24(cdrom),27(sudo),30(dip),46(plugdev),123(lpadmin),135(lxd),136(sambashare)
ubuntu@ubuntu-desktop:~$
```

+ 動作チェック

テストプログラムが実行できるか確認します。
```bash
cd ~/YDLidar-SDK/build
./tri_test
```

Baudrateはデータシートを確認して、「230400」にしました。

```bash
ubuntu@ubuntu-desktop:~/YDLidar-SDK/build$ ./tri_test
__   ______  _     ___ ____    _    ____
\ \ / /  _ \| |   |_ _|  _ \  / \  |  _ \
 \ V /| | | | |    | || | | |/ _ \ | |_) |
  | | | |_| | |___ | || |_| / ___ \|  _ <
  |_| |____/|_____|___|____/_/   \_\_| \_\

Baudrate:
0. 115200
1. 128000
2. 153600
3. 230400
4. 460800
5. 512000
Please select the lidar baudrate:3
Whether the Lidar is one-way communication[yes/no]:yes
YDLidar SDK initializing
YDLidar SDK has been initialized
[YDLIDAR]:SDK Version: 1.1.2
LiDAR successfully connected
[YDLIDAR]:Lidar running correctly ! The health status: good
LiDAR init success, Elapsed time 636 ms
Start to getting intensity flag
End to getting intensity flag
[CYdLidar] Successed to start scan mode, Elapsed time 2061 ms
[YDLIDAR] Fixed Size: 720
[YDLIDAR] Sample Rate: 3K
[YDLIDAR] Fixed Size: 720
[YDLIDAR] Sample Rate: 3K
[YDLIDAR]:Single Fixed Size: 670
[YDLIDAR]:Sample Rate: 3K
[YDLIDAR INFO] Single Channel Current Sampling Rate: 3K
[YDLIDAR INFO] Now YDLIDAR is scanning ......
[YDLIDAR]: User version 0.33
[YDLIDAR] Connection established in [/dev/ttyUSB0][230400]:
Firmware version: 1.0
Hardware version: 2
Model: T1
Serial: 2022111800010054
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
Scan received [670] points inc [0.009392]
^Csignal_handler(2)
Scan received [670] points inc [0.009392]
281473594396928 thread has been canceled
[YDLIDAR INFO] Now YDLIDAR Scanning has stopped ......
ubuntu@ubuntu-desktop:~/YDLidar-SDK/build$
```

# 準備2 : YDLidarのROS2パッケージをインストール

```bash
mkdir -p ydlidar_ros2_ws/src
cd ydlidar_ros2_ws/src/
git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git
cd ..
colcon build --symlink-install
```
ここで、ビルドエラーが発生しました。Pull requests に対策済みソースがあったため、そのまま利用しました。
+ https://github.com/YDLIDAR/ydlidar_ros2_driver/pull/20

ビルド結果を消した上で、再ビルドすると、ビルド成功しました。
```
rm -rf build/ install/ log/
colcon build --symlink-install
```
```bash
echo "source ~/ydlidar_ros2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc
```
```bash
chmod 0777 src/ydlidar_ros2_driver/startup/*
sudo sh src/ydlidar_ros2_driver/startup/initenv.sh
```

# 準備3 : LiDARノードのコンフィグ作成

[ydlidar_ros2_driver](https://github.com/YDLIDAR/ydlidar_ros2_driver.git)パッケージには、デバイス種類毎の設定ファイルが格納されていませんでした。

ROS1パッケージの以下を参考に、T-mini用の設定ファイルを作成します。
+ https://github.com/YDLIDAR/ydlidar_ros_driver/blob/master/launch/Tmini.launch

適宜以下の説明を参照しました。
+ https://github.com/YDLIDAR/ydlidar_ros2_driver#configure-ydlidar_ros_driver-internal-parameter

今回は、以下の2ファイルを作成しました。
+ `ydlidar_ros2_driver/params/Tmini.yaml`
+ `ydlidar_ros2_driver/launch/Tmini_launch.py`


`ydlidar.yaml`をコピーして`Tmini.yaml`を作成。差分は以下の通り。
```diff
-    sample_rate: 9
+    sample_rate: 4
-    intensity: false
+    intensity: true
-    range_max: 64.0
-    range_min: 0.01
-    frequency: 10.0
+    range_max: 16.0
+    range_min: 0.02
+    frequency: 6.0
```

+ `Tmini.yaml`の全設定値は以下の通り。(Pull requestsの内容も反映済み)
```bash
ydlidar_ros2_driver_node:
  ros__parameters:
    port: /dev/ttyUSB0
    frame_id: laser_frame
    ignore_array: ""
    baudrate: 230400
    lidar_type: 1
    device_type: 0
    sample_rate: 4
    abnormal_check_count: 4
    fixed_resolution: true
    reversion: true
    inverted: true
    auto_reconnect: true
    isSingleChannel: false
    intensity: true
    support_motor_dtr: false
    angle_max: 180.0
    angle_min: -180.0
    range_max: 16.0
    range_min: 0.02
    frequency: 6.0
    invalid_range_is_inf: false
```

+ `ydlidar_launch.py`をコピーして`Tmini_launch.py`を作成します。差分は以下の通りです。
```diff
     params_declare = DeclareLaunchArgument('params_file',
                                            default_value=os.path.join(
-                                               share_dir, 'params', 'ydlidar.yaml'),
+                                               share_dir, 'params', 'Tmini.yaml'),
                                            description='FPath to the ROS2 parameters file to use.')
```

ここでは、lauchパラメータ`params_file`のデフォルト値を変更しただけです
launch実行時にパラメータ指定して利用する場合、このファイルは作成不要です


# ROS2ノードの動作確認

+ ROS2のLiDARノードを起動
```bash
ros2 launch ydlidar_ros2_driver Tmini_launch.py
```

+ rviz2でLaserScanを見る

`config/ydlidar.rviz`を引数に指定して`rivz2`を起動することで、点群データが表示されることを確認できました。

```bash
rviz2 -d ydlidar_ros2_ws/src/ydlidar_ros2_driver/config/ydlidar.rviz
```

+ LiDARノードの起動ログ
```bash
ubuntu@ubuntu-desktop:~/ydlidar_ros2_ws$ ros2 launch ydlidar_ros2_driver Tmini_launch.py
[INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2022-12-12-22-48-33-694066-ubuntu-desktop-34657
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [ydlidar_ros2_driver_node-1]: process started with pid [34669]
[INFO] [static_transform_publisher-2]: process started with pid [34671]
[static_transform_publisher-2] [WARN] [1670852914.346607953] []: Old-style arguments are deprecated; see --help for new-style arguments
[ydlidar_ros2_driver_node-1] [INFO] [1670852914.384261830] [ydlidar_ros2_driver_node]: [YDLIDAR INFO] Current ROS Driver Version: 1.0.2
[ydlidar_ros2_driver_node-1]
[ydlidar_ros2_driver_node-1] YDLidar SDK initializing
[ydlidar_ros2_driver_node-1] YDLidar SDK has been initialized
[ydlidar_ros2_driver_node-1] [YDLIDAR]:SDK Version: 1.1.2
[static_transform_publisher-2] [INFO] [1670852914.411967297] [static_tf_pub_laser]: Spinning until stopped - publishing transform
[static_transform_publisher-2] translation: ('0.000000', '0.000000', '0.020000')
[static_transform_publisher-2] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-2] from 'base_link' to 'laser_frame'
[ydlidar_ros2_driver_node-1] LiDAR successfully connected
[ydlidar_ros2_driver_node-1] [YDLIDAR]:Lidar running correctly ! The health status: good
[ydlidar_ros2_driver_node-1] [YDLIDAR] Connection established in [/dev/ttyUSB0][230400]:
[ydlidar_ros2_driver_node-1] Firmware version: 1.0
[ydlidar_ros2_driver_node-1] Hardware version: 2
[ydlidar_ros2_driver_node-1] Model: T-mini Pro
[ydlidar_ros2_driver_node-1] Serial: 2022111800010054
[ydlidar_ros2_driver_node-1] [YDLIDAR INFO] Current Scan Frequency: 6.000000Hz
[ydlidar_ros2_driver_node-1] LiDAR init success, Elapsed time 828 ms
[ydlidar_ros2_driver_node-1] Start to getting intensity flag
[ydlidar_ros2_driver_node-1] lastPos 470 currPos 484 offset 14
[ydlidar_ros2_driver_node-1] Auto set intensity 1
[ydlidar_ros2_driver_node-1] End to getting intensity flag
[ydlidar_ros2_driver_node-1] [CYdLidar] Successed to start scan mode, Elapsed time 1902 ms
[ydlidar_ros2_driver_node-1] [YDLIDAR] Fixed Size: 677
[ydlidar_ros2_driver_node-1] [YDLIDAR] Sample Rate: 4K
[ydlidar_ros2_driver_node-1] [YDLIDAR INFO1] Current Sampling Rate : 4K
[ydlidar_ros2_driver_node-1] [YDLIDAR INFO] Now YDLIDAR is scanning ......
```