３行要約

Qt Creator を使うことで、ROS 2 の GUI アプリケーションを作りたいと思った。

ロボットの動きの可視化（シミュレーション）には Unity を使ったよ。

Qt Creator で作った GUI から Unity 上のロボットモデルを動かしたよ。

はじめに

可視ツールとしては Gazebo を使用することも多いかと思いますが、Gazebo は途中で API の大幅変更が入って初学者には厳しい（特に情報収集の点において）ことや、過去に使用したことがあり、ある程度の理解がある、といった理由により Unity を使用して可視化を行いたいと思います。最近はロボット開発でも使われつつあるようです。

全体図は次の通りです。
Qt 側の UI で角度を指定し、Unity 上のロボットモデルを動作させます。

~~また、動作後のロボットアーム先端の位置・姿勢を順運動学計算により算出し、画面上に表示します。~~
MoveIt を用いた順運動学計算まで行おうと思いましたが、あまりにも記事が長くなりすぎるので、ここでは解説をしません。

過去記事まとめ

この記事では、「インストール・準備編」と「トピック編」の３章まで実装されていることを前提としています。
逆に言うと、それ以降の部分については再度説明をしているところも多いです。適宜読み飛ばしてください。

GitHub レポジトリ

Qt を使用している部分については GitHub で公開しています。

環境

OS : Ubuntu 22.04
Qt : Qt 6.7.2
Qt Creator : Qt Creator 13.0.0
ROS : Humble Hawksbill
Unity : 2022.3.30f1　 ← New

Unityをインストールするために必要となる Unity Hub のインストールは下記公式サイトを参照してください。

あまり知られていない気もしますが、Unity は Ubuntu でも動きます。

初めて Unity を使用する場合は、エディターのインストールが必要です。
Unity Hub からインストールしてください。
保証はできませんが、バージョンについては 2022.3.xxf1 であれば問題なく動くと思います。

インストール完了後は、New project > 3D を選択し、プロジェクトを作成します。
プロジェクト名は任意です。好きにしてください。

Ubuntu 上において、C# エディターとして Visual Studio Code を使用する方法（備忘録）

Visual Studio Codeのインストール (Terminal)

他の方が UbuntuにVSCodeをインストールする3つの方法という記事を残してくれているので、そちらを参照しました。

上記記事内の「方法２」を使用し、インストールを行いました。

sudo snap install --classic code

とターミナルに入力するだけなので、楽で良いですね。ありがとうございます。

Visual Studio Code のインストール場所の確認 (Terminal)

which コマンドでインストールパスを確認しておきます。

~$ which code
/snap/bin/code

/snap/bin/code でした。

C# エディターとして VSCode を指定する (Unity)

Edit > Preferences... を選択します。

External Tools 内の External Scripts Editor において、Browse... を選択し、先ほど確認した Visual Studio Code のインストールパスを選択します。

あとは、C# Script をクリックするだけで、自動的に VSCode が立ち上がると思います。

NVIDIA の GPU を使用できるようにする方法（Ubuntuを物理マシンで動かしている場合）

Unity では OpenGL を使用しているため、ある程度の GPU を搭載している方が快適です。
ここでは、NVIDIA のドライバーをインストールする方法を簡単に紹介します。

搭載されているカードを正しく認識しているかを lspci コマンドにより確認します。

Command

lspci | grep VGA

なお、表示される数字（PCI ID）と、グラボの型番の対応表は NVIDIA の GitHub を参照してください。

対応するドライバを調べます。

Command

ubuntu-drivers devices

recommend されたドライバーのインストールを行います。

Command

sudo apt install nvidia-driver-555

再起動したのち、ドライバーが正しく動作しているかを確認します。

Command

nvidia-smi

1. URDF の読み込み (Unity)

まずは、Unity 上にロボットアームモデルを表示するところから始めましょう。

1.1. URDF-Importer のインストール

URDF (Unified Robot Description Format) で定義されたロボットを Unity へインポートするために URDF-Importer をインストールします。
インストール方法については公式の README を参照してください。

インストールが終わったら、Unity は一旦閉じて構いません。

1.2. URDF の準備

今回は、Elephant Robotics 社製の mycobot 320 m5 2020 を題材として使用します。
（なお、このロボットを採用した理由は、今私の手元に実機があるためです。今回の記事では実機は使いません。）

URDF については公式 GitHub に置かれていたので、ありがたく使わせていただきます。

ここ（mycobot_ros レポジトリ）から、zip ファイルをダウンロードし、解凍し、好きな場所に置いてください。
ここでは、Home ディレクトリ直下に配置したとして進めます。

URDF ファイルは下記のページで閲覧できます。

1.3. URDF の読み込み

ここからの内容は、使用するロボットによって大きく変わる可能性があります。
使用したいロボットに合わせて作業を進めてください。

注意点：「URDFファイルに書かれたパスには気をつけてね」

今回、使用する urdfファイルは次の場所に置かれていました。

mycobot_ros/mycobot_description/urdf/mycobot_320_m5_2020/mycobot_pro_320_m5_2020.urdf

それに対し、urdfファイル内では、

  <link name="base">
    <visual>
      <geometry>
	     <!--- 0.0 0 -0.04  1.5708 3.14159-->
       <mesh filename="package://mycobot_description/urdf/mycobot_320_m5_2020/base.dae"/>
      </geometry>
    <origin xyz = "0.0 0 0 " rpy = " 0 0 3.1415926"/>
    </visual>

などと、//mycobot_description/urdf/mycobot_320_m5_2020にdaeファイルを置くことが記述されています。
そのため、mycobot_description ディレクトリ以下を丸ごと、Unity の Assets/URDF 下へペーストすることにしました。Assets/URDF ディレクトリはあらかじめ作成しておきます。

なお、使用しないロボット（ mycobot 320 m5 2020 以外）に関するディレクトリは削除して構いません。

urdf ファイル側を編集してもよいと思いますが、修正箇所が多いのでやめました。

▲　Unity の Assets/URDF 下へ mycobot_description をペースト

▲　ディレクトリ構成

