UnityからRos-sharpを使って、Robovie-Zのメモリーマップに書き込む!

UnityからRos-sharpを使って、Robovie-Zのメモリーマップに書き込む方法をご紹介します!

Unity+ROS+Robovie-Zシリーズ最終回は、
Robovie-Z SDKに実装されているROSサービスをつかって、
Robovie-Zのメモリマップを書き換えてみます。

 

前回までは、

Robovie-ZでSDKを使ってROSを動かしてみた
ヴイストン株式会社さんからRobovie-Zをお借りしたので、さっそくSDKを使ってROSを試してみます! Robovie-Zの商品ページはここ Robovie-Z(ロボビーゼット) | ヴイ...
Ros-sharpを使って、UnityとROSで通信してみる
Ros-sharpを使って、UnityとROSで通信する方法をご紹介します。 テスト環境: Robovie-Z with Pi4 running ROS Melodic Win1...
UnityからRos-sharpを使って、Robovie-Zを動かす! その①
今回は、UnityアプリからRobovie-Zを動かします。 前回までは、 1. Unityプロジェクトを用意 使用するUnityプロジェクトの、Edit→Proje...

 

1.ROS サービスモデルを作る

ROSメッセージモデルを作った時の同じ要領で、
ROSサービスモデルを作ります。

 

パッケージ名を確認して実行。

アセットフォルダに、サービスモジュールが作成されます。

SDKには”MemRead”と”MemWrite”の2つのサービスが定義されています。
両方ともモジュールを作っておきましょう。

 

2.コードを書く

スクリプトを新規作成し、

ROSとサービスのやり取りをするコードを書きます。

using RosSharp.RosBridgeClient;
using RosSharp.RosBridgeClient.MessageTypes.RoboviezRosMsgs;
using RosSharp.RosBridgeClient.MessageTypes.Std;
using UnityEngine;
using System;
using System.Threading;
using System.Collections.Generic;

public class MemReadWrite : MonoBehaviour
{
    public int memAddress;
    public int dataToSend;

    private RosConnector rosConnector;
    private readonly int SecondsTimeout = 3;
    private MemReadRequest readRequest;
    private MemWriteRequest writeRequest;

    void Start()
    {
        rosConnector = GetComponent<RosConnector>();

        if (!rosConnector.IsConnected.WaitOne(SecondsTimeout * 1000))
            Debug.LogWarning("Failed to subscribe: RosConnector not connected");
    }

    void Update()
    {

        if (Input.GetKeyDown(KeyCode.R))            //press S key every time you want to send messages to ROS
        {
            readRequest = new MemReadRequest((short)memAddress, 2);
            rosConnector.RosSocket.CallService<MemReadRequest, MemReadResponse>("/roboviez_ros_controller/memmap_read", ReadServiceCallHandler, readRequest);
        }

        if (Input.GetKeyDown(KeyCode.W))            //press S key every time you want to send messages to ROS
        {
            Write(memAddress, dataToSend);
        }


    }

    public void Write(int addresss, int value)
    {
        MultiArrayLayout layout = new MultiArrayLayout();
        layout.data_offset = 0;
        layout.dim = new MultiArrayDimension[1];
        layout.dim[0] = new MultiArrayDimension("x", 2, 1);

        byte[] data = new byte[] { (byte)(value & 255), (byte)(value >> 8) };

        writeRequest = new MemWriteRequest((short)addresss, new UInt8MultiArray(layout, data));
        rosConnector.RosSocket.CallService<MemWriteRequest, MemWriteResponse>("/roboviez_ros_controller/memmap_write", WriteServiceCallHandler, writeRequest);
    }

    private static void ReadServiceCallHandler(MemReadResponse response)
    {

        byte[] values = new byte[2];
        response.Buf.CopyTo(values, 0);

        try
        {
            int value = BitConverter.ToInt16(values, 0);
            Debug.Log("Value Received:  " + value.ToString());
        }
        catch (Exception ex)
        {
            Debug.Log(ex.ToString());
        }

        //Debug.Log(BitConverter.ToString(response.Buf));
    }

    private static void WriteServiceCallHandler(MemWriteResponse response)
    {
        //Debug.Log("Service requested to ROS");
    }
}

できたスクリプトは、忘れずにGameObjectにドラグドロップして追加

 

3.Unityでアプリを実行

CPUがONになっていることを確認して、

ターミナルを開いて、ROSにログインし、RosBridgeServerを起動。

roslaunch rosbridge_server rosbridge_websocket.launch

別ターミナルから、roboviez_ros_controllerを起動。

rosrun roboviez_ros_samples roboviez_ros_controller.py

Unityでアプリを実行。

GameObjectを選択してInspectorウィンドウから、

書き込むアドレスと数値を入力。

今回は、SDKマニュアルの例と同じように、
アドレス3872番に、1234を書き込みます。

Gemeシーンをクリックしてアクティブにして、
キーボードの”R”キーを押し、数値を読み込むと?

次に、キーボードの”W”キーを押して、”1234”を書き込み、
”R”キーを押して再度、値を読み込むと?

アドレス3872番の数値が0から1234に変わりました!

メモリーマップへの書き込み成功です!

 

4. 何ができる?

これでメモリーマップの読み書きができるようになりました!

ということは、Unityアプリからロボットの各関節を、
自由にコントロールできるようになったということです!!!

今回は残念ながら時間切れで、検証できませんでしたが、
Unity上で踊る3Dモデルの関節の動きを、Robovie-Zにコピーしてシンクロダンス!

RealSenseを使って、人の動きを読み込み、Robovie-Zを動かしたり。

他にもいろいろ面白そうなことができそうですね!

今回は、Unityを使ってアプリを書きましたが、
ROSとのやりとりさえできれば、どんな開発環境でも問題ありません。

でも、UIありのアプリ作るなら、Unityが一番楽なんだよね。

 

みなさんも是非、Robovie-Zと遊んでみてください!

タイトルとURLをコピーしました