ROS2でUnity上のTurtleBot3を動かそう！

#UnityがROS2対応した！
先週(7/12)，UnityのROS対応用の github が大型アップデートがありました～．

https://github.com/Unity-Technologies/Unity-Robotics-Hub

現在，[TOPPERS/箱庭](https://toppers.github.io/hakoniwa/)WGでは，[ROSConJP 2021](https://roscon.jp/)にて，「箱庭:IoT/クラウドロボティクス時代の仮想シミュレーション環境」という発表枠で講演を予定しております．そんな理由もあって，ROS対応を推進しているのですが，箱庭の実装はUnity依存しているところが多く，UnityのROS機能対応範囲がすぐさま箱庭の機能範囲に直結するという実情がありました．

先日，[ROS で TOPPERS/箱庭・単体ロボット向けシミュレータを動かす！](https://qiita.com/kanetugu2018/items/a2a069dba6e26c4e7eda)にて，ROS1での箱庭対応を公開した矢先，UnityのROS2対応にでくわし，急遽，箱庭のROS2対応を進めることとなりました．

先週，そんなこんなで，箱庭のROS2対応がひと段落しましたので，本記事にて，ROS1/ROS2を使って箱庭上のロボットモデル(TurtleBot3/HackEV)を動作させるためのセットアップ手順を説明したいと思います．


* 2021.11.27
  * ROS2/docker環境でのインストール手順を記載したQiita記事を公開しました．
     * https://qiita.com/kanetugu2018/items/b132b48be2b59eb214fb

#前提とする環境
とりあえず，本記事ではWindows10を前提として説明します(原理的にはMac/Linuxでもいけますが今回は省略)．

## ROS1 の場合
現時点で，ROS1については，WSL1/Ubuntu 18.04.5 LTS/melodicで動作確認をしました．

#ROS2の場合
現時点で，ROS1については，WSL2/Ubuntu 20.04.2 LTS/foxyで動作確認をしました．


#ROS1/ROS2向けのTOPPERS/箱庭環境の構築手順
ROS1/ROS2向けのTOPPERS/箱庭環境の構築手順手順は以下の通りです．

1. Unityをインストールする
2. UnityのROSパッケージをインストールする
3. 箱庭のUnityパッケージをダウンロードする
3. ROSプログラムをビルドする
4. Unityに箱庭Unityパッケージをインポートする
5. 箱庭をインストールする
6. シミュレーション実行する

##Unityをインストールする
[UnityでROSを利用](https://github.com/Unity-Technologies/Unity-Robotics-Hub)するには，Unityのバージョンは`2020.2+`のものが必要のようです．

とりあえず，今回は，`2021.1.5f`をインストールしました．

Unityのインストールに関しては，以下をご参照ください．

https://dkrevel.com/makegame-beginner/getunityhub/

##UnityのROSパッケージをインストールする
Unityインストールして，Unityを起動すると，まっさらな環境が起動します．
この状態で，まずはUnit/ROSパッケージをインストールしましょう．

インストール手順はこちらを参照ください．

https://github.com/Unity-Technologies/ROS-TCP-Connector

現時点では，`https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.6.0`をインストールすると，ROS1/ROS2両方対応されます．


##箱庭のUnityパッケージをダウンロードする
以下で，ROS向け箱庭のUnityパッケージを公開していますので，バージョンに応じてダウンロードしてください．

https://github.com/toppers/hakoniwa-ros-samples/releases/tag/v1.1.0 

* ROS1の場合
  * single-robot-HackEV-hakoniwa-core-ros1.unitypackage
* ROS2の場合
  * single-robot-HackEV-hakoniwa-core-ros2.unitypackage

##Unityに箱庭Unityパッケージをインポートする
箱庭Unityパッケージをインポートするには，プロジェクト作成する必要があります．

プロジェクト作成場所は，後述するリポジトリ(https://github.com/toppers/hakoniwa-ros-samples) の clone 後の以下の場所になります．

* ROS1の場合
  *  作成場所：hakoniwa-ros-samples/ros1/unity
  *  プロジェクト名：tb3_ros1
* ROS2の場合
  *  作成場所：hakoniwa-ros-samples/ros2/unity
  *  プロジェクト名：tb3_ros2

ダウンロードしたUnityパッケージは，Unityのメニュー[Assets]->[import Package]->[Custom Package...]を選択し，先ほどダウンロードしたUnityパッケージを選択するとインポートできます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/0bedef0f-4677-0554-2752-580ebca1d85c.png)

インポート成功すると，UnityのProject上の`Scenes`フォルダ内に，「Toppers_Corse」というシーンが見えますので，これをダブルクリックしてください．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/1692ef75-f80c-6fa0-e59b-b9d05a8af09f.png)

成功すると以下のような画面が見えます．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/ad4286a7-1f81-6f8b-03a7-23983c81231a.png)

左下にHackev，上方にTurtleBot3が見えますね．
これでUnity側のインストールは終了です．お疲れ様でした．

##ROSプログラムをビルドする
UnityのロボットはROSプログラムで制御して動かします．

サンプルプログラムは以下で公開しております．

https://github.com/toppers/hakoniwa-ros-samples

まずは，git clone で上記リポジトリをクローンしましょう．ただし，ROS1の場合は，`WSL1/Ubuntu 18.04.5 LTS/melodic`のインストールされていることを前提とします．ROS2の場合は，`WSL2/Ubuntu 20.04.2 LTS/foxy`です．

```
$ git clone --recursive https://github.com/toppers/hakoniwa-ros-samples.git
```

次に，以下のワークスペース・フォルダに移動し，ビルドます．

* ROS1 の場合
  * hakoniwa-ros-samples/ros1/workspace
* ROS2 の場合
  * hakoniwa-ros-samples/ros2/workspace

ビルド方法は以下の通りです．

```
$ bash clean.bash
$ bash build.bash
```

##箱庭をインストールする

先ほどクローンしたリポジトリには，ROS向けの箱庭インストールツール類がそろっています．

* 2021/08/10 追記
  * 箱庭インストーラは、python2.7/jinja2を利用します
  * jinja2をインストールは、pipコマンドで行います。以下のサイトを参照ください。
  * https://qiita.com/sg0hsmt/items/f8fc8d587bff816654a8

###フォルダ構成

本リポジトリのフォルダ構成は以下の通りです．

```
.
├── ros1
│   ├── ros_json
│   ├── template
│   ├── unity
│   └── workspace
├── ros2
│   ├── ros_json
│   ├── template
│   ├── unity
│   └── workspace
├── settings
│   ├── ev3
│   ├── ev3_tb3
│   └── tb3
├── third-party
│   ├── ros1
│   └── ros2
└── utils
```

####ROSバージョン毎の作業/管理フォルダ
`ros1`，`ros2`フォルダでは以下を管理しています．

* ros_json
  * 箱庭がROSメッセージを解釈するためのデータ型定義
* template
  * Unityスクリプト自動生成用テンプレートファイル(jinja2)
* unity
  * Unityプロジェクト管理フォルダ
* workspace
  * ROSプログラム管理フォルダ

####インストール設定管理フォルダ
`settings`フォルダでは，箱庭をインストールするための設定ファイルを配置します．
現時点で，以下の３フォルダがあります．

* ev3
  * HackEV単体のロボット制御をするための設定ファイル管理フォルダ
* tb3
  * TurtleBot3単体のロボット制御をするための設定ファイル管理フォルダ
* ev3_tb3
  * HackEVとTurtleBot3のロボット制御をするための設定ファイル管理フォルダ

補足：settings配下のフォルダ名は，箱庭内では，ROS向けのパッケージ名として利用しています．

各フォルダでは，以下の２ファイルを配置する必要があります．

1. RosTopics.json
2. {ros1|ros2}_search_file_path.txt

RosTopics.jsonでは，Unity内のロボットが購読/出版するROSトピックをjson形式で定義します．

以下，設定例です．

```
{
	"fields": [
        {
            "topic_message_name": "scan",
            "topic_type_name": "sensor_msgs/LaserScan",
            "robot_name": "TB3RoboModel",
            "sub": false
        },
        {
            "topic_message_name": "imu",
            "topic_type_name": "sensor_msgs/Imu",
            "robot_name": "TB3RoboModel",
            "sub": false
        },
        {
            "topic_message_name": "cmd_vel",
            "topic_type_name": "geometry_msgs/Twist",
            "robot_name": "TB3RoboModel",
            "sub": true
        }
	]
}
```

{ros1|ros2}_search_file_path.txtでは，後述するインストーラが利用します．
設定内容としては，RosTopics.jsonで設定したROSトピックメッセージ定義ファイルの格納フォルダを列挙します．

設定例は，以下の通りです．

```
/opt/ros/foxy/share
{リポジトリをクローンしたフォルダ}/hakoniwa-ros-samples/ros2/workspace/src
```

`third-party`フォルダでは，UnityのROS対応githubリポジトリをsubmoduleとして管理しています．

###インストール内容
箱庭向けのインストール対象は以下の４点です．

* ROS制御プログラムがロボットと通信するROSメッセージ一覧ファイル(依存するもの含む)
  * settings/<pkg_name>/ros_msgs.txt ファイルが生成されます
* 箱庭がROSメッセージを解釈するためのデータ型定義の自動生成
  * ROSメッセージのデータ型をjson形式に変換したもの
  * {ros1|ros2}/ros_jsonフォルダ配下に配置されます
* 箱庭のコンフィグファイルの自動生成
  * シミュレーション対象となるロボットやROSメッセージ情報等を定義している
  * {ros1|ros2}/unity/<project名>フォルダ配下に配置されます
* Unityスクリプトの自動生成
  * UnityのROS APIを利用したROS通信処理実装(ROSメッセージ毎にコードの形が変わる)
  * {ros1|ros2}/unity/<project名>フォルダ配下の所定場所にファイル配置されます

###インストール手順
インストール手順は以下の通りです．
なお，事前に settings/ev3, tb3, ev3_tb3配下に，{ros1|ros2}_search_file_path.txtファイルを配置しておく必要があります．

##ROS1の場合

```
$ cd hakoniwa-ros-samples
$ bash utils/install.bash ros1 ros1/unity/tb3_ros1 settings/ev3
$ bash utils/install.bash ros1 ros1/unity/tb3_ros1 settings/tb3
$ bash utils/install.bash ros1 ros1/unity/tb3_ros1 settings/ev3_tb3
```

##ROS2の場合

```
$ cd hakoniwa-ros-samples
$ bash utils/install.bash ros2 ros2/unity/tb3_ros2 settings/ev3
$ bash utils/install.bash ros2 ros2/unity/tb3_ros2 settings/tb3
$ bash utils/install.bash ros2 ros2/unity/tb3_ros2 settings/ev3_tb3
```

##シミュレーション実行する
シミュレーション実行するには，WSLの端末を３個起動します．

* 端末A
  * UnityのROS-TCP-ENDPOINTを起動用
* 端末B
  * EV3のROS制御ノード用
* 端末C
  * TB3のROS制御ノード用

いずれも，以下のフォルダに移動します．

* ROS1 の場合
  * hakoniwa-ros-samples/ros1/workspace
* ROS2 の場合
  * hakoniwa-ros-samples/ros2/workspace


### UnityのROS-TCP-ENDPOINTを起動
まずは，ROS-TCP-ENDPOINTを起動します．

```
$ bash launch.bash
```

成功すると以下のようなログが出力されます．

```
[INFO] [1626927344.319005800] [TCPServer]: Starting server on 172.21.68.89:10000
```

### EV3制御プログラムノードを起動

```
$ bash run.bash ev3
```

### TB3制御プログラムノードを起動

```
$ bash run.bash tb3
```

## Unity のシミュレーションを起動

Unityのシミュレーション開始ボタンをクリックしてください．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/989c1a89-31f2-1415-b6a6-0ba34839f0ce.png)

