UnityからRos-sharpを使って、Robovie-Zのメモリーマップに書き込む方法をご紹介します！
Unity＋ROS＋Robovie-Zシリーズ最終回は、
Robovie-Z SDKに実装されているROSサービスをつかって、
Robovie-Zのメモリマップを書き換えてみます。
前回までは、
１．ROS サービスモデルを作る
ROSメッセージモデルを作った時の同じ要領で、
ROSサービスモデルを作ります。
パッケージ名を確認して実行。
アセットフォルダに、サービスモジュールが作成されます。
SDKには”MemRead”と”MemWrite”の２つのサービスが定義されています。
両方ともモジュールを作っておきましょう。
２．コードを書く
スクリプトを新規作成し、
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にドラグドロップして追加。
３．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番に、１２３４を書き込みます。
Gemeシーンをクリックしてアクティブにして、
キーボードの”R”キーを押し、数値を読み込むと？
次に、キーボードの”W”キーを押して、”１２３４”を書き込み、
”R”キーを押して再度、値を読み込むと？
アドレス3872番の数値が0から1234に変わりました！
メモリーマップへの書き込み成功です！
4. 何ができる？
これでメモリーマップの読み書きができるようになりました！
ということは、Unityアプリからロボットの各関節を、
自由にコントロールできるようになったということです！！！
今回は残念ながら時間切れで、検証できませんでしたが、
Unity上で踊る3Dモデルの関節の動きを、Robovie-Zにコピーしてシンクロダンス！
RealSenseを使って、人の動きを読み込み、Robovie-Zを動かしたり。
他にもいろいろ面白そうなことができそうですね！
今回は、Unityを使ってアプリを書きましたが、
ROSとのやりとりさえできれば、どんな開発環境でも問題ありません。
でも、UIありのアプリ作るなら、Unityが一番楽なんだよね。
みなさんも是非、Robovie-Zと遊んでみてください！
