🐚

ROS2とUnityのjoint_statesでURDF、実機のロボットを動かす

2022/11/30に公開

Unity

ROS2

urdf

tech

海洋ロボコンをやってた人です。
ROS2のアドカレには初投稿になりますが、よろしくお願いいたします。

今回は、ROS2とUnityの/joint_statesトピックでURDF、実機のロボットを動かしてみたので、その備忘録として手順を記載していきます。

/joint_statesトピックで動かすため、Rvizともリンクでき、プログラムをMoveIt等の対応に変えれば、こちらとも連携できることになります。

目的は単純で、水中環境でのシミュレーションを行いたいという点です。

スクリュー型ロボットにはUUV Simulator: ROS1、Plankton:ROS2といったシミュレーターがオープンソースで公開されています。

一方、ヒレを用いて推進するヒレ推進型ロボットでは、上記のようなROS/ROS2シミュレーターが公開されている事例が少なく、ヒレ推進型をシミュレーションできる環境が欲しいなという点がモチベーションです。

※一点補足すると、Gazeboでは尾びれで推進するロボットのシミュレーション方法が紹介されていますが、私は断念しました（下記）。
有識者の方がいましたら、ご教授いただけますと幸いです🙇

Unity内でROS/ROS2対応した各種センサを使う場合は、UnitySensorROSAssetsというパッケージが「Field-Robotics-Japan」様より公開されているため、こちらを使用して機体頭部についているRGBカメラの映像もROS2で送信してみます。

