はじめに

以下の記事では、rvizを用いてマップ上の点を指定することで、ロボットがそこに向かって動くナビゲーション機能の実装について紹介しました。

本記事では、同様のことをunityからゴールを送信することで行う方法を紹介します。
以下の動画では、unity内のバーチャルオブジェクトを動かすと、それに追従するようにリアルワールドのロボットも動いています。behavior treeを使ってゴールの少し後ろを追従するように制御しています。
これを応用すれば、例えばVRHMDを装着した人や、VR空間内の特定のバーチャルオブジェクトを持っている人を追いかけるロボットなどを作ることができます。

実行環境

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

Windows 10
ROS2 Foxy Fitzroy
Unity 2020.3.17f1
Turtlebot3 waffle pi
- gazebo等のシミュレーション環境でも問題なく動作します

実装

ロボットとマップを作る

Unity内で動かすロボットを適当にCubeか何かで作ります。向きがわかりやすいように、上面に矢印を貼り付けています。ロボットにはTurtlesimという名前をつけておきます。

また、ナビゲーションで使うマップを持ち込んでおきます。今回はリアルワールドでロボットをナビゲーションするので、事前に自分の部屋のマップを作っておきます。マップはpgm形式で保存されているので、imagemagickを使ってファイル形式をunityが扱える画像形式に変更します。その後、インポートした画像をImageコンポーネントにアタッチし、サイズをいい感じに調整します。

$ convert map.pgm map.png

ゴールを送信するスクリプト

適当なオブジェクトに以下のスクリプトをアタッチします。turtlesimフィールドには先ほど作ったロボットのオブジェクトを指定しておきます。

navigation2をbringupすると、PoseStamped型の/goal_poseトピックのサブスクライブが開始されます。これはその名の通り、ナビゲーションのゴールを示すトピックです。したがって、unityから/goal_poseトピックをパブリッシュすれば、そのタイミングで目的地に向かってロボットが動き出します。今回は、ゴールとしてunity内のバーチャルオブジェクトの座標を用いており、一定周期でturtlesimオブジェクトの座標を入れたトピックをパブリッシュしています。

GoalPublisher.cs

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

public class GoalPublisher : MonoBehaviour
{
    [SerializeField] string topicName = "/goal_pose";
    [SerializeField] GameObject turtlesim;
    ROSConnection ros;
    float timeElapsed;
    float publishMessageFrequency = 5f;
    PoseStampedMsg pose = new PoseStampedMsg();
    
    void Start()
    {
        ros = ROSConnection.GetOrCreateInstance();
        ros.RegisterPublisher<PoseStampedMsg>(topicName); 
    }

    void Update()
    {
        timeElapsed += Time.deltaTime;

        // 一定周期で中の処理を実行する
        if (timeElapsed > publishMessageFrequency)
        {
            SendPoseMsg(turtlesim.transform.position, turtlesim.transform.rotation);
            timeElapsed = 0;
        }
    }

    void SendPoseMsg(Vector3 linearVector, Quaternion angularVector) {
        var msg = new PoseStampedMsg();
        
        msg.pose.position.x = linearVector.z;
        msg.pose.position.y = -linearVector.x;
        msg.pose.position.z = 0f;

        msg.pose.orientation.x = angularVector.x;
        msg.pose.orientation.y = angularVector.y;
        msg.pose.orientation.z = angularVector.z;
        msg.pose.orientation.w = angularVector.w;

        ros.Publish(topicName, msg);
    }
}

動作確認

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

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

Turtlebot3を起動します。ここはgazeboでも問題ありません。

$ ros2 launch turtlebot3_bringup robot.launch.py

navigation2をbringupします。

$ ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True map:=/opt/ros/foxy/share/nav2_bringup/maps/turtlebot3_world.yaml default_bt_xml_filename:=<path>

この状態でunityのシーンを実行して、turtesimオブジェクトを動かすと、その少し後ろをリアルワールドのTurtlebot3が追いかけるように移動します。

Navigation2を使ってUnityのバーチャルオブジェクトにロボットを追従させる

はじめに

実行環境

実装

ロボットとマップを作る

ゴールを送信するスクリプト

動作確認