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

#はじめに

前回、Windows上でUnityとROS2を連携させる(1) ~環境構築編~という記事を書きました。

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

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

今回作成する簡易型シミュレータは「turtlesim」をUnityで作成することを目標とします。

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

課題①で作成した「Unityの簡易型シミュレータ」と「実機のTurtlebot3」に対して、共通のTopicをPublishすることで連携させて動かすことにトライします。

#実行環境

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

###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について

前回の記事でのサンプルコードでは、「PosRot.cs」という独自のRos Messageを使用していました。

しかし、Unityにインポートした「ROS TCP Connector」には、C#のメッセージクラス化されたRos Messageが事前に用意されています。

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

システムを構築する場合、基本的にはこれらの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

前回の記事を参考に、Unity側でRos MessageをBuildしてください。

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

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

##sceneの作成

以下の写真のような簡易的なシミュレータのsceneを作成していきます。

１：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の作成

こちらのサイトを参考に、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側で亀がグルグル回転していれば成功です。

#課題②：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を作成し、それを使用しました。

しかし、今回は「turtle_teleop_key」というturtlesim packageに入っているNodeを用いるため、TopicのTypeが異なることから独自のRos Messageを用いて通信することはできません。

そのため今回は、「ROS TCP Connector」に用意された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という機能があり、コマンドを実行する際にTopic名を簡単に変更することができます。

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

#Node③turtlebot3(ROS側)

turtlebot3_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が連携して動くのを確認してください。

#おわりに

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

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

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

#参考文献

• Windows上でUnityとROS2を連携させる(1) ~環境構築編~
• turtlesim
• Creating your first ROS2 Package
• remapping