urdfファイルを Assets/URDF ディレクトリに移動します。
(urdfファイルから見て、//mycobot_description/urdf/mycobot_320_m5_2020 に辿り着けるようにするというイメージです。)

ここからは、ロボットの種類に依らない、共通事項になります。

urdf ファイル（mycobot_pro_320_m5_2020.urdf）を右クリックし、Import Robot from Selected URDF file を選択します。

設定はデフォルトのままでOKです。Import URDF をクリックしてインポートします。

詳細については公式を参照してください。

あとは、辛抱強く（←ここ重要） 待っていると、scene ビューにロボットが表示されます。
場合によっては拡大しないと見えないかもしれません。
また、Hierarchy にも、firefighter という名前のオブジェクトが追加されていると思います。

オブジェクト名（firefighter）は任意の名前に変更して構いません。
また、ロボットが小さかったので、Scale を 10 倍しておきました。

1.4. Articulation Body の追加・編集

このまま実行しても、ロボットが根本から折れて落っこちていきます。シュールですね。

子オブジェクトである、base に Articulation Body コンポーネントがアタッチされていなかったので、アタッチしました。base は床に固定されているので、Immovable にチェックを入れておきます。
これによって、根本から折れることがなくなります。

なお、この状態で再生しても、ロボットが勝手に動きだしてしまうかと思います。
これは、Articulation Body コンポーネントの Use Gravity を On にしているためです。
特に問題ではないので安心してください。

▲　重力に負けてこのような姿勢になってしまうが、これでOK

1.5. 床の作成

3D Object から Plane をクリックし、Plane オブジェクトを作成します。

位置と大きさを調整します。

後は適当にマテリアルを作成し、適用します。

2. ROS-TCP-Connector のインストール (Unity)

ROS-TCP-Connector を使用することで、Qt と Unity の連携ができます。
インストール方法については公式の README を参照してください。

2つのパッケージ両方（ROS-TCP-Connector と Visualizations）を加えておきます。現時点（2024年7月9日）では v0.7.0 が最新版のようです。

TCP とは？（外部リンク集）

「分かりそう」で「分からない」でも「分かった」気になれるIT用語辞典

OSI参照モデルまとめ - Qiita

【図解】初心者にも分かる TCP/UDP 〜違いや共通点,使い分け,ポート番号,具体例について〜

自前で１から TCP を実装するわけではないので、概要が分かればひとまずは大丈夫です。

3. ROS メッセージの生成/ビルド (Unity)

Unity 上で ROS Message を使用できるようにします。
下記の２つの GitHub レポジトリから、それぞれ zip ファイルをダウンロードし、解凍（Extract Here / to）してください。

上が common_msgs 用（Joint_Stateで使用）、下が moveit_msgs 用です。
解凍後のディレクトリは Unity とは関係なく、好きな位置に配置して構いませんが、後で使用するので、パスは覚えておいてください。

▲ 解凍後は任意の位置に配置してOK（ディレクトリ名は変更してあります）

Unity に戻り、上部 Robotics から Generate ROS Messages... を選択します。

ROS message path に解凍後ディレクトリのパス(hoge/common_msgs)を入力します。
今回は、ROS Message として、JointState.msg を使用するので、JointState.msg を探してビルドします。

また、ROS message path に解凍後ディレクトリのパス(hoge/moveit_msgs)を入力します。
RobotTrajectory.msg を使用するので、RobotTrajectory.msg を探してビルド します。

無事、生成されました。

メッセージソースコード

ROS-TCP-Connector における、メッセージのソースコードへのリンクをまとめておきます。
いずれも GitHub へのリンクです。

RosMessageTypes.Sensor.JointStateMsg

RosMessageTypes.Std.HeaderMsg

RosMessageTypes.BuiltinInterfaces.TimeMsg

4. C# スクリプトの実装（Unity）

一般的には、ROS-TCP-Connector に含まれている ROS Connection コンポーネントを使うかと思います。しかし、使い方がよくわからないことに加え、非同期処理の際に UniTask を使用していないことから、自前で実装することにしました。

実装するクラスは全部で３つです。

クラス名	役割
ROSConnector	ROS側（Qt）とのTCP通信・メッセージの受信＆送信
JointsAngleChanger	受信したメッセージに従って各関節を回転させる
JointStatePublisher	動作後の各関節の角度をもとに、JointStateメッセージを発行する

4.1. UniTask のインストール

C# スクリプト内で async/await を使用するため、UniTask のインストールを行います。
Package Manager からインストール可能です。公式 ReadMe を参考にしてください。

async/await についてはこの辺が詳しいです。

冒頭 10 分だけでも見ておくとよいでしょう。

4.2. Scripting Define Symbols への ROS2 の追加

先ほどインストールした Unity-TCP-Connector で定義されているスクリプトでは、

#define ROS2

の記述がなく、ROS1 に設定されています。
この記述を各スクリプトに追記することで、ROS2 に設定することができますが、全てのスクリプトに追記するのは少々面倒です。

Unity では Scripting Define Symbols に登録することで、全てのスクリプトに対し、#define することができます。

Edit > Project Settings を開き、Player の Other Settings 内にある、Scripting Define Symbols にて、"ROS2" を追加し、Apply します。

4.3. ROSConnector.cs の実装

いよいよプログラムを書いていきます。まずは ROSConnector.cs からです。
このクラスは、Qt (ROS2) と TCP 通信を行うためのものです。
送信・受信の双方を担います。

MessageGeneration 名前空間の実装については公式 GitHub を参照してください。

ROSConnector.cs

using System;
using System.Net.Sockets;
using UnityEngine;
using Cysharp.Threading.Tasks;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;
using RosMessageTypes.Sensor;
using RosMessageTypes.Moveit;

public class ROSConnector : MonoBehaviour
{
    // ------------ TCP Connection ------------
    [SerializeField] string serverIP = "127.0.0.1"; // Qtアプリケーションが動作しているIPアドレス
    [SerializeField] int serverPort = 10000;        // Qtアプリケーションのポート番号
    private TcpClient client;
    private NetworkStream stream;

    // ------------ ROS Message ------------
    private MessageDeserializer messageDeserializer;
    private MessageSerializer messageSerializer;
    private JointStateMsg jointStateMsg;
    private RobotTrajectoryMsg robotTrajectoryMsg;

    // ------------ Event ------------
    public delegate void GetRobotTrajectoryMsgDelegate(RobotTrajectoryMsg robotTrajectoryMsg);
    public static event GetRobotTrajectoryMsgDelegate onRobotTrajectoryMsgReceived;

    void Start()
    {
        JointStatePublisher.OnJointStateUpdated += Publish;

        ConnectToServer().Forget();
    }

    async UniTaskVoid ConnectToServer()
    {
        client = new TcpClient();
        messageDeserializer = new MessageDeserializer();

        try
        {
            await client.ConnectAsync(serverIP, serverPort);
        }
        catch(Exception ex)
        {
            Debug.Log(ex.Message + "\nMake sure you are running the Qt application");
            return;
        }

        if (client.Connected)
        {
            Debug.Log("Connected to server!");
            stream = client.GetStream();
            ReceiveData().Forget();
        }
        else
        {
            Debug.Log("Failed to connect to server.");
        }

        return;
    }

    async UniTaskVoid ReceiveData()
    {
        byte[] buffer = new byte[1024];

        // 接続している間、受信し続けたい
        // await を使用しているため、無限ループには陥らない
        while (client.Connected)
        {
            int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length);
            if (bytesRead > 0)
            {
                messageDeserializer.InitWithBuffer(buffer);

                robotTrajectoryMsg = RobotTrajectoryMsg.Deserialize(messageDeserializer);

                onRobotTrajectoryMsgReceived?.Invoke(robotTrajectoryMsg);
            }
        }
    }

    void Publish(JointStateMsg jointStateMsg)
    {
        if(!client.Connected) return;

        messageSerializer = new MessageSerializer();

        messageSerializer.SerializeMessage(jointStateMsg);

        stream.Write(messageSerializer.GetBytes());
    }

    void OnApplicationQuit()
    {
        stream?.Close();
        if(client.Connected) client.Close();
    }
}

【プログラム解説】

名前空間のインポート

ROSConnector.cs(抜粋)

using System;
using System.Net.Sockets;
using UnityEngine;
using Cysharp.Threading.Tasks;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;
using RosMessageTypes.Sensor;
using RosMessageTypes.Moveit;

必要な名前空間を using します。

System.Net.Sockets　:　TCP を用いた通信を行うため
Cysharp.Threading.Tasks　:　UniTask を使用するため（System.Threading.Task ではない）
Unity.Robotics.ROSTCPConnector.MessageGeneration　:　ROS Message のデシリアライズ用
RosMessageTypes.Sensor　:　ROS Message (Joint State Msg) の定義場所
RosMessageTypes.Moveit　:　ROS Message (Robot Trajectory Msg) の定義場所

変数定義

ROSConnector.cs(抜粋)

    // ------------ TCP Connection ------------
    [SerializeField] string serverIP = "127.0.0.1";
    [SerializeField] int serverPort = 10000;
    private TcpClient client;
    private NetworkStream stream;

    // ------------ ROS Message ------------
    private MessageDeserializer messageDeserializer;
    private MessageSerializer messageSerializer;
    private JointStateMsg jointStateMsg;
    private RobotTrajectoryMsg robotTrajectoryMsg;

    // ------------ Event ------------
    public delegate void GetRobotTrajectoryMsgDelegate(RobotTrajectoryMsg robotTrajectoryMsg);
    public static event GetRobotTrajectoryMsgDelegate onRobotTrajectoryMsgReceived;

必要な変数を定義していきます。また、新しいデータを受信したときに発火するイベントも定義します。

Start 関数

ROSConnector.cs(抜粋)

void Start()
{
    JointStatePublisher.OnJointStateUpdated += Publish;

    ConnectToServer().Forget();
}

ただの Start 関数で、ConnectToServer メソッドを呼んでいますが、.Forget() をつけるのがミソです。Forget() は非同期操作の完了を待たずに処理を進めるための拡張メソッドです。これがないと警告が出ます。

なお、Forget() は UniTaskExtensions.cs にて定義されています。

また、JointStatePublisher クラス（後で定義）の OnJointStateUpdated というイベントに Publish() をハンドラーとして追加しています。

ConnectToServer 非同期メソッド（Start() 内で呼び出し）

ROSConnector.cs(抜粋)

    async UniTaskVoid ConnectToServer()
    {
        client = new TcpClient();

        try
        {
            await client.ConnectAsync(serverIP, serverPort);
        }
        catch(Exception ex)
        {
            Debug.Log(ex.Message + "\nMake sure you are running the Qt application");
        }

        if (client.Connected)
        {
            Debug.Log("Connected to server!");
            stream = client.GetStream();
            ReceiveData().Forget();
        }
        else
        {
            Debug.Log("Failed to connect to server.");
        }
    }

await client.ConnectAsync(serverIP, serverPort) によりサーバーへの非同期接続を試みます。接続が成功した場合はデータの受信処理を開始します。

ReceiveData 非同期メソッド

ROSConnector.cs(抜粋)

async UniTaskVoid ReceiveData()
{
    byte[] buffer = new byte[1024];
    while (client.Connected)
    {
        int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length);
        if (bytesRead > 0)
        {
            messageDeserializer.InitWithBuffer(buffer);

            robotTrajectoryMsg = RobotTrajectoryMsg.Deserialize(messageDeserializer);

            onRobotTrajectoryMsgReceived?.Invoke(robotTrajectoryMsg);
        }
    }
}

