UnityでNavigation2のゴールを送信するGUIを作る【後編】

はじめに

本記事では、UnityからNavigation2のゴールを送信するGUIを作る方法を紹介します。

この記事は前編と後編に分かれています。後編では、手順2~4の、initial poseの設定、ゴールの送信、ナビゲーションの実行について説明します。

前編はこちら↓

初期位置を送信する

ナビゲーションを開始する際には、マップ上のどの位置にロボットが存在するかどうかを、初期位置として設定する必要があります。

/initialposeトピックにPoseWithCovariance型のメッセージをパブリッシュすれば、初期位置が設定されます。以下は、それを行うunityのスクリプトです。今回は自動的にマップの初期位置を固定でパブリッシュするようにしています。

public void SendInitialPose() {
	PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg();
	HeaderMsg header = new HeaderMsg();

	header.stamp.sec = 0;
	header.stamp.nanosec = 0;
	header.frame_id = "map";

	pose.pose.position.x = 0.0;
	pose.pose.position.y = 0.0;
	pose.pose.position.z = 0.0;
	pose.pose.orientation.x = 0.0;
	pose.pose.orientation.y = 0.0;
	pose.pose.orientation.z = 0.0;
	pose.pose.orientation.w = 0.1;

	PoseWithCovarianceStampedMsg poseStampedMsg = new PoseWithCovarianceStampedMsg(header, pose); 
	_ros.Publish("/initialpose", poseStampedMsg);

	Debug.Log(pose.pose);
}

マウス座標をもとにゴールを設定する

以下のメソッドを使って、マウスカーソルから出たrayとマップを表示したplaneとの衝突を確認し、その座標をゴールの座標として用います。カメラのProjectionはOrthographicに設定します。

public Vector3 GetMousePosition() {
	Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);

	if (Physics.Raycast(ray, out hit))
	{
		var hitPos = hit.point;
		hitPos.y = 0f;
		return hitPos;
	}

	return new Vector3(0f, -100f, 0f);
}

こうして得られたマウスの座標を、マップの座標系に変換します。具体的には、ロボットの初期位置が(0,0,0)になるように、オフセットを加えて座標を調整します。_originPosはロボットの初期値を格納したVector3です。

_hitPos.x -= _mapReader._originPos.x;
_hitPos.y -= _mapReader._originPos.y;
_hitPos.z -= _mapReader._originPos.z;
// 向きを反転させる
_hitPos.x *= -1f;

位置を決めることはできました。あとは角度です。マップ上でマウスの左ボタンを押し込むと、その場所にマウスに合わせて回転する矢印が出現し、ボタンを離すとその瞬間の矢印の向きがゴールの角度として設定されるようにしました。

if (_isMouseButtonDown)
{
	var direction = _raycastHitPoint - _arrowObj.transform.position;
	direction.y = 0;
	_lookRotation = Quaternion.LookRotation(direction, Vector3.up);
	_arrowObj.transform.rotation = Quaternion.Lerp(_arrowObj.transform.rotation, _lookRotation, 0.1f);
}

_goalPosition = new Vector3(_hitPos.x, 0.2f, _hitPos.z);
_goalRotation = Quaternion.Lerp(_arrowObj.transform.rotation, _lookRotation, 0.1f).eulerAngles;

設定したゴールをパブリッシュする

navigation2のゴールはPoseStamped型で指定できます。したがって、unityでもPoseStampedMsgクラスのインスタンスを作り、それをパブリッシュします。以下のメソッドに、先ほど求めたゴールの位置と角度を引数として渡すと、メッセージをパブリッシュします。

public void SendGoal(Vector3 pos, Vector3 rot) {
	var goalPosition = pos;
	var goalRotation = rot;

	PoseMsg pose = new PoseMsg();
	HeaderMsg header = new HeaderMsg();

	// header
	header.stamp.sec = 0;
	header.stamp.nanosec = 0;
	header.frame_id = "map";

	// pose
	pose.position.x = goalPosition.z;
	pose.position.y = goalPosition.x;
	pose.position.z = 0;
	pose.orientation.x = 0;
	pose.orientation.y = 0;
	pose.orientation.z = goalRotation.y;
	pose.orientation.w = 0;

	PoseStampedMsg poseStampedMsg = new PoseStampedMsg(header, pose); 
	var topicName = _topicNames[_TBSelector.value];
	_ros.Publish(topicName, poseStampedMsg);

	Debug.Log(poseStampedMsg.pose);
}

受信したゴールをもとにナビゲーションを実行する

ここはROS2側の作業になります。適当にC++でパッケージを作ります。

以下のスクリプトは、unityからパブリッシュされるPoseStampedメッセージをサブスクライブし、それをNavigateToPoseアクションに投げるということを行なっています。

この実装は、以下の記事で紹介されていたものを参考にさせていただきました。

動作確認

UnityとROSで通信するために、ターミナルからros_tcp_endpointを起動しておきます。

$ ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=<your_IP_address>

gazeboを起動します。

ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

navigation2をbringupします。

ros2 launch nav2_bringup bringup_launch.py map:=<path_to_your_map>

作成したROS2ノードを起動します。

ros2 run action_tutorials_cpp nav2_send_goal

この状態でunityのシーンを実行してマップをクリックすると、navigation2のゴールが設定され、gazeboのロボットがそこに向かって自律走行します。（本記事では紹介していませんが、サブスクライブした/cmd_velの値を使ってunity内のロボットオブジェクトを動かす処理を行なっています）

おわりに

今後は、脱rviz計画を進めるべく、ロボットに搭載されたlidarデータの可視化や、ナビゲーションの経路の可視化などの機能を実装していきたいと考えています。