ROS2の勉強　第9弾：LiDAR（YDLIDAR）

# #プログラミング ROS2< LiDAR（YDLIDAR） >

## はじめに
ROS2(バージョンアップしたROS)を難なく扱えるようになることが目的である．その第8弾として，「LiDAR（YDLIDAR）」を扱う．

## 環境
##### コンピュータ
|||
|:-:|:--|
|デバイス|raspberry pi 4 Model B|
|メモリ|3.7 GiB|
|Graphics|llvmpipe (LLVM 13.0.1, 128 bits)|
|OS|Ubuntu|

##### Ubuntu
|||
|:-:|:--|
|リリース|22.04.1 LTS 64ビット|
|カーネル|Linux 5.15.0-1015-raspi arm64|

##### ROS2
|||
|:-:|:--|
|Distribution|Humble|

https://qiita.com/porizou1/items/5dd915402e2990e4d95f

Raspberry Piも他のPCと一括して管理できるようにTeamViewerでリモートアクセスを可能にしたが，
その時の話はまた今度．

##### LiDAR
|||
|:-:|:--|
|メーカー|YDLIDAR|
|シリーズ|X2|

## ROSでYDLIDARを扱う
過去の記事では，ROS1（Noetic）でYDLIDARを使えるように環境構築した．
詳しくは以下を参照されたい．

https://qiita.com/Yuya-Shimizu/items/81ba49ec38041fd23efb

本記事では，ROS1（Noetic）-> ROS2（Humble）の環境変更に伴って，同様にYDLIDARを使うためのまとめである．

## ROS2でのYDLIDAR
ROS1のとき同様，YDLIDARがgithubでそれ用のパッケージを用意してくれていたみたいである

https://github.com/YDLIDAR/ydlidar_ros2_driver

これで，また簡単に環境が構築できると思ったが，現実にはそうはいかなかった．
ROS2 Humbleにまだ対応していないためか，ビルドが通らない．以下では順を追ってどこでうまくいかず，どのように解決したのかについて記述する．

### 環境構築
まず，公式GitHubからレポジトリをROS2のワークスペースに持ってくる．

```bash:git cloneでレポジトリを持ってくる
git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git
```

早速ビルド
```bash:ビルド
cd ydlidar_ros2_ws
colcon build --symlink-install
```
が，ここでエラーが発生！
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/cd2eba69-db2a-0279-4739-7e8fa7a911cc.png)

これは，レポジトリのREADMEでも書いてあったもので，想定済みのエラーである．単にydlidar_sdkがないとのことなので，これを別でインストールしてくる必要があるとの催促がある．

#### YDLidar-SDKの構築
以下のレポジトリに従って，構築していく．

https://github.com/YDLIDAR/YDLidar-SDK

```bash:Install CMake
sudo apt install cmake pkg-config
```

```bash:Install Extra
sudo apt-get install swig
sudo apt-get install python3-pip
```

早速ビルドする．

```bash:ビルド
git clone https://github.com/YDLIDAR/YDLidar-SDK.git
mkdir YDLidar-SDK/build
cd YDLidar-SDK/build
cmake ..
make
sudo make install
```
```bash:PythonAPI用にインストール
cd YDLidar-SDK
pip install .
```

これにより，``~/YDLidar-SDK/build``内にテストプログラムが生成されているので，それで接続チェックをする．YDLIDAR X2をRaspberry PiのUSBポートに接続．以下のコマンドにて権限を付与．

```bash:権限の付与
sudo chmod 777 /dev/ttyUSB0
```
人によって，``ttyUSB0``の数字は変わるかもしれない．私の環境では，Raspberry PiのUSBポートにYDLIDAR X2のみを接続しているため分かりやすい．

##### テストを実行
```bash:テスト実行
cd ~/YDLidar-SDK/build
./tri_test
```

##### テスト結果
今回使用しているLiDARはBaudrateが115200なので，0番を選択して，通信は一方通信yesと選択することで，読みにいってくれている．

http://www.ydlidar.com/Public/upload/files/2022-05-04/YDLIDAR%20X2%20Data%20Sheet%20V1.1(211230).pdf

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/12e995fe-7740-73d1-5284-890c12dcf856.png)
接続完了後，スキャンデータを受け取っていることがログから確認できる．


#### ydlidar_ros2_driverの構築

##### 失敗（修正前）
では，準備も整い早速ビルドしてみる

```bash:ビルド
colcon build --symlink-install
```