また、私事で恐縮ですが、ROS2メインで水中ロボットつくっているので、UniSimの開発は非常に気になっています(^^♪

※2022/12/08：水中環境試せそうなUnityパッケージを備忘録として追記しました。
※2022/12/30: UWRoboticsSimulatorのリンクを追記しました。
※2023/02/26追記：本記事の「ROS2」表記、正しくは「ROS 2」です。

1: ROS2とUnityのjoint_statesでURDF、実機のロボットを動かす

動作環境は以下の通りです。

Laptop PC
- Ubuntu 20.04
- ROS2 Foxy
- intel CORE i7
- GEFORCE RTX 3070
Unity Hub 3.3.0
Unity 2021.3.8f1

この記事を読むことで、下のように、自身のオリジナルロボットをUnity-ROS2間の/joint_statesで動かせるようになります。

使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。

1.1: Unity等のインストール

最初にUnityや今回使用するロボットモデルの準備等を行っていきます。

UnityHub/Unityのインストール

まずはUnityをインストールします。下記よりUnity Hubをインストールし、Unity Hub経由でUnityをインストールしましょう。

URDFモデルの作成

続いて、使用するモデルも作成します。
使用するのは、Fusion360やInventor等で作成したstl式のURDFモデルになります。

こちらは下記記事で紹介しているので、是非この機会にオリジナルのURDFモデルを作成してみてください。

ROS-TCP-Endpointのインストール

ROS/ROS2とUnity間で通信するために

Unity側
- ROS-TCP-Connector
Ubuntu ROS2側
- ROS-TCP-Endpoint

をインストールします。

すごく噛み砕くと、上記のROS-TCP-ConnectorとROS-TCP-Endpointのパッケージにより、UnityとUbuntu-ROS2側の通信ができるということかと。

詳しくはROS-Unity Integration 等を参照していただければと思います。

ROS-TCP-EndpointはUbuntu側のrosワークスペース下にインストールするので、下記でビルドするところまで準備しましょう。

mkdir -p unity_ros2_ws/src
cd ~/unity_ros2_ws/src
git clone -b main-ros2 https://github.com/Unity-Technologies/ROS-TCP-Endpoint
cd ~/unity_ros2_ws
colcon build

ROS-TCP-Endpointのブランチは「main-ros2」を選択します。
ブランチ指定せずに、クローンすると上手くUnity-ROS間の通信ができないので、ブランチには気をつけてください。

ROS-TCP-Connectorのインストール

次に、Unity側にROS-TCP-Connectorをインストールしていきます。

ROS-TCP-ConnectorはUnityのプロジェクト内で設定することになるので、最初にUnityのプロジェクトを作成しましょう。

New project > templates [3D], projectname[XXXUnityApp], Location 任意

としてプロジェクトを作ります。

私は「MantaV2UnityApp」という名前でプロジェクトを作成しています。

※xxx_xxx_xxxというスネークケースでの命名規則でプロジェクト名を作成していますが、C#ではパスカルケースで命名するのが一般のようです。

新規プロジェクト作成後、Package ManagerよりROS-TCP-Connectorをインストールします。

Window > Package Manager > Package Manager > + > Add Package from git URL
と進み、以下をペーストしたのち「add」により追加します。

ROS-TCP-Connector

https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector

この後に使用する

URDFをUnityで読み込む「URDF Importer」
ROSのトピックを可視化する「Robotics Visualizations」
Unityのセンサツール「Unity Sensors」
上記をROS/ROS2で接続する「Unity Sensors ROS」

のパッケージも追加しておきます。

https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer

https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations

https://github.com/Field-Robotics-Japan/UnitySensors.git

https://github.com/Field-Robotics-Japan/UnitySensorsROS.git

Robotics Visualizationsは以下を参照してください。

このRobotics Visualizations(画像の/TopicsMenu.PNG)には、/tf, /points_test, /visualization_marker, /rosout_aggなどのトピックを可視化できる機能もあるそうです。（試してないのでどのような感じかは不明ですが）

最終的には、下図のようになっているかと思います。

※ Unity Robotics Visualizations, UnitySensors, UnitySensorsROSも追加した場合は、こちらもインクルードされていることを確認しておきます。

任意ですが、Physicsのソルバータイプを変えることで、ジョイントの抵抗等を改善できます。

下記のドキュメントにあるように

Projected Gauss Seidel:投影されたガウスザイデル
- デフォルトのPhysXソルバータイプ
Temporal Gauss Sidel:一時的なガウスザイデル
- ジョイント抵抗の改善ができ、ソルバーを使用したシミュレーション中に異常動作が発生した場合に役立つ

とあるので

Edit > Project Settings > Physics　> Solve Type を任意に変更

しておきましょう。

1.2: UnityへオリジナルURDFロボットを読み込む

UnityプロジェクトやROS通信パッケージの準備ができたので、早速stl式のURDFロボットを読み込んでいきましょう。

Project > Assetsフォルダ下にxxx_unityというフォルダを作成（Assetsの場所で右クリック > Create > Folder）し、descriptionファイルをドラッグ&ドロップしてコピーします。

また、後述するC#プログラムをまとめる「Scripts」というフォルダも作成しておきます。

次に、xxx_description/urdf下のメインとなるxxx.urdfファイルをxxx_descriptionフォルダと同じ階層に移動させ、stlファイルのインクルード記述を以下のように指定します。

manta_v2.urdf

-<mesh filename="file://$(find manta_v2_description)/meshes/link_ballast_1.stl" scale="0.001 0.001 0.001"/>

+<mesh filename="package://manta_v2_description/meshes/link_ballast_1.stl" scale="0.001 0.001 0.001"/>

stlファイルインクルードのパス解決ができたら、xxx.urdfファイルを右クリックしImport Robot from Selected URDFよりUnity上にURDFロボットをインポートしましょう。

下図のように上手く表示できたらOKです。

1.3: joint_statesでUnity上のURDFを動かす

さて、ここから本題のプログラムに入っていきましょう。

joint_states_subscriberのプログラム

Assets > Scripts下でCreate > C# Scriptを選択し、新しいプログラム「JointStateSub」を作成します。

JointStateSub.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;

public class JointStateSub : MonoBehaviour
{
    public ArticulationBody[] articulationBodies;
    public string topicName = "/joint_states";
    public int jointLength = 19;
    private ROSConnection ros;

    void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.Subscribe<JointStateMsg>(topicName, Callback);
    }

    void Callback(JointStateMsg msg)
    {
        for (int i = 0; i < jointLength; i++)
        {
            ArticulationDrive aDrive = articulationBodies[i].xDrive;
            aDrive.target = Mathf.Rad2Deg * (float)msg.position[i];
            articulationBodies[i].xDrive = aDrive;
        }
    }
}

プログラム作成後、Hierarchyウィンドウにて
右クリック > Create Empty
で「JointSub」という名称の空のオブジェクトを作成します。

次に、「JointSub」というオブジェクトを選択すると、Unity画面右側に「Inspector」という、選択したオブジェクトの情報が見えるウィンドウが表示されます。

確認できたら、記述した「JointStateSub.cs」のプログラムを「Inspector」側にドラッグ&ドロップでシーンに適応させます。

シーン適応後、InspectorウィンドウでUnity側の「Articulation Bodies」とURDFロボットの各リンクに紐づけていきます。

Inspector Joint State Sub(Script) > Articulation Bodiesの数値入力欄にロボットの可動軸数を入力（ここでは19）。

Elementに、各リンクのBodyを「Hierarchy」からドラッグ&ドロップして適応させて完了です。

joint_states_publisherのプログラム

Subscriber側のプログラムがかけたら、Publisher側のプログラムも作成します。