成功するとこうなります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/0e82bf1d-96fa-4541-73d5-e19056ab3c4c.png)

また，ROS-TCP-ENDPOINT起動端末では，以下のログが出力されます．

```
[INFO] [1626927505.129422600] [TCPServer]: Connection from 172.21.64.1
[INFO] [1626927507.675631900] [TCPServer]: RegisterSubscriber(ev3_actuator, <class 'ev3_msgs.msg._ev3_pdu_actuator.Ev3PduActuator'>) OK
[INFO] [1626927507.890973900] [TCPServer]: RegisterSubscriber(cmd_vel, <class 'geometry_msgs.msg._twist.Twist'>) OK
[INFO] [1626927508.022069200] [TCPServer]: RegisterPublisher(ev3_sensor, <class 'ev3_msgs.msg._ev3_pdu_sensor.Ev3PduSensor'>) OK
[INFO] [1626927508.507704200] [TCPServer]: RegisterPublisher(scan, <class 'sensor_msgs.msg._laser_scan.LaserScan'>) OK
[INFO] [1626927508.700018800] [TCPServer]: RegisterPublisher(imu, <class 'sensor_msgs.msg._imu.Imu'>) OK
[INFO] [1626927508.924359100] [TCPServer]: RegisterPublisher(odom, <class 'nav_msgs.msg._odometry.Odometry'>) OK
```

