オドメトリを使ってROS2とUnityの間でロボットの位置を同期させる

はじめに

本記事では、ROS2で動くロボットの位置と角度(オドメトリ)をもとに、Unity側のロボットを同期して動かす方法について説明します。ROS2側でロボットを動かすと、オドメトリの値(自己位置)が変化し、それをUnityに送ることでUnity側のロボットを同期して動かします。

実行環境

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

• ubuntu 20.04.2 LTS
• ROS2 Foxy Fitzroy
• Unity 2020.3.17f1

このページにしたがって、UnityにはROS2との連携に必要なROS-TCP-Connectorをインストールしておきます。また、動作確認の手順については以下の記事でも紹介しています。

コード

オドメトリ(/odomトピック)をサブスクライブして、そこに含まれる位置と角度をもとにUnityのオブジェクトを移動させるコードです。Unityで移動を実現するには様々な方法がありますが、今回はtransformプロパティの値を書き換えることで移動させています。

nav_msgs/Odometry Messageの構造は、以下のページから確認できます。オドメトリのメッセージには、現在のロボットの世界座標系における位置と角度を示すPoseメッセージが含まれています。

また、最初に受信した姿勢をinitialPoseとして保存し、以後受信した姿勢はこれとの差分を取っているので、Unity側のロボットは常に原点からスタートします。

RobotController.cs

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

public class RobotController : MonoBehaviour
{
    [SerializeField] private GameObject robot;
    [SerializeField] private string topicName = "/odom";
    private ROSConnection ros;
    private Pose initialPose;
    private bool receivedFisrtMsg;

    void Start()
    {
        ros = ROSConnection.instance;
        ros.Subscribe<OdometryMsg>(topicName, ReceiveOdomMsg);
    }

    private void ReceiveOdomMsg(OdometryMsg msg)
    {
        Vector3 pos = msg.pose.pose.position.From<FLU>();
        Quaternion rot = msg.pose.pose.orientation.From<FLU>();

        if (!receivedFisrtMsg) {
            // 初期位置を保存
            initialPose.position = pos;
            initialPose.rotation = rot;
            receivedFisrtMsg = true;
        }

        // 原点をリセット
        Vector3 adjustedPos = pos - initialPose.position;
        Quaternion adjustedRot = rot * Quaternion.Inverse(initialPose.rotation);
        
        // ロボット用のオブジェクトにサブスクライブした位置と角度を設定
        robot.transform.position = adjustedPos;
        robot.transform.rotation = adjustedRot;
    }
}

動作確認

ターミナルからros_tcp_endpointとgazeboを起動しておきます。gazeboを起動すると、/odomトピックがパブリッシュされるようになります。

$ ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=<your IP address>

$ ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py
$ ros2 topic list
/camera/camera_info
/camera/image_raw
/clock
/cmd_vel
/imu
/joint_states
/odom
/parameter_events
/performance_metrics
/robot_description
/rosout
/scan
/tf
/tf_static

上のコードを適当なオブジェクトにアタッチして、Unityのシーンを実行します。/cmd_velを送ってgazeboのロボットを動かすと/odomトピックに含まれる姿勢が変化し、Unityのロボットもそれと同じように動きます。