▽ 非同期でストリーム (stream) からデータをバッファ (buffer) に詰め込みます。

int bytesRead = await stream.ReadAsync(buffer, 0, buffer.Length);

▽ InitWithBuffer() を使用し、bufferを代入しつつ、messageDeserializer を初期化します。
　詳細は MessageDeserializer クラスを参照してください。

messageDeserializer.InitWithBuffer(buffer);

▽ Deserialize() を用いて、データを RobotTrajectoryMsg に復元します。
　詳細は先ほど生成した RobotTrajectoryMsg クラスを参照してください。
　その中に Deserialize() が定義されているはずです。

robotTrajectoryMsg = RobotTrajectoryMsg.Deserialize(messageDeserializer);

▽ イベントを発火します。

onRobotTrajectoryMsgReceived?.Invoke(robotTrajectoryMsg);

Publish （イベントハンドラー）

ROSConnector.cs(抜粋)

void Publish(JointStateMsg jointStateMsg)
    {
        if(!client.Connected) return;

        messageSerializer = new MessageSerializer();
        
        messageSerializer.SerializeMessage(jointStateMsg);

        stream.Write(messageSerializer.GetBytes());
    }

JointStatePublisher クラスから送られてきた JointStateMsg をシリアライズしたのち、Byte型配列をストリームに書き込みます。

終了処理

ROSConnector.cs(抜粋)

 void OnApplicationQuit()
{
    stream?.Close();
    if(client.Connected) client.Close();
}

再生を停止した際の処理です。
OnApplicationQuit は MonoBehaviour クラスで定義されている、Unity 特有のものです。

このスクリプトを実装しただけでは、

ErrorMessage

Assets/Scripts/ROSConnector.cs(29,9): error CS0103:
The name 'JointStatePublisher' does not exist in the current context

というエラーが出ると思います。JointStatePublisher をまだ定義していないので当たり前です。気にせず先へ進んでください。

4.4. JointsAngleChanger.cs の実装

受信したメッセージに従ってロボットの各関節を回転させるためのクラスです。

JointsAngleChanger.cs

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using RosMessageTypes.Moveit;

public class JointsAngleChanger : MonoBehaviour
{
    const int JOINT_NUMBER = 6; 
    float[] _angles = new float[JOINT_NUMBER];

    [SerializeField] ArticulationBody[] _articulationBodies = new ArticulationBody[JOINT_NUMBER];

    [Header("Settings")]
    [SerializeField] float _stiffness = 10000f;
    [SerializeField] float _damping = 100f;
    [SerializeField] float _forceLimit = 10000f;

    void Start()
    {
        for(int jointIndex = 0; jointIndex < JOINT_NUMBER; jointIndex++)
        {
            var xDrive = _articulationBodies[jointIndex].xDrive;

            xDrive.stiffness = _stiffness;
            xDrive.damping = _damping; 
            xDrive.forceLimit = _forceLimit;

            _articulationBodies[jointIndex].xDrive = xDrive;
        }

        ROSConnector.onRobotTrajectoryMsgReceived += OnRobotTrajectoryMsgReceived;
    }

    private void OnRobotTrajectoryMsgReceived(RobotTrajectoryMsg robotTrajectoryMsg)
    {
        UnpackMessage(robotTrajectoryMsg);
        RotateJoints();
    }

    private void UnpackMessage(RobotTrajectoryMsg robotTrajectoryMsg)
    {
        int pointsNumber = robotTrajectoryMsg.joint_trajectory.points.Length;

        for(int i = 0; i < JOINT_NUMBER; i++)
        {
            // 今回は目標地点の角度のみ使用する
            _angles[i] = (float)(robotTrajectoryMsg.joint_trajectory.points[pointsNumber - 1].positions[i] * 180.0f / Math.PI);
        }
    }

    private void RotateJoints()
    {
        for(int jointIndex = 0; jointIndex < JOINT_NUMBER; jointIndex++)
        {
            ArticulationDrive articulationDrive = _articulationBodies[jointIndex].xDrive;

            articulationDrive.target = _angles[jointIndex];

            _articulationBodies[jointIndex].xDrive = articulationDrive;
        }
    }
}

【プログラム解説】

private 変数定義

[SerializeField] ArticulationBody[] _articulationBodies = new ArticulationBody[JOINT_NUMBER];

Articulation Body を使用することで、ロボットアームにおける物理的な連結を表現することができます。Articulation Body クラスのリファレンスはここです。
jointArticulationBodies には各リンクの Articulation Body コンポーネントを格納します。

Start関数

JointsAngleChanger.cs(抜粋)

void Start()
{
    for(int jointIndex = 0; jointIndex < JOINT_NUMBER; jointIndex++)
    {
        var xDrive = _articulationBodies[jointIndex].xDrive;

        xDrive.stiffness = _stiffness;
        xDrive.damping = _damping; 
        xDrive.forceLimit = _forceLimit;

        _articulationBodies[jointIndex].xDrive = xDrive;
    }

    ROSConnector.onRobotTrajectoryMsgReceived += OnRobotTrajectoryMsgReceived;
}

各関節における xDrive の初期設定（剛性や減衰など）を行います。
xDrive は構造体なので、上記のような書き方になります。

_articulationBodies[jointIndex].xDrive.stiffness = _stiffness; // これだとコンパイルエラーになる

また、イベントハンドラーの設定も行います。

UnpackMessage 関数

robotTrajectoryMsg.joint_trajectory.points は配列であり、ここに軌道上の各ポイントごとの情報が格納されています。
今回は、軌道については深く考えずに、目標地点へ到達することのみを考えます。
そのため、配列の最後の要素のみ使用しています。

障害物がある場合などには、軌道が重要になります。

private void UnpackMessage(RobotTrajectoryMsg robotTrajectoryMsg)
{
    int pointsNumber = robotTrajectoryMsg.joint_trajectory.points.Length;

    for(int i = 0; i < JOINT_NUMBER; i++)
    {
        _angles[i] = (float)(robotTrajectoryMsg.joint_trajectory.points[pointsNumber - 1].positions[i] * 180.0f / Math.PI);
    }
}

メッセージから、各ジョイントの角度の情報を取得し、_angle[] に格納します。
メッセージのおける角度はラジアン表記となっているので 180.0f / Math.PI を掛けて deg に変換しておきます。

RotateJoints 関数

private void RotateJoints()
{
    for(int jointIndex = 0; jointIndex < JOINT_NUMBER; jointIndex++)
    {
        ArticulationDrive articulationDrive = _articulationBodies[jointIndex].xDrive;

        articulationDrive.target = _angles[jointIndex];

        _articulationBodies[jointIndex].xDrive = articulationDrive;
    }
}

ArticulationDrive の target に関節角度を代入し、各リンクにアタッチされている Articulation Body コンポーネントを更新することで、ロボットモデルの関節を回します。

【補足】RobotTrajectoryMsg の ToString について

RobotTrajectoryMsg クラスにおいて、ToString() はオーバーライドされており、いい感じに出力されるようになっています。デバッグの際には役立つこともあると思います。

Debug.Log(robotTrajectoryMsg.ToString());

▲ 出力はこんな感じ

4.5. JointStatePublisher.cs の実装

0.5 秒間隔で ArticulationBody から各関節の角度を取得して JointStateMsg を作成します。
プログラム自体は、JointStateMsg を new してメッセージを作成し、イベントを発火しているだけなので、詳しい解説は省略します。

_jointStateMessage.name[i] には関節名を入れることに注意してください。
リンク名ではありません。　

URDF ファイルにおける、
<joint name="joint2_to_joint1" type="revolute"> の部分を参照してください。

JointStatePublisher.cs

using System;
using UnityEngine;
using RosMessageTypes.Sensor;

public class JointStatePublisher : MonoBehaviour
{
    const int JOINT_NUMBER = 6;

    [SerializeField] ArticulationBody[] _articulationBodies = new ArticulationBody[JOINT_NUMBER];

    string[] _jointName = new string[6]{
        "joint2_to_joint1", "joint3_to_joint2" , "joint4_to_joint3" , "joint5_to_joint4" , "joint6_to_joint5" , "joint6output_to_joint6"
    };

    public static event Action<JointStateMsg> OnJointStateUpdated;
    
    [SerializeField,Range(0.01f,2.0f)] float _publishMessageFrequency = 0.5f;
    private float _timeElapsed;

    private JointStateMsg _jointStateMessage;

