やりたいこと

以下の動画。Unity環境内に配置された移動ロボット（車体）を、ROS側のコントローラからの指令を受けて動かし、移動ロボットの姿勢情報とカメラ画像をUnity側からROS側に渡す。

Unity Robotics HUB の感じでROS2 と Unityを連携する pic.twitter.com/PXL8YApr0r
— moriitkys (@moriitkys) June 11, 2023

注意

更新予定です。以下のすべての記述は雑です（自分用なので）。休みにビール飲みながら検証したものですので。
特にコードはひどいです。
あとこれは以下のデdemoなどを一通り終えた人が対象です。基本的なROS-Unity通信設定ができていない場合はGitHubのインストラクションなどに従って設定してください。

構成

ROS2側 (Ubuntu 22.04 ROS humble)
・Joyスティックのコントローラからcmd_velをPublish。
・imageトピックを読み取って画像を表示。robot_poseトピックを受け取ってechoで表示。
Unity側 (Windows 11)
・移動ロボットがcmd_velコマンドを受け取って動作。
・ImagePublisherで移動ロボットに取り付けられたカメラと移動ロボットの姿勢情報をPublish。

Unity側のコード

以下のコードをScriptsフォルダーに作成し、移動ロボットのGameObjectにアタッチ。

RosPubSub.cs

using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;

/// <summary>
///
/// </summary>
public class RosPubSub : MonoBehaviour
{
    ROSConnection rosConnection;
    public string topicName = "robot_pose";

    // The game object
    public GameObject cube;
    // Publish the cube's position and rotation every N seconds
    public float publishMessageFrequency = 0.5f;

    private void MoveObject(TwistMsg cmdMsg)
    {
        //cube.transform.position += new Vector3((float)cmdMsg.linear.z, (float)cmdMsg.linear.y, (float)cmdMsg.linear.x);
        var r = new Vector3((float)cmdMsg.angular.y, -(float)cmdMsg.angular.z, (float)cmdMsg.angular.x);
        cube.transform.position += cube.transform.TransformDirection(new Vector3((float)cmdMsg.linear.z, (float)cmdMsg.linear.y, (float)cmdMsg.linear.x)) * Time.deltaTime;
        cube.transform.localEulerAngles += r;

        TwistMsg robotPose = new TwistMsg();
        robotPose.linear.x = cube.transform.position.x;
        robotPose.linear.y = cube.transform.position.y;
        robotPose.linear.z = cube.transform.position.z;
        robotPose.angular.x = cube.transform.localEulerAngles.x;
        robotPose.angular.y = cube.transform.localEulerAngles.y;
        robotPose.angular.z = cube.transform.localEulerAngles.z;

        rosConnection.Publish(topicName, robotPose);
    }

    void Start()
    {
        // start the ROS connection
        rosConnection = ROSConnection.GetOrCreateInstance();
        rosConnection.RegisterPublisher<TwistMsg>(topicName);


        rosConnection.Subscribe<TwistMsg>("cmd_vel", MoveObject);

    }

    private void Update()
    {

    }
}

以下のコードを移動ロボットのカメラオブジェクトにアタッチ。

ImagePublisherRos.cs


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

public class ImagePublisherRos : MonoBehaviour
{
    [SerializeField] private string topicName = "image";
    [SerializeField] private string frameId = "camera";

    private float timeElapsed = 0f;
    private float timeStamp = 0f;

    private ROSConnection rosConnection;
    private CompressedImageMsg imgMsg;

    private GameObject mainCamObj;
    private Camera cam;

    private Texture2D tex;
    private int targetWidth = 1104;
    private int targetHeight = 535;


    // Start is called before the first frame update
    void Start()

    {
        this.mainCamObj = GameObject.Find("Camera_01");
        this.cam = mainCamObj.GetComponent<Camera>();

        var render = new RenderTexture(this.targetWidth, this.targetHeight, 24);
        this.cam.targetTexture = render;
        this.cam.Render();
        RenderTexture.active = render;

        var d_width = this.cam.targetTexture.width;
        var d_height = this.cam.targetTexture.height;

        this.tex = new Texture2D(d_width, d_height, TextureFormat.RGB24, false);
        this.tex.ReadPixels(new Rect(0, 0, d_width, d_height), 0, 0);
        this.tex.Apply();

        // setup ROS
        this.rosConnection = ROSConnection.GetOrCreateInstance();
        this.topicName += "/compressed";
        this.rosConnection.RegisterPublisher<CompressedImageMsg>(this.topicName);

        // setup ROS Message
        this.imgMsg = new CompressedImageMsg();
        this.imgMsg.header.frame_id = this.frameId;
        this.imgMsg.format = "jpeg";

    }