#デモ
あとは，「開始」ボタンを押下するだけです．

![demo1.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/3bfae69e-9690-0ddd-3852-6735e33b4c07.gif)


#rviz対応について
2021/07/30時点で，ROS1について，rviz対応しましたので，箱庭のrviz環境構築手順を以下に示します．

1. rviz 表示用の launch ファイルの編集
2. ROS対応版箱庭起動後に，rvizを起動する
3. シミュレーション実行する
4. レーザスキャナの検出内容を可視化する

## rviz 表示用の launch ファイルの編集

rvizで TurtleBot3 の内部状態を表示するために，以下のファイルを編集する必要があります．

* /opt/ros/melodic/share/urdf_tutorial/launch/display.launch

編集結果は以下の通りです．
※joint_state_publisherをコメントアウトして，最新のrobot_state_publisherを利用する修正です

```
<launch>

  <arg name="model" default="$(find urdf_tutorial)/urdf/01-myfirst.urdf"/>
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <!-- <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" /> -->
  <!-- <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /> -->
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>
```

## ROS対応版箱庭起動後に，rvizを起動する

`launch.bash`を実行後に，以下のコマンドを実行して，rvizを起動します．

```
$ roslaunch urdf_tutorial display.launch model:=/opt/ros/melodic/share/turtlebot3_description/urdf/turtlebot3_burger.urdf.xacro
```

起動成功すると以下のようになります．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/244147/5ed6c39e-e0b1-9a66-b30f-5189211e62d3.png)

## シミュレーション実行する
rvizを起動した状態で，シミュレーションを実行してください．
rviz 上の TurtleBot3 が動き出すと思います．


## レーザスキャナの検出内容を可視化する
さらに，レーザスキャナの情報を表示するには，左のメニューの「Add」ボタン→新しい画面で「By Topic」タブ→/scanを選択すると，以下のようにスキャナ情報が点群で見えるようになります．

https://twitter.com/kanetugu2020/status/1421027026417307648


# お知らせ：箱庭もくもく会

TOPPERS箱庭WGでは，もくもく会を不定期に開催しています．
本記事の内容を実際に試してみたい方や試してみたけど詰まっている方は，ぜひご参加ください．画面共有しながら参加者同士でサポートしたり，一緒に新しい機能を実装したりと”もくもく”しましょう．

https://hakoniwa.connpass.com/