ここで注意しなければいけないのが、ROSの時間システムです。

ROSのPublisherで単純に文字列等をトピックで送る場合は問題ありませんが、joint_statesやセンサ値等はROSの時間システムを正しく記述しなければなりません。

そのため、今回はすでにプログラムとして公開されている「Robotics-Nav2-SLAM-Example」よりROSの時間システムに関するプログラムを流用します。

こちらの「Script」から以下をUnityのAssets/Scripts下にコピーしましょう。

Clock.cs
TimeStamp.cs

上記の準備ができたら、Assets > Scripts下でCreate > C# Scriptを選択し、新しいプログラム「JointStatePub」を作成します。

JointStatePub.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;
using RosMessageTypes.Std;
using RosMessageTypes.BuiltinInterfaces;

using Unity.Robotics.Core;

public class JointStatePub : MonoBehaviour
{
    public string topicName = "/joint_states";
    public int jointLength = 19;  
    float time;

    public ArticulationBody[] articulationBodies;
    private ROSConnection ros;

    public string frameId = "";
    public string[] name = new string[] {};
    public double[] position = new double[] {};
    public double[] velocity = new double[] {};
    public double[] effort = new double[] {};

    // Set Parameters
    public float stiffness = 10000;
    public float damping = 100;
    public float forceLimit = 100;

    void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.RegisterPublisher<JointStateMsg>(topicName, 15);

        for (int i = 0; i < jointLength; i++)
        {
            Debug.Log("Bodies index:"+articulationBodies[i].index);
            SetParameters(articulationBodies[i]);
            Debug.Log("articulation param:"+articulationBodies[i]);
        }
    }

    void FixedUpdate()
    {
        time += Time.deltaTime;
        if (time<0.05f) return;
        time = 0.0f;
        var timestamp = new TimeStamp(Clock.Now);

        ArticulationDrive xDrive = this.articulationBodies[1].xDrive;

        if (Input.GetKey(KeyCode.Z)){
            xDrive.target -= 1f;
        }
        else if (Input.GetKey(KeyCode.X)){
            xDrive.target += 1f;      
        }

        JointStateMsg joint_msg = new JointStateMsg{
            header = new HeaderMsg
            {
                frame_id = frameId,
                stamp = new TimeMsg{
                    sec = timestamp.Seconds,
                    nanosec = timestamp.NanoSeconds,
                },
            },
            name = name,
            position = position,
            velocity = velocity,
            effort = effort,
        };

        this.articulationBodies[1].xDrive = xDrive;  
        joint_msg.position[1] = xDrive.target/Mathf.Rad2Deg;
        Debug.Log("pos:"+joint_msg.position[1]);
        ros.Publish(topicName, joint_msg);
    }

    private void SetParameters(ArticulationBody joint)
    {
        ArticulationDrive drive = joint.xDrive;
        // drive.lowerLimit = -30;
        // drive.upperLimit = 30;
        drive.stiffness = stiffness;
        drive.damping = damping;
        drive.forceLimit = forceLimit;
        // drive.target = 0;
        // drive.targetVelocity = 0;

        joint.xDrive = drive;
    }

}

ここで少しプログラムの説明を加えておくと

JointStatePub.cs

  ArticulationDrive xDrive = this.articulationBodies[1].xDrive;

        if (Input.GetKey(KeyCode.Z)){
            xDrive.target -= 1f;
        }
        else if (Input.GetKey(KeyCode.X)){
            xDrive.target += 1f;      
        }

の部分は、パソコンの「Z」「X」キーでarticulationBodies[1]番目のジョイントを動かすようになっています。

また、JointState.msgの内容をC#で記述するには

JointStatePub.cs

        JointStateMsg joint_msg = new JointStateMsg{
            header = new HeaderMsg
            {
                frame_id = frameId,
                stamp = new TimeMsg{
                    sec = timestamp.Seconds,
                    nanosec = timestamp.NanoSeconds,
                },
            },
            name = name,
            position = position,
            velocity = velocity,
            effort = effort,
        };

とします。

もちろんheader部分を分割して記述しても良いですし、書き方は多様にあると思います。

最後の「SetParameters」はArticulationBody.xDrive内部のパラメータを設定する関数になります。

JointStatePub.cs

    private void SetParameters(ArticulationBody joint)
    {
        ArticulationDrive drive = joint.xDrive;
        // drive.lowerLimit = -30;
        // drive.upperLimit = 30;
        drive.stiffness = stiffness;
        drive.damping = damping;
        drive.forceLimit = forceLimit;
        // drive.target = 0;
        // drive.targetVelocity = 0;

        joint.xDrive = drive;
    }

