Windows上でUnityとROS2を連携させる(2) ~簡易型シミュレータ作成編~

#はじめに

前回、[Windows上でUnityとROS2を連携させる(1) ~環境構築編~](https://qiita.com/sfc_nakanishi_lab/items/40ab94a2f19f375f42db)という記事を書きました。

本記事では、その続きとして、ROS2からのTopicを可視化することを目的として、以下の2つの課題に取り組みます。


###課題①：簡易型シミュレータの作成

今回作成する簡易型シミュレータは「[turtlesim](https://docs.ros.org/en/foxy/Tutorials/Turtlesim/Introducing-Turtlesim.html)」をUnityで作成することを目標とします。
![Turtlesim.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/47b0d2e9-9d6f-a7d3-9cb6-8c129672b200.gif)

###課題②：Turtlebot3とUnityのturtlesimを連携させて動かす

課題①で作成した「Unityの簡易型シミュレータ」と「実機のTurtlebot3」に対して、共通のTopicをPublishすることで連携させて動かすことにトライします。
![Turtlebot3&Turtlesim.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/989f6444-9dca-2dc3-32b8-643b1f141ac3.gif)

#実行環境

本記事で用いる実行環境は以下の通りです。

###Windows PC
| 項目 | 値 |
|:-:|:-:|
| CPU | Core i7-9750H  |
|  OS |  Windows10 |
| ROS2    |  Foxy Fitzroy |
|Unity|2020.3.16f1|

###Raspberry Pi 4

| 項目 | 値 |
|:-:|:-:|
| CPU |1.5GHz クアッドコア Cortex-A72(ARMv8、64bit L1=32KB、L2=1M) |
|メモリ |UD-RP4B8：8GB|
|  OS |  Ubuntu Desktop 20.04 |
| ROS2    |  Foxy Fitzroy |
|Turtlebot3|Waffle Pi|




#課題①：簡易型シミュレータの作成

##システム概略


ROS側から、速度・角速度情報(Twist)をPublishし、その情報を元にUnity側では亀が動きます。

* Node
 * turtlesim_node(Unity側)
 * turtlesim_teleop_node(ROS側)

* Topic
 * /cmd_vel[unity_robotics_demo_msgs/msg/Twist]


#Ros Messageについて

[前回の記事でのサンプルコード](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/publisher.md)では、「[PosRot.cs](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/ros2_packages/unity_robotics_demo_msgs/msg/PosRot.msg)」という独自のRos Messageを使用していました。

しかし、Unityにインポートした「[ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector)」には、C#のメッセージクラス化されたRos Messageが事前に用意されています。

Unityの「Packages/ROS TCP Connector/Runtime/Messages」の中に、Ros Messageがあることが確認できると思います。

![スクリーンショット 2021-09-24 19.16.50.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/7c50fb8d-8d82-ca02-826f-89865a7c74f2.png)

システムを構築する場合、基本的にはこれらのRos Messageを使うことになると思います。

しかし、システムに合わせて独自のRos Messageを作成する場合もあるため、課題①では独自のRos Messageを作成する練習を兼ねて、デフォルトのRos Messageを使用しない方法を用います。




##独自のRos Messageを作成する

###Topicのタイプについて

独自のROS Messageを作成する場合、Topic名は同じでもTypeが異なります。つまり、同じ`/cmd_vel`という名前を指定しても、Typeが違うと通信ができません。Subscribeする側もPublishする側も同じTypeのTopicをインポートし使用する必要があります。

* 通常のTopicのType
 * `cmd_vel[geometry_msgs/msg/Twist]`
* 今回の独自のTopicのType
 * `cmd_vel[unity_robotics_demo_msgs/msg/Twist]`
   　

###Twist.msgファイルの作成

「/dev_ws/src/unity_robotics_demo_msgs/msg」に以下のファイルを追加してください。

```Twist.cs
float32 linear_x
float32 linear_y
float32 linear_z
float32 angular_x
float32 angular_y
float32 angular_z
```




###CMakeListに追加


「/dev_ws/src/unity_robotics_demo_msgs/CMakeLists」に以下の記述を追加してください。

`"msg/Twist.msg"`


###Build

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call "C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Auxiliary\Build\vcvarsall.bat" x86_amd64
$ call install/setup.bat
$ colcon build --merge-install
$ call install/setup.bat
```

※`call install/setup.bat`
1回目：Buildに使用する環境を設定
2回目：新しくBuildされたパッケージを環境に追加

###Unity側でのBuild

[前回の記事](https://qiita.com/sfc_nakanishi_lab/items/40ab94a2f19f375f42db#message-generationの実行)を参考に、Unity側でRos MessageをBuildしてください。



#Node①：turtlesim_node(Unity側)の作成

「turtlesim_node」はROS2からの速度・角速度情報(Twist)をSubscribeし、亀を動かします。


##sceneの作成

以下の写真のような簡易的なシミュレータのsceneを作成していきます。
![スクリーンショット 2021-09-25 20.07.59.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/df303481-96fe-547d-2b09-71ab6393c775.png)
**１：Assets内で、「turtlesim」というsceneを作る**

**２：Hierarchyで、右クリックし「Plane」をセットする（位置や姿勢などの値を全て0にする）**

**３：Hierarchyで、右クリックし「Create Empty」を作成し、「turtlesim」と名づける**

**４：Hierarchyで、右クリックし、「cube」を２つセットし、「turtle_body」、「turtle_head」と名づける**

* 「turtle_body」
 * Position.yを「0.5」にセットする。
 * Add ComponentでRigidbodyを追加する
 * Use GravityをOFFにする
* 「turtle_head」
 * スケールをx,y,z全て「0.5」にする
 * Position.zを「0.75」にセット。
 * 「turtle_body」の子にする。

**５：Hierarchyで、右クリックし「Create Empty」を作成し、「turtlesim_subscriber」と名づける**

* 「turtle_body」の子にする。

**６：Assets内で、「material」フォルダを作成する**

* フォルダ内で右クリックし、「Create」→「Material」で2つ作成する
* turtle_headとturtle_bodyのための好きな色を選択し、オブジェクトにD&Dする 

**７：コードの作成**

* Assets内に「turtlesim」というフォルダを作成
* フォルダ内に「turtlesim_node」というファイルを作成
* ファイルに以下のコードをコピー&ペーストする
* 「turtlesim_node」を「turtlesim_subscriber」にDrag&Drop
* 「turtlesim_subscriber」のInspectorのScriptの「Cube」に「turtle_body」Drag&Dropする

```turtlesim_node.cs

using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Twist = RosMessageTypes.UnityRoboticsDemo.TwistMsg; //独自のRos Messageを使用
   
public class turtlesim_node : MonoBehaviour
{
     public Rigidbody cube;
   
     void Start()
     {
       ROSConnection.instance.Subscribe<Twist>("/cmd_vel", turtlesim_move);
     }
   
     void turtlesim_move(Twist Msg)
     {
        print(Msg);
        cube.velocity = transform.forward * Msg.linear_x;
        cube.angularVelocity = new Vector3(0, Msg.angular_z, 0);
     }
}

```

#Node②：turtlesim_teleop_node(ROS2側)の作成

「turtlesim_teleop_node」はUnityへ速度・角速度情報(Twist)をPublishします。

###ROS2 Packageの作成

[こちらのサイト](https://docs.ros.org/en/foxy/Tutorials/Creating-Your-First-ROS2-Package.html)を参考に、ROS2のPackageを作成します

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws\src
$ ros2 pkg create --build-type ament_python --node-name turtlesim_teleop_node unity_turtlesim
```

###Build

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call "C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Auxiliary\Build\vcvarsall.bat" x86_amd64
$ colcon build --merge-install
```

###コードの作成

Package作成時に作られた「turtlesim_teleop_node」ファイルを下記のコードに書き換える。

```turtlesim_teleop_node.py
import rclpy
from rclpy.node import Node
from unity_robotics_demo_msgs.msg import Twist #独自のRos Messageを使用
   
class turtlesim_teleop_node(Node):
   
    def __init__(self):
        super().__init__('turtlesim_teleop_node')
        self.publisher_ = self.create_publisher(Twist, 'cmd_vel', 10)
        timer_period = 0.5  # seconds
        self.timer = self.create_timer(timer_period, self.timer_callback)
        self.i = 0
   
    def timer_callback(self):
        twist = Twist()
        twist.linear_x = 1.0
        twist.angular_z = 1.0
        self.get_logger().info(f'Publishing: {twist}')
        self.publisher_.publish(twist) 
   
   
def main(args=None):
    rclpy.init(args=args)
   
    turtle_pub = turtlesim_teleop_node()
   
    while rclpy.ok():
        rclpy.spin_once(turtle_pub)
   
if __name__ == '__main__':
    main()
```

###Build

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call "C:\Program Files (x86)\Microsoft Visual Studio\2019\Community\VC\Auxiliary\Build\vcvarsall.bat" x86_amd64
$ colcon build --merge-install
```

###実行する

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call install/setup.bat
$ ros2 run unity_turtlesim turtlesim_teleop_node
```

###Topicの確認

Topicが正常にPublishされているかを確認します。

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call install/setup.bat
$ ros2 topic list
$ ros2 topic echo /cmd_vel
```

#実際に通信してみる



###ros_tcp_endpointの起動

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call install/setup.bat
$ ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=<your IP address>
```

###Publisher(ROS側)の起動
```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call install/setup.bat
$ ros2 run unity_turtlesim turtlesim_teleop_node
```

###Subscriber(Unity側)の起動

Unityの「run」ボタンを押す



Unity側で亀がグルグル回転していれば成功です。

![Turtlesim.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/47b0d2e9-9d6f-a7d3-9cb6-8c129672b200.gif)

#課題②：Turtlebot3とUnityのturtlesimを連携させて動かす

課題①でUnityに簡易的なシミュレータを作成することができました。

ここからは、「実機のTurtlebot3」と「Unityのturtlesim」に対して、同じTwist MessageをPublishすることで連携して動かすことにトライします。

※この先はTurtlebot3のセットアップが完了していることを前提に進めます。



##システム概略

ROS側から、速度・角速度情報(Twist)をPublishし、その情報を元にUnity側ではturtlesim、ROS側ではturtlebot3が動きます。

* Node
 * turtlesim_node2(Unity側)
 * turtle_teleop_key(ROS側)
 * turtlebot3(ROS側)

* Topic
 * /cmd_vel[geometry_msgs/msg/Twist]


#Ros Messageについて

課題①で作成した簡易的シミュレータでは、[独自のRos Message](https://qiita.com/drafts/ad81e16bd52805347534/edit#ros-messageについて)を作成し、それを使用しました。

しかし、今回は「turtle_teleop_key」という[turtlesim package](https://docs.ros.org/en/foxy/Tutorials/Turtlesim/Introducing-Turtlesim.html)に入っているNodeを用いるため、TopicのTypeが異なることから独自のRos Messageを用いて通信することはできません。

そのため今回は、[「ROS TCP Connector」に用意されたRos Message](https://qiita.com/drafts/ad81e16bd52805347534/edit#ros-messageについて)を用いたシミュレータを作成します。



#Node①：turtlesim_node2(Unity側)の作成

「turtlesim_node2」はROS2からの速度・角速度情報(Twist)をSubscribeします。

###コードの作成

```turtlesim_node2.cs
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry; //「ROS TCP Connector」のRos Messageを使用
      
public class turtlesim_node : MonoBehaviour
{
    public Rigidbody cube;
      
    void Start()
    {
        ROSConnection.instance.Subscribe<TwistMsg>("/cmd_vel", turtlesim_move);
    }
      
    void turtlesim_move(TwistMsg Msg)
    {
        print(Msg);
        cube.velocity = transform.forward * (float)Msg.linear.x;
        cube.angularVelocity = new Vector3(0, (float)Msg.angular.z, 0);
    }
}
```

#Node②turtle_teleop_key(ROS側)

「turtle_teleop_key」はデフォルトでは、`turtle1/cmd_vel`というTopicをPublishします。

しかし、「Turtlebot3」と「Unityのturtlesim」は`/cmd_vel`という名前のTopicをsubscribeします。

そのため今回は、「turtle_teleop_key」のTopic名を`/cmd_vel`に変更します。

ROS2には、[remapping](https://docs.ros.org/en/foxy/Tutorials/Turtlesim/Introducing-Turtlesim.html#remapping)という機能があり、コマンドを実行する際にTopic名を簡単に変更することができます。

```
$ ros2 run turtlesim turtle_teleop_key --ros-args --remap turtle1/cmd_vel:=/cmd_vel
```

#Node③turtlebot3(ROS側)

[turtlebot3_bringup](https://emanual.robotis.com/docs/en/platform/turtlebot3/bringup/#bringup)を実行し、Nodeを立ち上げます。



###ros_tcp_endpointの起動

```
$ call C:\opt\ros\foxy\x64\local_setup.bat
$ cd \dev_ws
$ call install/setup.bat
$ ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=<your IP address>
```

最後にros_tcp_endpointの起動し、turtlebot3とunityのturtlesimが連携して動くのを確認してください。

![Turtlebot3&Turtlesim.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/989f6444-9dca-2dc3-32b8-643b1f141ac3.gif)

#おわりに

今回はROS2からのTopicを可視化することを目的として、簡易型シミュレータをUnity内に作成する方法について解説しました。また、共通のTwist Messageをturtlebot3とUnityのturtlesimがsubscribeすることで連携して動かすこともできました。

しかし、今回のシステムでは実空間におけるturtlebot3のタイヤのスリップなどを考慮していないため、時間が経つにつれてシミュレータとの位置・姿勢のズレが拡大します。

そこで次回は、Unityのシミュレータと実機のTurtlebot3の位置・姿勢をマッピングさせる方法について解説します。

#参考文献
* [Windows上でUnityとROS2を連携させる(1) ~環境構築編~](https://qiita.com/sfc_nakanishi_lab/items/40ab94a2f19f375f42db)
* [turtlesim](https://docs.ros.org/en/foxy/Tutorials/Turtlesim/Introducing-Turtlesim.html)
* [Creating your first ROS2 Package](https://docs.ros.org/en/foxy/Tutorials/Creating-Your-First-ROS2-Package.html)
* [remapping](https://docs.ros.org/en/foxy/Tutorials/Turtlesim/Introducing-Turtlesim.html#remapping)