    void Update()
    {
        _timeElapsed += Time.deltaTime;
        
        if (_timeElapsed < _publishMessageFrequency) return;

        JointStateMsg _jointStateMessage = new JointStateMsg
        {
            name = new string[_articulationBodies.Length],
            position = new double[_articulationBodies.Length],
            velocity = new double[_articulationBodies.Length],
            effort = new double[_articulationBodies.Length],
            header = new RosMessageTypes.Std.HeaderMsg
            {
                stamp = new RosMessageTypes.BuiltinInterfaces.TimeMsg
                {
                    sec = (int)DateTimeOffset.UtcNow.ToUnixTimeSeconds(),
                    nanosec = (uint)(DateTime.Now.Millisecond * 1000000),
                },
                frame_id = ""
            },
        };

        for (int i = 0; i < _articulationBodies.Length; i++)
        {
            _jointStateMessage.name[i] = _jointName[i];
            _jointStateMessage.position[i] = (_articulationBodies[i].jointPosition)[0];
        }

        OnJointStateUpdated?.Invoke(_jointStateMessage);
        _timeElapsed = 0;
    }
}

4.6. 作成したスクリプトのアタッチ

Create Empty で空オブジェクトを作成します。（名前は ROS-Connector にしました。）
その後、実装した ROSConnector.cs をアタッチします。

Server IP には 127.0.0.1 ( = localhost ) または仮想マシンの IP アドレスを入力します。
今回は、Unity も Qt も同一OS上で動かしているので、127.0.0.1 とします。

今回の記事では扱いませんが、Qt ( ROS2 ) を Ubuntu（ゲストOS）で動かし、Windows （ホストOS）で動かしている Unity で可視化する、なんてこともできるようです。

Server Port は 10000（デフォルト）のままで OK です。

作成した JointsAngleChanger を、先ほど ROS Connection をアタッチしたオブジェクトに、アタッチします。
その後、 Joint Articulation Bodies に各リンク（の Articulation Body ）をドラッグ＆ドロップで登録します。今回使用するロボットは６軸なので、配列の長さも６になります。
なお、Articulation Body クラスのリファレンスはここです。

同様に Create Empty で空オブジェクトを作成します。（名前は Robot-Controller にしました。）
その後、実装した JointStatePublisher.cs をアタッチします。

また、同様にJoint Articulation Bodies に各リンク（の Articulation Body ）をドラッグ＆ドロップで登録します。

これらのスクリプトでは、警告が８つくらい出ると思います。
原因は名前の衝突なのですが、解決法がまだわかっていないので放置しています。

メッセージの定義が書いてあるスクリプトを編集しようと思いましたが、そこには
//Do not edit! This file was generated by Unity-ROS MessageGeneration.
と書いてあるので、編集するのは気が引けます。

解決法が分かるまではこのままで進めます。

ここまでで、Unity での実装は終了です。次の項目からは Qt での実装になります。
Unity は一旦閉じても構いません。

5. TCP を用いた通信の実装 (UnityConnector.cpp) (Qt)

5.1. Qt の準備

src ディレクトリ内に、3つのディレクトリを作成しておきます。
.cppファイルは source に、.hファイルは header に、.uiファイルは ui に作成していきます。

Qt を閉じている場合は .workspace をクリックしてプロジェクトを開いてください。

5.2. CMAKE_PREFIX_PATH の編集 [重要]

CMake で find_package をした際に、Qt のパッケージを見つけられるように、CMAKE_PREFIX_PATH にパスを追加しておきます。
CMAKE_PREFIX_PATH は「プロジェクト」内の「ビルド」で編集できます。

入力するパスは、どこに Qt をインストールしたかによって変わりますが、

<installed path>/Qt/<version e.g. 6.7.2>/gcc_64/lib/cmake

のような恰好でしょう。

私の場合は下記のようになりました。

CMAKE_PREFIX_PATH=/home/yupopoi/ROS-App/install/ros_app:/home/yupopoi/Qt/6.7.2/gcc_64/lib/cmake

もし、上記のようなディレクトリが見つからない（6.7.x ディレクトリが存在しない場合）は、Qt Maintenance Tool を使用してインストールしてください。

5.3. CMakesLists.txt への追記（Qt6 Core Widgets）

MOC, UIC, RCC の自動化

まず、Qtで使用するツールを自動で実行できるように設定します。

MOC（Meta-Object Compiler）は、QtのC++拡張を扱うプログラムです。シグナル・スロットもこれを使っているそうです。

UIC（User Interface Compiler）は、Qt Designer が生成した XML 形式のUI定義 (.ui) ファイルを読み込み、対応する C++ ヘッダーファイルまたは Python ソースファイルを作成します。

RCC（Resource Compiler）は、ビルド中にリソースを Qt アプリケーションに埋め込むために使用されます。ここでいう「リソース」は画像や音声などです。

CMakeLists.txt（抜粋）

set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTOUIC ON)
set(CMAKE_AUTORCC ON)

必要なコンポーネントの追加

Qt6ライブラリのうち、 Core と Widgets と Network コンポーネントを探し、プロジェクトに追加します。

CMakeLists.txt（抜粋）

find_package(Qt6 COMPONENTS Core Widgets Network REQUIRED)
qt_standard_project_setup()

インクルードパスの追加

CMAKE_CURRENT_BINARY_DIRに存在するヘッダーファイルへのインクルードパスを追加します。具体的なヘッダーファイルの例としては ui_mainwindow.h があります。
また、/src/header , /src/source , /src/ui へのインクルードパスも追加しておき,
C++ プログラム上で長いパスを書かなくても済むようにしておきます。

CMakeLists.txt（抜粋）

include_directories(${CMAKE_CURRENT_BINARY_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/header)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/source)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/ui)

この段階での CMakeLists.txt 全文

CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(ros_app)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTOUIC ON)
set(CMAKE_AUTORCC ON)

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(Qt6 COMPONENTS Core Widgets REQUIRED)
qt_standard_project_setup()

include_directories(${CMAKE_CURRENT_BINARY_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/header)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/source)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src/ui)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

5.4. UnityConnector.cpp の実装

Unity とやり取りをするための ROS 側のパッケージとしては ROS-TCP-Endpoint が用意されてますが、うまく Qt Creator に組み込めなかったので、Qt 側については QTcpServer class を継承して実装することにします。

また、ROS-TCP-Endpoint は python で書かれているのに対し、C++で書きたかったという理由もあります。

5.4.1. ファイル作成

header ディレクトリに UnityConnector.h を、source ディレクトリに UnityConnector.cpp をそれぞれ作成します。また、source ディレクトリに main.cpp を作成しておきます。

5.4.2. CMakeLists.txt の編集

実行可能ファイルの作成

MainWindow という名前の実行可能ファイルを作成します。
ソースファイル、ヘッダーファイルを含めます。
（後で変換された UI ヘッダーファイル（${UI_HEADERS}）も含めます。）

ここから先、新しい .cpp / .h ファイルを作成するたびに、add_executable に追記していってください。 そうしないと実行可能ファイルに追加されません。

CMakeLists.txt（抜粋）

add_executable(MainWindow
    src/source/main.cpp
    src/header/UnityConnector.h
    src/source/UnityConnector.cpp
)

他ライブラリとのリンク付け

CMakeLists.txt（抜粋）

ament_target_dependencies(MainWindow rclcpp std_msgs)
target_link_libraries(MainWindow ${Qt6Core_LIBRARIES})
target_link_libraries(MainWindow ${Qt6Widgets_LIBRARIES})
target_link_libraries(MainWindow ${Qt6Network_LIBRARIES})

5.4.3. ビルド・実行可能ファイルの設定

ここで一旦ビルドを通しておきます。

その後、カスタム実行構成に実行ファイルを設定します。左のバーにある「プロジェクト」から「実行」を選択します。
「実行ファイル」に、build/<package name> 内にある実行ファイルのパスを入力します。
「作業ディレクトリ」には src ディレクトリを指定します。

この段階で実行すると...

