はじめに

本記事では、UnityでNavigation2のゴールを送信するGUIを作る方法を紹介します。最終的には、以下のようにunityのシーンに表示されたマップをマウスでクリックすると、その座標にgazeboのロボットが移動するようになります。

現状、navigation2にゴールを送信する主な方法としては、以下の方法があります。

ターミナルやスクリプトから直接トピックを送信する
rvizのGUIを介して送信する

前者はGUIがないためマップを見ながらゴールを与えることができません。したがって、より自由なナビゲーションを実行するためには、後者のrvizが望まれます。一方で、rvizの動作は不安定なことが多く、筆者の環境では10分に1回くらいの頻度でフリーズするか、挙動がおかしくなります。また、VR/ARとROSを一緒に使う場合などは、unityからゴールを設定できたほうが良いでしょう。

そこで、今回はUnityからNavigation2のゴールを送信するためのGUIを作成したので、本記事ではその方法を紹介します。実装は以下の手順で行いました。

マップを読み込めるようにする
initial poseを送信できるようにする
ゴールを送信できるようにする
ゴールを受信したらナビゲーションを実行するようにする

この記事は前編と後編に分かれています。前編では、手順1のマップ読み込み機能の実装について説明します。

後編はこちら↓

また、本記事で用いているコードの全文は、以下のリポジトリで公開しています。

実行環境

本記事で用いる実行環境は以下の通りです。

Ubuntu 20.04.2 LTS
ROS2 Foxy Fitzroy
Unity 2020.3.17f1
YamlDotNet for Unity 11.2.1
- https://assetstore.unity.com/packages/tools/integration/yamldotnet-for-unity-36292?locale=ja-JP

マップを読み込む

これはナビゲーション実行中のrvizの画面です。ナビゲーションは、事前に作成したマップのデータとリアルタイムで得られたマップのデータをマッチングしながら行うわけですが、まずはunityでもこのマップを表示したいと思いました。

navigation2で使うマップのデータはpgmとyamlの2種類のファイル形式で保存されています。pgmファイルはグレースケールで表された占有格子地図の画像を、yamlファイルは解像度や原点の座標などのパラメータを保持しています。今回はgazeboとrvizを使って、turtlebot3_gazeboパッケージに含まれるturtlebot3_worldのマップを作成しました。

また、unityのほうでは、MeshRendererにマップのテクスチャを割り当てるplaneと、cube等を使って適当にロボットのオブジェクトを配置しておきます。

画像ファイルの読み込み

マップ画像をunityのシーンにテクスチャとして表示します。早速ですが、unityはpgmファイルを画像として扱ってくれません。したがって、imagemagickを使ってファイル形式をunityが扱える画像形式（png）に変更します。なお、ファイル名を修正する要領で拡張子を.pgmから.pngに変更するだけだと、画像の幅や高さなどの内部パラメータがバグってしまいます。

$ convert map.pgm map.png

その後、unityのランタイムでどこかに格納したpngファイルをテクスチャとして読み込みます。ローカルに保存したpngファイルを読み込み、それと同じサイズのTexture2Dを生成する方法は、以下の記事を参考にしました。

var texture = TextureReader.GetTextureFromPngFile(path);
meshRenderer.material.mainTexture = texture;

座標を合わせる

さて、マップ画像がシーンに表示できましたが、これだけだと以下のような問題があります。

マップのサイズが合っていない（そのままだと正方形になっている）
ロボットの初期位置をどこにするべきかわからない

そこで、yamlファイルに保存されたパラメータを使ってこれらを修正します。

yamlファイルの読み込み

pgmと同様に、yamlファイルもunityのデフォルトの機能では読むことができません。そこで、YamlDotNet for Unityというアセットを使います。

マップのyamlファイルは以下のような構成になっています。

image: map.pgm
mode: trinary
resolution: 0.05
origin: [-1.25, -2.4, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

imageはpgmファイルの相対パス、resolutionは解像度、originは原点座標、negateは白黒反転させるかどうか、occupied_threshは障害物とみなす閾値、free_thresはフリースペースとみなす閾値です。なお、このoriginとは、マップの左下の座標です。マップ座標系の原点は、マップの生成を開始した座標、つまりロボットの初期位置です。つまり、originの値はロボットの初期位置と左下の原点の間の距離を示しています。

これと同じ構成のクラスを作り、それを使ってデシリアライズします。

public class DeserializedObject
{
	public string image;
	public string mode;
	public float resolution;
	public float[] origin;
	public int negate;
	public float occupied_thresh;
	public float free_thresh;
}

public static DeserializedObject Deserialize(string yamlName)
{
	StreamReader sr = new StreamReader(yamlName);
	string text = sr.ReadToEnd();
	var input = new StringReader(text);
	var deserializer = new DeserializerBuilder()
								.WithNamingConvention(UnderscoredNamingConvention.Instance)
								.Build();

	DeserializedObject deserializeObject = deserializer.Deserialize<DeserializedObject>(input);
	return deserializeObject;
}

実際に読み込む際は、以下のメソッドを叩きます。

var yaml = YamlReader.Deserialize(_yamlPath);

読み込んだパラメータをもとに座標を調整

それでは、先ほど読み込んだyamlファイルのパラメータをもとに、マップとロボットの位置合わせをします。

まずは、マップを表示しているplaneのスケールを、画像ファイルのスケールに合わせます。

// ここで0.1をかけているのは、planeが他のcube等よりも10倍大きいためです
var scaleX = yaml.resolution * texture.width * 0.1f;
var scaleZ = yaml.resolution * texture.height * 0.1f;
_plane.transform.localScale = new Vector3(scaleX, 1f, scaleZ);

次に、unityにおけるロボットの原点座標を計算し、ロボットをそこに移動させます。
-texture.height/2f*yaml.resolutionと-texture.width/2f*yaml.resolutionで、unityにおけるマップの左下の座標を求めます。textureの幅と高さの単位はピクセルになっているので、yamlファイルのresolution[pixel/m]をかけて、メートルに直します。その後、originの値をオフセットとして加えることで、unityにおけるロボットの初期座標を求めることができます。

_originPos = new Vector3(
	(-texture.height/2f*yaml.resolution)+(yaml.origin[1]), 
	0f, 
	((-texture.width/2f*yaml.resolution)+(yaml.origin[0]))*-1f
);

動作確認

ここまでの流れを一通り実装してunityのシーンを実行すると、最終的には以下のような画面が作れると思います。ロボットとマップのスケールが合致しており、初期位置も正しく設定されています。使用するマップによって、planeのサイズやロボットの位置は異なります。