Unity Robotics Hub 入門 (8) - ROS–Unity Integrationのサンプルコード

「Unity Robotics Hub」のROS–Unity Integrationのサンプルコードを確認してみます。

・Unity 2020.3

前回

Unity Robotics Hub 入門 (7) - Navigation 2 SLAM Example｜npaka｜note

1. パブリッシュ

トピック「pos_rot」でGameObjectの姿勢を0.5秒毎にパブリッシュするコードです。

(1) パブリッシャーの登録
(2) メッセージの生成
(3) パブリッシュ

・unity_scripts/RosPublisherExample.cs

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

// パブリッシュ
public class RosPublisherExample : MonoBehaviour
{
    ROSConnection ros; // ROSコネクション
    public string topicName = "pos_rot"; // トピック名

    public GameObject cube; // GameObject
    public float publishMessageFrequency = 0.5f; // パブリッシュレート
    private float timeElapsed; // 経過時間

    // スタート時に呼ばれる
    void Start()
    {
        // ROSコネクションの準備
        ros = ROSConnection.GetOrCreateInstance();

        // パブリッシャーの登録 (1)
        ros.RegisterPublisher<PosRotMsg>(topicName);
    }

    // スタート時に呼ばれる
    private void Update()
    {
        // 0.5秒毎
        timeElapsed += Time.deltaTime;
        if (timeElapsed > publishMessageFrequency)
        {
            cube.transform.rotation = Random.rotation;

            // メッセージの生成 (2)
            PosRotMsg cubePos = new PosRotMsg(
                cube.transform.position.x,
                cube.transform.position.y,
                cube.transform.position.z,
                cube.transform.rotation.x,
                cube.transform.rotation.y,
                cube.transform.rotation.z,
                cube.transform.rotation.w
            );

            // パブリッシュ (3)
            ros.Publish(topicName, cubePos);
            timeElapsed = 0;
        }
    }
}

2. サブスクライブ

トピック「color」をサブスクライブし、GameObjectの色を変更するコードです。

(1) サブスクライブ時のコールバックの登録。
(2) サブスクライブ時のコールバックの実装。

・unity_scripts/RosSubscriberExample.cs

using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.UnityRoboticsDemo.UnityColorMsg;

// サブスクライブ
public class RosSubscriberExample : MonoBehaviour
{
    ROSConnection ros; // ROSコネクション

    public GameObject cube; // GameObject

    // スタート時に呼ばれる
    void Start()
    {
        // ROSコネクションの準備
        ros = ROSConnection.GetOrCreateInstance();

        // サブスクライブ時のコールバックの登録 (1)
        ros.Subscribe<RosColor>("color", ColorChange);
    }

    // サブスクライブ時のコールバックの実装 (2)
    void ColorChange(RosColor colorMessage)
    {
        // GameObjectの色の変更
        cube.GetComponent<Renderer>().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a);
    }
}

3. サービス

 サービス「obj_pose_srv」でリクエストを受信しレスポンスを返すコードです。

(1) サービスのリクエスト受信時のコールバックの登録。
(2) サービスのリクエスト受信時のコールバックの実装。

・unity_scripts/RosUnityServiceExample.cs

using RosMessageTypes.UnityRoboticsDemo;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;

// サービス
public class RosUnityServiceExample : MonoBehaviour
{
    ROSConnection ros; // ROSコネクション
    string m_ServiceName = "obj_pose_srv"; // サービス名

    // スタート時に呼ばれる
    void Start()
    {
        // ROSコネクションの準備
        ros = ROSConnection.GetOrCreateInstance();

        // サービスのリクエスト受信時のコールバックの登録 (1)
        ros.ImplementService<ObjectPoseServiceRequest, ObjectPoseServiceResponse>(m_ServiceName, GetObjectPose);
    }

    // サービスのリクエスト受信時のコールバックの実装 (2)
    private ObjectPoseServiceResponse GetObjectPose(ObjectPoseServiceRequest request)
    {
        // レスポンスの生成
        ObjectPoseServiceResponse objectPoseResponse = new ObjectPoseServiceResponse();
        GameObject gameObject = GameObject.Find(request.object_name);
        if (gameObject)
        {
            objectPoseResponse.object_pose.position = gameObject.transform.position.To<FLU>();
            objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To<FLU>();
        }

        // レスポンスを返す
        return objectPoseResponse;
    }
}

4. サービス呼び出し

定期的にサービス「pos_srv」にリクエストし、行き先を取得して、GameObjectを移動させるコードです。

(1) サービスのレスポンス受信時のコールバックの登録。
(2) メッセージの生成。
(3) リクエストの送信。
(4) サービスのレスポンス受信時のコールバックの実装。

・unity_scripts/RosServiceCallExample.cs

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

// サービス呼び出し
public class RosServiceCallExample : MonoBehaviour
{
    ROSConnection ros; // ROSコネクション
    public string serviceName = "pos_srv"; // サービス名

    public GameObject cube; // GameObject
    public float delta = 1.0f; // 距離のしきい値
    public float speed = 2.0f; // 速度
    private Vector3 destination; // 行き先
    float awaitingResponseUntilTimestamp = -1; // 待機時間

    // スタート時に呼ばれる
    void Start()
    {
        // ROSコネクションの準備
        ros = ROSConnection.GetOrCreateInstance();

        // サービスのレスポンス受信時のコールバック (1)
        ros.RegisterRosService<PositionServiceRequest, PositionServiceResponse>(serviceName);

        // 行き先の準備
        destination = cube.transform.position;
    }

    // フレーム毎に呼ばれる
    private void Update()
    {
        // GameObjectの位置を行き先(destination)に近づける
        float step = speed * Time.deltaTime;
        cube.transform.position = Vector3.MoveTowards(cube.transform.position, destination, step);

        // 現在位置と行き先が近い時
        if (Vector3.Distance(cube.transform.position, destination) < delta && Time.time > awaitingResponseUntilTimestamp)
        {
            // メッセージの生成 (2)
            PosRotMsg cubePos = new PosRotMsg(
                cube.transform.position.x,
                cube.transform.position.y,
                cube.transform.position.z,
                cube.transform.rotation.x,
                cube.transform.rotation.y,
                cube.transform.rotation.z,
                cube.transform.rotation.w
            );

            // リクエストの送信 (3)
            PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos);
            ros.SendServiceMessage<PositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);

            // 待機時間の更新
            awaitingResponseUntilTimestamp = Time.time + 1.0f;
        }
    }

    // サービスのレスポンス受信時のコールバックの実装 (4)
    void Callback_Destination(PositionServiceResponse response)
    {
        // 行き先の更新
        awaitingResponseUntilTimestamp = -1;
        destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z);
    }
}

5. 参考

Unity-Robotics-Hub/README.md at main 揃 Unity-Technologies/Unity-Robotics-Hub

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門