エラー: /usr/lib/gcc/x86_64-linux-gnu/11/../../../x86_64-linux-gnu/Scrt1.o: in function `_start':

のようなエラーが現れます。これは main 関数が存在していないことに起因して起こります。

main.cpp

#include <rclcpp/rclcpp.hpp>

int main(int argc, char *argv[])
{
    std::cout << "Hello, World" << std::endl;
}

なんでもいいので、main.cpp 内に main 関数を用意してあげるとエラーが発生しなくなります。

5.4.4. プログラム実装

UnityConnector.h

UnityConnector.h

#ifndef UNITYCONNECTOR_H
#define UNITYCONNECTOR_H

#include <QObject>
#include <QTcpServer>
#include <QTcpSocket>

class UnityConnector : public QTcpServer
{
    Q_OBJECT

    public:
        explicit UnityConnector(int port = 10000, QObject *parent = nullptr);
        int startListen();
        int Publish(std::shared_ptr<const QByteArray> message);
        QByteArray SubscribedData();

    signals:
        void onSubscribedDataFromUnity();

    private slots:
        void onNewConnection();
        void Read();

    private:
        QTcpSocket *socket;
        int port_;
        QByteArray subscribedData_;
};

#endif // UNITYCONNECTOR_H

UnityConnector.cpp

#include "UnityConnector.h"

UnityConnector::UnityConnector(int port, QObject *parent) : QTcpServer(parent), socket(nullptr)
{
    port_ = port;
    connect(this, &QTcpServer::newConnection, this, &UnityConnector::onNewConnection);
}

int UnityConnector::startListen()
{
    if (this->listen(QHostAddress::Any, port_))
    {
        return 1;
    }

    return 0;
}

int UnityConnector::Publish(std::shared_ptr<const QByteArray> message)
{
    if(socket == nullptr) return 0;

    if (socket->isOpen()) {

        socket->write(message->constData(), message->size());
        return 1;
    }

    return 0;
}

QByteArray UnityConnector::SubscribedData()
{
    return subscribedData_;
}

void UnityConnector::onNewConnection()
{
    socket = this->nextPendingConnection();

    connect(socket, &QTcpSocket::readyRead, this, &UnityConnector::Read);
}

void UnityConnector::Read()
{
    subscribedData_ = socket->readAll();

    emit onSubscribedDataFromUnity();
}

【プログラム解説】

コンストラクタ

UnityConnector.cpp(抜粋)

UnityConnector::UnityConnector(int port, QObject *parent) : QTcpServer(parent), socket(nullptr)
{
    port_ = port;
    connect(this, &QTcpServer::newConnection, this, &UnityConnector::onNewConnection);
}

UnityConnector クラスは QTcpServer クラスを継承しています。ここでは、QTcpServer クラス（親クラス）のコンストラクタを呼び出しています。

&QTcpServer::newConnection は QTcpServer クラスで定義されているシグナルで、新しいコネクションが利用可能になる際に発行されます。
シグナルが発行されたら、onNewConnection が呼ばれるようにしておきます。

また、socket は QTcpSocket クラスのポインタで、nullptr に初期化されます。

Publish 関数

UnityConnector.cpp(抜粋)

int UnityConnector::Publish(std::shared_ptr<const QByteArray> message)
{
    if(socket == nullptr) return 0;

    if(socket->isOpen()) {

        socket->write(message->constData(), message->size());
        return 1;
    }

    return 0;
}

QByteArray に変換した JointState メッセージを TCP を用いて送信します。
その際には QTcpSocket クラスの write 関数を使用します。

onNewConnection スロット

UnityConnector.cpp(抜粋)

void UnityConnector::onNewConnection()
{
    socket = this->nextPendingConnection();
}

nextPendingConnection() によって最初の待機中の接続リクエストが取り出され、それに対応する QTcpSocket オブジェクトが返されて、QTcpSocket *socket に代入されます。

5.5. main 関数の編集（Option / Qt）

ここで一旦 main 関数を編集して、Qt と Unity 間で通信ができることを確かめます。
飛ばして６章に進んでも構いません。

std::make_shared<UnityConnector>() によって、先ほど実装した UnityConnector クラスのインスタンスを作成し、それを管理するポインタを生成します。
その後、startListen() により、着信接続を listen するようサーバーに指示します。

main.cpp

#include "UnityConnector.h"

#include <rclcpp/rclcpp.hpp>
#include <QtWidgets/QApplication>

int main(int argc, char *argv[])
{
    auto unityConnector = std::make_shared<UnityConnector>();

    QApplication a(argc, argv);

    unityConnector->startListen();

    return a.exec();
}

5.6. 通信ができているか確認（Option / Qt & Unity）

▷ をクリックして、実行可能ファイルを実行します。

その後、Unity 側も実行（再生）すると、

という、ログが Console に出力されると思います。これで完了です。

6. GUI 作成

先に GUI を作ってしまいましょう。

また、この章では下記の２つのクラスを実装します。

クラス名	役割
mainwindow	UI の保持・管理
JointAngleChanger	値が変更されたことを検知し、シグナルを発行する

6.1. uiファイルの作成

ui ディレクトリ内に ui ファイルを作成します。

次の画面では、Main Window を選択します。

ファイル名はデフォルトで良いでしょう。

6.2. GUI レイアウト設定

Qt Designer で GUI を作成します。

5 Terminal において、designer と入力し実行すると、Qt Designer が起動します。

６軸ロボットアームを使用するので、QDoubleSpinBox を６つ用意しました。小数を扱うため、 Double SpinBox を使用します。

将来的に、逆運動学による制御を行うことも考え、FK（順運動学）と IK（逆運動学）を切り替えられるように Tab Widget を用意しておきます。

また、画面下部には 2つ QLabel を用意しておき、関節の角度やロボットアーム先端の位置・姿勢を表示できるようにします。

▲ オブジェクトインスペクタ

▲ プロパティエディターの suffix を編集すると末尾に文字を付けられる。

完成形はこのようになります。

今回、詳細については解説しませんが、IK 側の UI も作成しておきました。
IK 側にも SpinBox が６つあることだけ頭に入れておいてもらえるとこの先が理解しやすくなると思います。

6.3. CMakeLists の編集

ui_mainwindow.h の自動生成

CMakeLists.txt（抜粋）

set(CMAKE_AUTOUIC_SEARCH_PATHS src/ui)
set(UI_FILES src/ui/mainwindow.ui)
qt6_wrap_ui(UI_HEADERS ${UI_FILES})

CMAKE_AUTOUIC_SEARCH_PATHS に、Qt の自動 UI コンパイル（qt6_wrap_ui）のための検索パスを設定します。そのため、mainwindow.ui を含むディレクトリを指定します。

qt6_wrap_ui マクロを使用して、指定された UI ファイル（ここでは mainwindow.ui）を C++ のヘッダーファイル（ui_mainwindow.h）に変換します。${UI_HEADERS} には変換されたヘッダーファイルが設定されます。

実行可能ファイルの作成

add_executable(MainWindow
    src/source/main.cpp
    src/header/UnityConnector.h
    src/source/UnityConnector.cpp
    ${UI_HEADERS} # 追加
)

実行可能ファイルに変換された UI ヘッダーファイル（${UI_HEADERS}）を含めます。

6.4. JointAngleChanger.cpp

JointAngleChanger.cpp と JointAngleChanger.h を作成し、CMakeLists も編集します。

CMakeLists.txt

CMakeLists.txt（抜粋）

add_executable(MainWindow
    src/source/main.cpp
    src/header/UnityConnector.h
    src/source/UnityConnector.cpp
    src/header/JointAngleChanger.h    # 追記
    src/source/JointAngleChanger.cpp  # 追記
    ${UI_HEADERS}
)

JointAngleChanger.h

JointAngleChanger.h

#ifndef JOINTANGLECHANGER_H
#define JOINTANGLECHANGER_H

#include <QObject>
#include <QVector>
#include <QDoubleSpinBox>

class JointAngleChanger : public QObject
{
    Q_OBJECT

public:
    JointAngleChanger(QVector<QDoubleSpinBox *> spinBoxes);
    QVector<double> JointAngles(){return jointAngles_;};

signals:
    void onAnyChanged();

private:
    void onSpinboxChanged(int jointNumber, double value);
    QVector<QDoubleSpinBox *> spinBoxes_;
    QVector<double> jointAngles_;
};

#endif // JOINTANGLECHANGER_H

JointAngleChanger.cpp

#include "JointAngleChanger.h"

JointAngleChanger::JointAngleChanger(QVector<QDoubleSpinBox *> spinBoxes)
{
    spinBoxes_ = QVector<QDoubleSpinBox *>(spinBoxes.size());

    for(int i = 0; i < spinBoxes.size(); i++)
    {
        spinBoxes_[i] = spinBoxes[i];
        spinBoxes_[i]->setRange(-170.0, 170.0);
        spinBoxes_[i]->setSingleStep(1.0);

        QObject::connect(spinBoxes_[i],
                         QOverload<double>::of(&QDoubleSpinBox::valueChanged),this,
                         [=](double value)
                         {
                            onSpinboxChanged(i, value);
                         });
    }

    jointAngles_ = QVector<double>(spinBoxes.size());
}

void JointAngleChanger::onSpinboxChanged(int jointNumber, double value)
{
    jointAngles_[jointNumber] = value;

    emit onAnyChanged();
}

【プログラム解説】

コンストラクタ

JointAngleChanger.cpp（抜粋）

QObject::connect(spinBoxes_[i],
                 QOverload<double>::of(&QDoubleSpinBox::valueChanged),this,
                 [=](double value)
                 {
                    onSpinboxChanged(i, value);
                 });

コンストラクタ内の connect 関数により、DoubleSpinBox の値が変更された際、 onSpinboxChanged が実行されるようにしておきます。
（値の変更検知＆変更された SpinBox とその値を伝達）

onSpinboxChanged 関数

JointAngleChanger.cpp（抜粋）

void JointAngleChanger::onSpinboxChanged(int jointNumber, double value)
{
    jointAngles_[jointNumber] = value;

    emit onAnyChanged();
}

変更された値を保存するとともに、onAnyChanged を発行することで、値が変更されたことをクラス外部に伝えます。

6.5. mainwindow.cpp

mainwindow.cpp と mainwindow.h に作成し、CMakeLists も編集します。

CMakeLists.txt

CMakeLists.txt（抜粋）

add_executable(MainWindow
    src/source/main.cpp
    src/header/UnityConnector.h
    src/source/UnityConnector.cpp
    src/header/JointAngleChanger.h
    src/source/JointAngleChanger.cpp
    src/header/mainwindow.h    # 追記
    src/source/mainwindow.cpp  # 追記
    ${UI_HEADERS}
)

mainwindow.h

mainwindow.h

#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QtWidgets>
#include <QtWidgets/QMainWindow>

#include <QLabel>
#include <QVector>

#include "ui_mainwindow.h"
#include "JointAngleChanger.h"

QT_BEGIN_NAMESPACE
namespace Ui
{
class MainWindow;
}
QT_END_NAMESPACE

class mainwindow : public QMainWindow
{
    Q_OBJECT

public:
    explicit mainwindow(QWidget *parent = nullptr);
    QVector<double> JointAngles() const;
    void UpdateStateLog(const QString &angle, const QString &position, const std::vector<double> &angles, const std::vector<double> &transform);

signals:
    void onAngleChanged();

private:
    std::unique_ptr<Ui::MainWindow> ui = nullptr;
    std::unique_ptr<JointAngleChanger> jointAngleChanger = nullptr;

    QVector<QDoubleSpinBox *> spinBoxes_FK;
    QVector<QDoubleSpinBox *> spinBoxes_IK;
    QTabWidget *kinemaTab = nullptr;

    std::shared_ptr<QLabel> angleLog = nullptr;
    std::shared_ptr<QLabel> positionLog = nullptr;
};
#endif // MAINWINDOW_H

mainwindow.cpp

#include "mainwindow.h"

mainwindow::mainwindow(QWidget *parent) : QMainWindow(parent)
{
    ui = std::make_unique<Ui::MainWindow>();
    ui->setupUi(this);
    kinemaTab = ui->KinemaTab;

    // ----- GUI Angle / Pose Inputs -----

    spinBoxes_FK.clear();
    spinBoxes_FK.append(ui->AngleSpinBox_1);
    spinBoxes_FK.append(ui->AngleSpinBox_2);
    spinBoxes_FK.append(ui->AngleSpinBox_3);
    spinBoxes_FK.append(ui->AngleSpinBox_4);
    spinBoxes_FK.append(ui->AngleSpinBox_5);
    spinBoxes_FK.append(ui->AngleSpinBox_6);

    jointAngleChanger = std::make_unique<JointAngleChanger>(spinBoxes_FK);
    QObject::connect(jointAngleChanger.get(), &JointAngleChanger::onAnyChanged, this, [this](){emit onAngleChanged();});

    spinBoxes_IK.clear();
    spinBoxes_IK.append(ui->AngleSpinBox_posx);
    spinBoxes_IK.append(ui->AngleSpinBox_posy);
    spinBoxes_IK.append(ui->AngleSpinBox_posz);
    spinBoxes_IK.append(ui->AngleSpinBox_rr);
    spinBoxes_IK.append(ui->AngleSpinBox_rp);
    spinBoxes_IK.append(ui->AngleSpinBox_ry);

    // ----- Log -----
    angleLog = std::make_shared<QLabel>(ui->AnglesLog);
    positionLog = std::make_shared<QLabel>(ui->PositionLog);
    angleLog->setText("Joint angle information is displayed here. This sentence will be overwritten later.");
    positionLog->setText("Position and orientation information is displayed here. This sentence will be overwritten later.");
}

QVector<double> mainwindow::JointAngles() const
{
    return jointAngleChanger->JointAngles();
}

void mainwindow::UpdateStateLog(const QString &angle, const QString &position, const std::vector<double> &angles, const std::vector<double> &transform)
{
    angleLog->setText(angle);
    positionLog->setText(position);

    int tabIndex = kinemaTab->currentIndex();

    if(tabIndex == 0) // FK
    {
        for(int boxIndex = 0; boxIndex < spinBoxes_FK.size(); boxIndex++)
        {
            spinBoxes_FK[boxIndex]->blockSignals(false);
            spinBoxes_IK[boxIndex]->blockSignals(true);
        }

        for(int boxIndex = 0; boxIndex < spinBoxes_IK.size(); boxIndex++)
        {
            spinBoxes_IK[boxIndex]->setValue(transform[boxIndex]);
        }
    }
    else if(tabIndex == 1) //IK
    {
        for(int boxIndex = 0; boxIndex < spinBoxes_FK.size(); boxIndex++)
        {
            spinBoxes_FK[boxIndex]->blockSignals(true);
            spinBoxes_IK[boxIndex]->blockSignals(false);
        }

        if(angles.size() == 0) return;

        for(int boxIndex = 0; boxIndex < spinBoxes_IK.size(); boxIndex++)
        {
            spinBoxes_FK[boxIndex]->setValue(angles[boxIndex] * 180 / M_PI); // unit : deg
        }
    }
}

【プログラム解説】

コンストラクタ

spinBoxes_FK.clear();
spinBoxes_FK.append(ui->AngleSpinBox_1);
spinBoxes_FK.append(ui->AngleSpinBox_2);
spinBoxes_FK.append(ui->AngleSpinBox_3);
spinBoxes_FK.append(ui->AngleSpinBox_4);
spinBoxes_FK.append(ui->AngleSpinBox_5);
spinBoxes_FK.append(ui->AngleSpinBox_6);

jointAngleChanger = std::make_unique<JointAngleChanger>(spinBoxes_FK);
QObject::connect(jointAngleChanger.get(), &JointAngleChanger::onAnyChanged, this, [this](){emit onAngleChanged();});

spinBoxes_FK に SpinBox を追加していきます。また、何かしらの値が変更された際にそのことを伝えるシグナルを発行できるようにします。
IK についても同様の処理を行いますが、ここではシグナル発行は実装していません。

UpdateStateLog 関数

Log に情報を表示します。また、SpinBox における数値も自動で変更できるようにします。
例えば、FK 側で数値を変更した場合に、それに対応して IK 側の数値も変更するといったことです。

その際、

spinBoxes_FK[boxIndex]->blockSignals(false);
spinBoxes_IK[boxIndex]->blockSignals(true);

として、直接 UI から数値を変更していない SpinBox からシグナルが発行されないようにしておきます。
（とはいえ、今回は順運動学計算までは行わないので、特に意味はありません。）

6.6. main.cpp 関数の編集

mainwindow を表示できるようにします。

main.cpp

#include "mainwindow.h"
#include "UnityConnector.h"

#include <rclcpp/rclcpp.hpp>
#include <QtWidgets/QApplication>

int main(int argc, char *argv[])
{
    auto unityConnector = std::make_shared<UnityConnector>();

    QApplication a(argc, argv);

    mainwindow mainWindow;
    mainWindow.show();

    unityConnector->startListen();

    return a.exec();
}

ここで、一旦ビルド＆実行してみてもいいでしょう。

7. TCP用スクリプトの作成 (Qt)

一番きつかった部分です。（感想）

クラス名	役割
UnityConnector（５章）	Unity との TCP 通信・メッセージの受信＆送信
SerializedTrajectoryMaker	シリアライズ済の軌道情報を生成する
JointStateHandler	JointStateメッセージをデシリアライズする

7.1. moveit_msg のインストール

今回は、先を見据えて moveit_msgs を使用します。
コマンドを使用してインストールします。

Command

sudo apt-get install ros-humble-moveit-msgs

7.2. CMakeLists.txt の編集

sensor_msgs と moveit_msgs を使用するため、CMakeLists.txt の編集を行います。

CMakeLists.txt（抜粋）

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)   #この行を追加
find_package(moveit_msgs REQUIRED)   #この行も追加

find_package(Qt6 COMPONENTS Core Widgets Network REQUIRED)
qt_standard_project_setup()

CMakeLists.txt(抜粋)

ament_target_dependencies(MainWindow rclcpp std_msgs sensor_msgs moveit_msgs) #sensor_msgs と moveit_msgs を追加 
target_link_libraries(MainWindow ${Qt6Core_LIBRARIES})
target_link_libraries(MainWindow ${Qt6Widgets_LIBRARIES})

7.3. SerializedTrajectoryMaker の実装

RobotTrajectoryMsg を QByteArray に変換し UnityConnector へ渡すためのクラスです。
~~実は、実装する上で一番大変だったところでした。~~

SerializedTrajectoryMaker.h

#ifndef SERIALIZEDTRAJECTORYMAKER_H
#define SERIALIZEDTRAJECTORYMAKER_H

#include <QVector>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/serialization.hpp>

#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <trajectory_msgs/msg/joint_trajectory.h>
#include <trajectory_msgs/msg/joint_trajectory_point.hpp>

class SerializedTrajectoryMaker
{
    public:
        static void Serialize(moveit_msgs::msg::RobotTrajectory trajectory);
        static void createSingleTrajectorySerializedMessage(QVector<double> angles, bool isRadian = true);
    
        static std::shared_ptr<const QByteArray> getSerializedMessage()
        {
            return std::shared_ptr<const QByteArray>(serializedMessage_);
        };
    
    private:
        static std::shared_ptr<QByteArray> serializedMessage_;
};

#endif // SERIALIZEDTRAJECTORYMAKER_H

SerializedTrajectoryMaker.cpp

#include "SerializedTrajectoryMaker.h"

std::shared_ptr<QByteArray> SerializedTrajectoryMaker::serializedMessage_ = std::make_shared<QByteArray>();

void SerializedTrajectoryMaker::Serialize(moveit_msgs::msg::RobotTrajectory trajectory)
{
    trajectory.joint_trajectory.joint_names = {"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"};

    auto now = std::chrono::system_clock::now();
    auto unix_seconds = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
    auto unix_nanoseconds = std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();

    trajectory.joint_trajectory.header.stamp.sec = unix_seconds;
    trajectory.joint_trajectory.header.stamp.nanosec = unix_nanoseconds;

    auto createSerializeMessage = [trajectory]()
    {
        rclcpp::Serialization<moveit_msgs::msg::RobotTrajectory> serializer;
        rclcpp::SerializedMessage serializedMessage;

        serializer.serialize_message(&trajectory, &serializedMessage);
        return serializedMessage;
    };

    auto serialized_msg = createSerializeMessage();

    const uint8_t* msg_uint_ptr = serialized_msg.get_rcl_serialized_message().buffer;

    serializedMessage_ = std::make_shared<QByteArray>(reinterpret_cast<const char*>(msg_uint_ptr), serialized_msg.size());
}

void SerializedTrajectoryMaker::createSingleTrajectorySerializedMessage(QVector<double> angles, bool isRadian)
{
    moveit_msgs::msg::RobotTrajectory trajectory;
    trajectory_msgs::msg::JointTrajectory joint_trajectory;
    trajectory_msgs::msg::JointTrajectoryPoint point;
    
    if(!isRadian)
    {
        for(int i = 0; i < angles.size(); i++)
        {
            angles[i] = angles[i] * M_PI / 180;
        }
    }

    point.positions = {angles[0], angles[1], angles[2], angles[3], angles[4], angles[5]};

    joint_trajectory.points.push_back(point);

    trajectory.joint_trajectory = joint_trajectory;

    Serialize(trajectory);
}

【プログラム解説】

Serialize 関数

moveit_msgs::msg::RobotTrajectory を受け取って、それをシリアライズする関数です。

メッセージ追記

trajectory.joint_trajectory.joint_names = {"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"};

auto now = std::chrono::system_clock::now();
auto unix_seconds = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
auto unix_nanoseconds = std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();

trajectory.joint_trajectory.header.stamp.sec = unix_seconds;
trajectory.joint_trajectory.header.stamp.nanosec = unix_nanoseconds;

trajectory.joint_trajectory.joint_names には関節の名前を記入できるようになっていますが、ここでは任意の名前でOKです。
その下には、タイムスタンプの設定を行っています。必要というわけではないですが、デバッグの際の手掛かりにはなるでしょう。

時刻（秒）の型は int のようです。まあ、これによって問題が起こるのは2038年のことなので、気にしないでおきましょう。

ラムダ式

auto createSerializeMessage = [trajectory]()
{
    rclcpp::Serialization<moveit_msgs::msg::RobotTrajectory> serializer;
    rclcpp::SerializedMessage serializedMessage;

    serializer.serialize_message(&trajectory, &serializedMessage);
    return serializedMessage;
};

auto serialized_msg = createSerializeMessage();

ここでは、Serialize 関数が受け取った trajectory メッセージをキャプチャーし、シリアライズします。

シリアライズ後のメッセージは serialized_msg に格納されます。
面倒なので型推論を用いていますが、実際には rclcpp::SerializedMessage 型です。

各クラス・構造体のリファレンスへのリンクを下記にまとめておきます。

rclcpp::Serialization

rclcpp::SerializationBase (上記Serializationクラスの基底クラス)

rclcpp::SerializedMessage

rcutils_uint8_array_t ( = rcl_serialized_message_t )

Byte列（QByteArray）に変換

const uint8_t* msg_uint_ptr = serialized_msg.get_rcl_serialized_message().buffer;

serializedMessage_ = 
    std::make_shared<QByteArray>(reinterpret_cast<const char*>(msg_uint_ptr), serialized_msg.size());

buffer により、シリアライズされたメッセージデータを保持するポインタを取得します。
その後、reinterpret_cast<const char*>() により、uint8_t* を char* へ（強制的に）キャストし、QByteArray の共有ポインタに変換します。

これにより、メッセージを QTcpSocket を介して Unity に送ることができるようになります。

createSingleTrajectorySerializedMessage 関数

やたらと長い関数名ですが、これは目標とする関節角度を１組（６軸なので６つの角度）のみを保持している RobotTrajectory メッセージを作成し、それをシリアライズします。

今回は、GUI から各関節の角度を入力し、それらの情報を Unity に送信することを目標としています。
そのため、必ずしも RobotTrajectory を使用する必要はなく、Joint State メッセージを使用しても実装自体は出来ます。
しかし、後々 MoveIt を使用するなどといったことを考えると、この段階で RobotTrajectory を使用してしまった方が楽なのでそうしておきます。

SerializedTrajectoryMaker.cpp(抜粋)

void SerializedTrajectoryMaker::createSingleTrajectorySerializedMessage(QVector<double> angles, bool isRadian)
{
    moveit_msgs::msg::RobotTrajectory trajectory;
    trajectory_msgs::msg::JointTrajectory joint_trajectory;
    trajectory_msgs::msg::JointTrajectoryPoint point;

    joint_trajectory.joint_names = {"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"};

    if(!isRadian)
    {
        for(int i = 0; i < angles.size(); i++)
        {
            angles[i] = angles[i] * M_PI / 180;
        }
    }

    point.positions = {angles[0], angles[1], angles[2], angles[3], angles[4], angles[5]};

    joint_trajectory.points.push_back(point);

    trajectory.joint_trajectory = joint_trajectory;

    Serialize(trajectory);
}

push_back(point) によって、角度情報を joint_trajectory の末尾に追加します。
その後、trajectory を引数として、Serialize() を実行し、シリアライズします。

getSerializedMessage 関数

static std::shared_ptr<const QByteArray> getSerializedMessage()
{
    return std::shared_ptr<const QByteArray>(serializedMessage_);
};

ヘッダーファイル内に定義してある値返却用関数です。
シリアライズしたメッセージ（QByteArray形式）の共有ポインタを返します。
static 関数にすることで、インスタンス化する必要がなくなります。

7.4. JointStateHandler の実装

Unity から送られてきた JointState メッセージを受け取り、デシリアライズします。

JointStateHandler.h

JointStateHandler.h

#ifndef JOINTSTATEHANDLER_H
#define JOINTSTATEHANDLER_H

#include <QObject>
#include <QVector>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/serialization.hpp>
#include <sensor_msgs/msg/joint_state.hpp>

class JointStateHandler : public QObject
{
    Q_OBJECT

    public:
        void Deserialize(QByteArray subscribedData);
        sensor_msgs::msg::JointState::SharedPtr SubscribedMessage();

    signals:
        void onDeserializedMessage() const;

    private:
        const int LINK_NUMBER = 6;
        sensor_msgs::msg::JointState subscribedMessage_;
        void deserializeJointState(QByteArray subscribedData);
};

#endif // JOINTSTATEHANDLER_H

JointStateHandler.cpp

#include "JointStateHandler.h"

void JointStateHandler::Deserialize(QByteArray subscribedData)
{
    deserializeJointState(subscribedData);
    emit onDeserializedMessage();
}

sensor_msgs::msg::JointState::SharedPtr JointStateHandler::SubscribedMessage()
{
    return std::make_shared<sensor_msgs::msg::JointState>(subscribedMessage_);
}

void JointStateHandler::deserializeJointState(QByteArray subscribedData)
{
    auto Deserialize = [](QByteArray serialized_message)
    {
        rclcpp::Serialization<sensor_msgs::msg::JointState> deserializer;
        sensor_msgs::msg::JointState message;

        const char* serialized_data = serialized_message.constData();
        size_t serialized_data_size = serialized_message.size();

        rcl_allocator_t allocator = rcl_get_default_allocator();

        rclcpp::SerializedMessage serialized_msg(serialized_data_size, allocator);

        memcpy(serialized_msg.get_rcl_serialized_message().buffer, serialized_data, serialized_data_size);
        serialized_msg.get_rcl_serialized_message().buffer_length = serialized_data_size;
        serialized_msg.get_rcl_serialized_message().buffer_capacity = serialized_data_size;

        deserializer.deserialize_message(&serialized_msg, &message);

        return message;
    };

    subscribedMessage_ = Deserialize(subscribedData);
}

【プログラム解説】

JointStateHandler::Deserialize 関数

void JointStateHandler::Subscribe(QByteArray subscribedData)
{
    deserializeJointState(subscribedData);
    emit onDeserializedMessage();
}

この関数は、Unity から受信したメッセージ（デシリアライズはされていない）を購読し、デシリアライズ用関数にメッセージを流します。

deserializeJointState 関数

JointStateHandler.cpp(抜粋)

void JointStateHandler::deserializeJointState(QByteArray subscribedData)
{
    auto Deserialize = [](QByteArray serialized_message)
    {
        rclcpp::Serialization<sensor_msgs::msg::JointState> deserializer;
        sensor_msgs::msg::JointState message;

        const char* serialized_data = serialized_message.constData();
        size_t serialized_data_size = serialized_message.size();

        rcl_allocator_t allocator = rcl_get_default_allocator();

        rclcpp::SerializedMessage serialized_msg(serialized_data_size, allocator);

        memcpy(serialized_msg.get_rcl_serialized_message().buffer, serialized_data, serialized_data_size);
        serialized_msg.get_rcl_serialized_message().buffer_length = serialized_data_size;
        serialized_msg.get_rcl_serialized_message().buffer_capacity = serialized_data_size;

        deserializer.deserialize_message(&serialized_msg, &message);

        return message;
    };

    subscribedMessage_ = Deserialize(subscribedData);
}

シリアライズされたメッセージをデシリアライズする関数です。

デシリアライズする際にも SerializationBase クラスを用います。メンバ関数として、deserialize_message() が定義されています。

deserialize_message() の詳細

deserialize_message() の引数・返り値・説明は次の通りです。

返り値　： void
第１引数： const SerializedMessage *serialized_message
第２引数： void *ros_message

Deserialize a serialized stream to a ROS message.
Parameters
[in] serialized_message The serialized message to be converted to ROS2 by rmw.
[out] ros_message The deserialized ROS2 message.

プログラム設計

ここは、実装をしていたときのメモ書きを、せっかくなので編集して掲載した部分になります。結論だけ知りたい方は飛ばして構いません。

SerializedMessage クラスのリファレンスをここに、
rcl_allocator_t 構造体のリファレンスをここに貼っておきます。

SerializedMessage クラスのコンストラクタ（の１つ）は次のように定義されています。

SerializedMessage (const rcl_allocator_t & allocator = rcl_get_default_allocator())

引数として要求されているのは、rcl_allocator_t 構造体のアドレスです。

Unity から受け取ったデータは QByteArray 型になっています。
なので、QByteArray を rcl_allocator_t に変換すれば勝ちというわけです。

===

アロケーターについてはよくわからないので調べてみます。
Microsoft によると、

アロケーターは、C++ 標準ライブラリでコンテナーに格納された要素の割り当ておよび解放を処理するために使用されます。 std::array を除くすべての C++ 標準ライブラリのコンテナーは、allocator<Type> 型のテンプレートパラメーターを持ちます。この Type はコンテナー要素の型を表しています。

結局よくわかりませんが、動的なメモリの確保・解放を行うために必要なものって感じでしょうか。なお、allocation = 割り当てです。

===

とはいえ、やることといったら、ChatGPT に聞くだけです。
「QByteArray を rcl_allocator_t に変換する方法」と聞いたらプログラムを書いてくれます。
良い時代になったものです。
あとは、ChatGPT の出力を参考にして、手直しします。

各クラス・構造体のリファレンスへのリンクを下記にまとめておきます。

QByteArray

rclcpp::Serialization

rclcpp::SerializationBase (上記Serializationクラスの基底クラス)

rclcpp::SerializedMessage

rcl_allocator_t

シリアライズデータの準備

const char* serialized_data = serialized_message.constData();
size_t serialized_data_size = serialized_message.size();

リファレンスにも書いてありますが、QByteArray.constData() は、バイト配列に格納されている const 型データへのポインタを返します。データそのものを返すわけではありません。

SerializedMessageの初期化とデータのコピー

memcpy(serialized_msg.get_rcl_serialized_message().buffer, serialized_data, serialized_data_size);
serialized_msg.get_rcl_serialized_message().buffer_length = serialized_data_size;
serialized_msg.get_rcl_serialized_message().buffer_capacity = serialized_data_size;

memcpy() を使用して、シリアライズされたデータを SerializedMessage のバッファにコピーします。また、コピーされたデータのサイズを設定します。

デシリアライズの実行

deserializer.deserialize_message(&serialized_msg, &message);

デシリアライズを実行します。

8. main.cpp（Qt）

インスタンスを生成したり、シグナル・スロット接続を行っています。

main.cpp

#include "mainwindow.h"
#include "UnityConnector.h"
#include "JointStateHandler.h"
#include "SerializedTrajectoryMaker.h"

#include <rclcpp/rclcpp.hpp>
#include <QtWidgets/QApplication>

int main(int argc, char *argv[])
{
    setvbuf(stdout, nullptr, _IONBF, BUFSIZ);
    rclcpp::init(argc, argv);

    std::shared_ptr<JointStateHandler> jointStateHandler = std::make_shared<JointStateHandler>();
    
    std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();

    std::thread executor_thread([executor](){executor->spin();});

    auto unityConnector = std::make_shared<UnityConnector>();

    QApplication a(argc, argv);

    mainwindow mainWindow;
    mainWindow.show();

    // ---- CONNECTIONS ----

    // ANGLES SETTING : MAIN WINDOW => JOINT STATE HANDLER / UNITY CONNECTOR
    QObject::connect(&mainWindow, &mainwindow::onAngleChanged,
                     [&mainWindow, &unityConnector]()
                     {
                         auto angles = mainWindow.JointAngles();

                         SerializedTrajectoryMaker::createSingleTrajectorySerializedMessage(angles, false);

                         auto msg = SerializedTrajectoryMaker::getSerializedMessage();

                         unityConnector->Publish(msg);
                     });

    // SUBSCRIBED DATA : UNITY CONTROLLER => JOINT STATE HANDLER
    QObject::connect(unityConnector.get(), &UnityConnector::onSubscribedDataFromUnity,
                     [&unityConnector, &jointStateHandler]()
                     {
                        jointStateHandler->Deserialize(unityConnector->SubscribedData());
                     });

    // DESERIALIZED MESSAGE : JOINT STATE HANDLER => MAIN WINDOW
    QObject::connect(jointStateHandler.get(), &JointStateHandler::onDeserializedMessage,
                     [&jointStateHandler, &mainWindow]()
                     {
                        auto subscribedData = jointStateHandler->SubscribedMessage();

                        // ここから下は仮

                        auto angles = subscribedData->position;

                        QString jointAnglesString_ = "Joint Angles :";

                        for(uint jointIndex = 0; jointIndex < 6; jointIndex++)
                        {
                            std::ostringstream oss;
                            oss << std::fixed << std::setprecision(1) << angles[jointIndex] * 180 / M_PI;

                            jointAnglesString_ += " " + oss.str() + " /";
                        }

                        mainWindow.UpdateStateLog(jointAnglesString_, "", angles, std::vector<double>(6));
                     });

    // ---- ESTABLISH TCP CONNECTION ----

    unityConnector->startListen();

    return a.exec();
}

9. 実行（Qt / Unity）

ビルドを行い、Qt で作成した実行可能ファイルを実行します。
ビルドが通らない場合は、add_executable にファイルを追加していることを確認してください。

その後、Unity も実行（再生）します。
Qt の mainwindow 上にある SpinBox の数値を変えると、ロボットモデルも一緒に動き出すと思います。

これでひとまず、Unity 上のロボットを自前実装の GUI で操作することができるようになりました。
この後は、moveit を乗っけて逆運動学計算とかしたいですね。