RealSense T265の rs_t265.launch.pyのオドメトリ出力をros2-for-unity経由でGameObjectに伝えるだけのc#スクリプト

T265Realsense2Box.cs

using System;
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace ROS2
{

/// <summary>
/// An example class provided for testing of basic ROS2 communication
/// </summary>
[RequireComponent(typeof(ROS2UnityComponent))]

public class T265Realsense2Box : MonoBehaviour
{

    private ROS2UnityComponent ros2Unity;
    private ROS2Node ros2Node;
    private ISubscription<nav_msgs.msg.Odometry> nav_sub;

    public GameObject cube;

    private Vector3 position;
    private Quaternion rotation;

    void Start()
    {
        ros2Unity = GetComponent<ROS2UnityComponent>();
    }

    // Update is called once per frame
    void Update()
    {
        if (ros2Unity.Ok())
        {
            if (ros2Node == null)
            {
                ros2Node = ros2Unity.CreateNode("ROS2UnityTalkerNode");
                nav_sub = ros2Node.CreateSubscription<nav_msgs.msg.Odometry>("/camera/odom/sample", msg => {
                    //  get position
                    position = new Vector3((float)(msg.Pose.Pose.Position.X), (float)(msg.Pose.Pose.Position.Z), (float)(msg.Pose.Pose.Position.Y));
                    //  get rotation as quaternion
                    rotation = new Quaternion((float)(-1*msg.Pose.Pose.Orientation.X), (float)(-1*msg.Pose.Pose.Orientation.Z), (float)(-1*msg.Pose.Pose.Orientation.Y), (float)(msg.Pose.Pose.Orientation.W));
                });
            }
            cube.transform.position = position;
            cube.transform.rotation = rotation;
        }
    }
}
}  // namespace ROS2