だが，別の内容でビルドエラー！ここから5時間以上の奮闘．．．
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/b05cc974-b907-385d-c2fd-d3cc1c778cc6.png)
上記のエラー内容を見ると，どうもプログラムがおかしそうだということで修正を試みた．
修正箇所はエラー内容から把握するに，同じようなところで21箇所あるようだ．

```cpp:問題箇所
node->declare_parameter("port");
```
ただ，何が問題であるかは分からなかったため，とりあえず``declare_parameter``関数を調べた．

https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver/issues/65

同じようなエラーで困っているというIssueを見つけた．
どうもROSのディストリビューションによって作り方が異なるのか，バージョンがあっていないからバージョンをちゃんと合わせて使ってみてという回答を見た．そのため，そのGitHubにおいてHumble仕様ではどのように``declare_parameter``関数が使われているのか調べると，以下のようにあった．

https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver/blob/humble-devel/src/hlds_laser_publisher.cpp

```cpp:Humbleでの関数
node->declare_parameter<std::string>("port");
```
問題箇所との違いは，***関数名と引数の間に型を置いているのか***どうかだけである．
これが原因であった．すべての箇所をそれぞれふさわしいデータ型に合わせて編集した．

```:追記
文字型 -> <std::string>
整数型 -> <std::int16_t>
小数型 -> <std::float_t>
論理型 -> <bool>
```

##### 成功（修正後）
```bash:再度ビルド
colcon build --symlink-install
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/951300/5d4d1800-8b79-7d84-25f6-b5ad8d91df77.png)

見事ビルドに成功！一度目のビルドで少し警告は出てくるが，2回目以降出てこない．内容は，単に使っていない変数がありますよとのことなので，特に気にすることもないと思われる．

### 検証
以下のようにYDLidarに合わせた設定をyamlファイルに書き込んでおく．
```yaml:ydlidar.yaml
ydlidar_ros2_driver_node:
  ros__parameters:
    port: /dev/ttyUSB0
    frame_id: laser_frame
    ignore_array: ""
    baudrate: 115200
    lidar_type: 1
    device_type: 0
    sample_rate: 3
    abnormal_check_count: 4
    resolution_fixed: true
    reversion: false
    inverted: true
    auto_reconnect: true
    isSingleChannel: true
    intensity: false
    support_motor_dtr: true
    angle_max: 180.0
    angle_min: -180.0
    range_max: 12.0
    range_min: 0.1
    frequency: 5.0
    invalid_range_is_inf: false
```

この設定については，次の記事を参考にしている．

https://qiita.com/porizou1/items/57edeeda15bbec76a462

launchファイルも上記の記事に従って，編集しているため参照されたい．

#### 実行

```bash:実行コマンド
ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py
```
うまくyamlファイルが反映されていない時は，再度``colcon build``してから実行を試みると良い．

上記のコマンドは，Rviz上でYDLIDAR X2で取得したレーザトピックを表示してくれる設定で，YDLIDARの起動とRvizの起動をしてくれる．その時の様子を動画像で，下に示す．

<iframe width="560" height="315" src="https://www.youtube.com/embed/HLRX1_oMV_A" title="" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>

## 感想
今回は，思った以上に大変な作業であった．久しぶりにデバッグの地獄を垣間見れた感じであった．ただ，5時間でうまくいったため，それは良かった．これで，最新のROS2（Humble）でも手持ちのLiDARを動かせるようになった．ここからは，Twitterで教えてもらったLiDARだけでのSLAMというものをしてみたい．非常に楽しみである．

https://twitter.com/MSRC05347939/status/1578392927910256642?s=20&t=INEsvvoptfjxVzTe4H7IBw

ここを参考にするとよいみたい．ちょうど，YDLidarだけど，ROS1なので，これもROS2仕様にできたらと思う．

https://archit0994.wixsite.com/architshah/post/manage-your-blog-from-your-live-site

## 参考文献

https://qiita.com/porizou1/items/5dd915402e2990e4d95f

https://qiita.com/Yuya-Shimizu/items/81ba49ec38041fd23efb

https://github.com/YDLIDAR/ydlidar_ros2_driver

https://github.com/YDLIDAR/YDLidar-SDK

http://www.ydlidar.com/Public/upload/files/2022-05-04/YDLIDAR%20X2%20Data%20Sheet%20V1.1(211230).pdf

https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver/issues/65

https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver/blob/humble-devel/src/hlds_laser_publisher.cpp

https://qiita.com/porizou1/items/57edeeda15bbec76a462