    // Update is called once per frame
    void Update()
    {
        this.timeElapsed += Time.deltaTime;

        if (this.timeElapsed > (0.5f))
        {
            // Update ROS Message
            int sec = (int)Math.Truncate(this.timeStamp);

            uint nanosec = (uint)((this.timeStamp - sec) * 1e+9);

            var render = new RenderTexture(this.targetWidth, this.targetHeight, 24);
            this.cam.targetTexture = render;
            this.cam.Render();
            RenderTexture.active = render;
            var d_width = this.cam.targetTexture.width;
            var d_height = this.cam.targetTexture.height;
            this.tex = new Texture2D(d_width, d_height, TextureFormat.RGB24, false);
            this.tex.ReadPixels(new Rect(0, 0, d_width, d_height), 0, 0);
            this.tex.Apply();

            byte[] data = this.tex.EncodeToJPG();

            var compressedImageMsg = new CompressedImageMsg(this.imgMsg.header, this.imgMsg.format, data);

            this.rosConnection.Publish(this.topicName, compressedImageMsg);

            // Update time
            this.timeElapsed = 0;
            this.timeStamp = Time.time;
        }

    }
}

ImagePublisherRos.csのカメラ解像度を設定する部分がべた書きなので、これ改善必要あり。

位置姿勢を扱うところはごちゃごちゃしてるので注意。

以下のようにして複数カメラがある場合でも特定のカメラを区別して取得できるように名前を使う。
this.mainCamObj = GameObject.Find("Camera_01");

移動ロボットに取り付けられたカメラの設定は画像のように変更すること。

ROS2側

$ cd ~/ros2_ws/src
$ ros2 pkg create --build-type ament_python image_subscriber
$ touch ~/ros2_ws/src/image_subscriber/image_subscriber.py

image_subscriber.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge
import cv2
import numpy as np

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__("image_subscriber")
        #self.declare_parameter('image_topic_name', '/camera/color/image_raw')
        self.declare_parameter('image_topic_name', '/image/compressed')
        image_topic_name = self.get_parameter('image_topic_name').get_parameter_value().string_value

        video_qos = rclpy.qos.QoSProfile(depth=10)
        video_qos.reliability = rclpy.qos.QoSReliabilityPolicy.BEST_EFFORT

        self.sub_img = self.create_subscription(
            CompressedImage,
            image_topic_name,
            self.on_image_subscribed,
            video_qos
        )
        self.cnt = 0
        print("init image subscriber")
    
    def on_image_subscribed(self, img):
        print("subscribed" + str(self.cnt))
        self.cnt += 1
        np_arr = np.fromstring(img.data.tobytes(), np.uint8)
        input_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        output_image = self.process_image(input_image)
        cv2.imshow("Image", output_image)
        cv2.waitKey(1)
        
    def process_image(self, frame):
        frame = cv2.resize(frame, (640, 480))
        return frame


def main(args=None):
    try:
        rclpy.init(args=args)
        rclpy.spin(ImageSubscriber())
    
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()
        

if __name__ == "__main__":
    main()

package.xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>image_subscriber</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="robotics@todo.todo">robotics</maintainer>
  <license>TODO: License declaration</license>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
    <exec_depend>rclpy</exec_depend>
    <exec_depend>sensor_msgs</exec_depend>
  </export>
</package>

setup.py
entry_pointsに以下を追加。
"image_subscriber = image_subscriber.image_subscriber:main"

そして
colcon_build

実行

Joy Stick　コントローラの実行には以下を参考にする。

ros2 run joy joy_node
#別端末で
ros2 run joy_translate joy_translate_node

ROS-TCP-Endpointの実行

ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=xxx.xxx.xxx.xxx

移動ロボットの画像読み取り

ros2 run image_subscriber image_subscriber

ロボット姿勢

ros2 topic echo robot_pose

参考

ROS側　いろいろ組み合わせる
ROS 2 FoxyでRealSense D435の画像をsubscribeする(QoSの設定とPython、C++実装)
https://yhrscoding.hatenablog.com/entry/2021/09/26/104445
https://qiita.com/srs/items/3c3c22f1cbd41f90c5aa

なんか肝心なところが無い気がする
https://github.com/Field-Robotics-Japan/UnitySensors

Unity側　string変換などの変なことせずに素直にPublish
https://forum.unity.com/threads/publish-game-camera-stream-via-ros.1124503/
いろいろいらんことしてる
https://github.com/michaeljenkin/unityros/issues/3
肝心のところがない気がする
https://github.com/Field-Robotics-Japan/UnitySensorsROSArchive

Unity　カメラ切り替え
https://mogi0506.com/unity-camera-change/

Unity カメラズーム
https://detail.chiebukuro.yahoo.co.jp/qa/question_detail/q13112544023

Unity　オブジェクトの移動の基本
https://teratail.com/questions/206548

使ったモデル
https://www.turbosquid.com/ja/3d-models/3d-model-stylized-cartoon-car-free-free-1778654