LoginSignup
6
5

More than 1 year has passed since last update.

@sfc_nakanishi_lab(慶應義塾大学・環境情報学部・中西研究室)

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

Last updated at Posted at 2021-09-26

はじめに

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

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

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

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

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

課題①で作成した「Unityの簡易型シミュレータ」と「実機のTurtlebot3」に対して、共通のTopicをPublishすることで連携させて動かすことにトライします。
Turtlebot3&Turtlesim.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について

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

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

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

スクリーンショット 2021-09-24 19.16.50.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

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

Node①：turtlesim_node(Unity側)の作成

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

sceneの作成

以下の写真のような簡易的なシミュレータのsceneを作成していきます。
スクリーンショット 2021-09-25 20.07.59.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の作成

こちらのサイトを参考に、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

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

Turtlebot3&Turtlesim.gif

おわりに

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

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

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

参考文献

6
5
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
6
5