基本的にstiffness, damping, forceLimitを変えれば良いと思いますが、他のパラメータも設定できるようにしています。

ArticulationDriveについては下記ドキュメントを参照してください。

プログラム作成後、Hierarchyウィンドウにて
右クリック > Create Empty
で「JointPub」という名称の空のオブジェクトを作成します。

次に、「JointPub」というオブジェクトを選択すると、Unity画面右側に「Inspector」という選択したオブジェクトの情報が見えるウィンドウが表示されるので、記述した「JointStatePub.cs」のプログラムを「Inspector」側にドラッグ&ドロップでシーンに適応させます。

シーン適応後、InspectorウィンドウでUnity側の「Articulation Bodies」とURDFロボットの各リンクに紐づけていきます。

Inspector Joint State Sub(Script) > Articulation Bodiesの数値入力欄にロボットの可動軸数を入力（ここでは19）。

Elementに、各リンクのBodyを「Hierarchy」からドラッグ&ドロップして適応させます。

また、/joint_statesはnameの配列も持つので、こちらも「Name:19」と設定し、リンクの名称も入力ていきます。

あとは、Positionも配列数も可動軸の数にそろえて設定完了です。
Frame Id,Velocity, Effort等は任意で設定してください。

その他Unityの設定

ROS Settings

ヘッダーのRobotics > ROS SettingsからProtocol：ROS2と設定します。
その他パラメータはデフォルトで問題ありません。

Planeの作成

Hierarchy > 3D Object > Plane

で平面を作成しておきます。

Main Cameraの位置を変える

ロボットの正面等、PositionやRotationを調整します。

ロボットのパラメータを調整

Hierarchyからロボットのオブジェクトを選択し、「Controller(Script)」の各パラメータを以下のように調整します。

Stiffness: 10000
Damping: 100
Force Limit: 1000
Speed: 30
Torque: 100
Acceleration: 5

1.4: joint_statesトピックで実機、Rviz、Unityを連携する

プログラム等の準備ができたら、実際にUnity、Rviz、実機で連携させてみます。

JointStateSubのプログラムを用いて、実機、Rviz、Unityを連携するので、JointPub側のScriptのチェックマーク（左側のInsector欄）はOFFにしておきましょう。

ではまず、1つ目のターミナルでROS-TCP-Endpointを起動します。

cd ~/unity_ros2_ws
. install/setup.bash
ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=0.0.0.0

続いて、2つ目のターミナルコマンドでURDFロボットをRviz上に表示させます。

cd ~/manta_ws/
. install/setup.bash

ros2 launch manta_v2_bringup manta_bringup.launch.py

最後に、Unity側で「Play」ボタンを押してUnity-ROS間の通信を開始します。

joint-pub-guiでカーソルを動かし、リンクできていれば動作確認完了です。

また実機との連携は単純で、/joint_statesトピックをSubscribeして得た角度データをそのままサーボモータの角度に入れているだけです。

なので連携と言っても、やっていることは

joint-pub-gui →　URDFモデル、Unityモデル、実機

となっているだけです。

上記のJointStatePubは、Unity→URDFモデル・実機となっており、Unity側でpubした/joint_statesでUnity、RvizのURDFモデルと実機を動かすこともできます。

1.5: Visualizationsでjoint_states_msgを可視化

実機を動かしていると、各センサ値・現在の関節角度などを画面上で可視化できると都合が良いことが多々あります。

そこでUnityでは、Robotics Visualizations というパッケージを使用して可視化することが可能です。

Packages > Unity Robotics VisualizationsにDefaultVisualizationsSuite.prefabというアセットがあるので、こちらをHierarchyのシーン内にドラッグ&ドロップで追加しましょう。

追加後、Playボタンで通信してみると

Topics
Transforms
Layout

というUIが追加されているのが分かります。

Topicsから/joint_states : 2Dを選択すると、以下のようにros2 topic echo /joint_statesと同等の機能をUnityでも使用することができます。

std_msg等もVisualizationできるので、普段Rviz上で表示している他のパラメータも可視化できそうですね。

1.6: UnitySensorsROSでRGBカメラを表示する

冒頭で紹介したUnitySensorROSを使用して、機体頭部のRGBカメラをROSノードで通信・確認してみます。

まずは

Packages > UnitySensorsROS > Scripts > Runtime > RGBCameraからRGBCameraPublisher.cs
Packages > UnitySensors > Scripts > Runtime > RGBCameraからRGBCamera.cs

をAssets > Scripts下にコピーします。

続いて、カメラの対象となるオブジェクト(今回は「link_rs_camera_1」)をHierarchyのシーンで選択しておき