はじめに

今回はROS2とUnityの連携の一環として慣性計測ユニット（IMU）からの情報をUnity上のシミュレータの物体の挙動に反映するという簡単なサンプルを紹介しようと思います。
この情報をうまく反映できれば、路面上の凹凸による縦揺れの挙動や、坂道での挙動を表現することができます。

なお、本記事で行っている内容は、Windows上でUnityとROS2を連携させる (1) ~環境構築編~および、Windows上でUnityとROS2を連携させる(2) ~簡易型シミュレータ作成編~の延長の内容になります。

また、使用している環境は以下の通りです。概ね、構築編で使用している環境と同じです。

PC

項目	バージョン/スペック
CPU	Core i7-9750H
GPU	NVIDIA GeForce RTX 2070
OS	Windows10
ROS2	Foxy Fitzroy
Unity	2020.3.18f1

Raspberry Pi 4

項目	バージョン/スペック
CPU	1.5GHz クアッドコア Cortex-A72(ARMv8、64bit L1=32KB、L2=1M)
メモリ	UD-RP4B8：8GB
OS	Ubuntu Desktop 20.04
ROS2	Foxy Fitzroy
TurtleBot3	Waffle Pi

Unity側でやること

オブジェクトの設定

配置

新しく作成したシーンに、フィールドとなるPlaneと実機のCubeを配置します。CubeオブジェクトはわかりやすいようにTB3と名前を変えておきます。

RigidBodyの設定

実機のオブジェクトにRigidBodyをつけます。単にcmd_velで動かすだけならこれでいいのですが、このあとのスクリプトからの操作の関係で、RigidBody内のConstrainsの設定を以下のようにしてください(FreezeRotationのXとZにチェック）。

![constraints.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2035642/98a1dc56-3d27-d1c8-e805-c544ddd5ebaf.png)

これで配置の設定は以上です。

スクリプトの記述

Create→C#Srciptで新しくスクリプトを作成し以下のコードを記述してください。

IMU.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Sensor;       
using RosMessageTypes.Geometry;

public class IMU : MonoBehaviour
{
    private Material material;
    public Rigidbody ROS_Object;

    [Range(0f, 100f)]
    public float Mass = 1.0f;

    Vector3 angularVel, linearAcc;

    // Start is called before the first frame update
    void Start()
    {
        ROSConnection.instance.Subscribe<ImuMsg>("/imu", imuVisualizer);
        ROSConnection.instance.Subscribe<TwistMsg>("/cmd_vel", turtlesim_move);
    
    }


    void imuVisualizer(ImuMsg Msg)
    {
        angularVel = new Vector3(
                                    (float)Msg.angular_velocity.x,
                                    (float)Msg.angular_velocity.y,
                                    (float)Msg.angular_velocity.z);

        linearAcc = new Vector3(
                                   (float)Msg.linear_acceleration.x,
                                   (float)Msg.linear_acceleration.y,
                                   (float)Msg.linear_acceleration.z);

        float F = Mass * (linearAcc.z - 9.8f);

        ROS_Object.AddForce(new Vector3(0f, -F, 0f));

        //Debug
        Debug.Log("angular velocity: " + angularVel);
        Debug.Log("linear acceleration: " + linearAcc);
        Debug.Log("force:" + F);
    }

    void turtlesim_move(TwistMsg Msg)
    {
        ROS_Object.velocity = transform.forward * (float)Msg.linear.x * 10;
        ROS_Object.angularVelocity = new Vector3(0, -(float)Msg.angular.z * 1, 0);

    }

}

IMUの情報を受け取って反映している部分についてかいつまんで説明します。

関数imuVisualizerの中身を見て下さい。この関数自体、ROSから/imuのトピックをサブスクライブしたときのコールバックになっています。

そもそも/imuから得られる情報は、三軸の加速度と三軸の角速度になっていて、この関数の中では、Vector3で受け取っています。

//角速度
angularVel = new Vector3((float)Msg.angular_velocity.x,
                         (float)Msg.angular_velocity.y,
                         (float)Msg.angular_velocity.z);

//加速度
linearAcc = new Vector3((float)Msg.linear_acceleration.x,
                        (float)Msg.linear_acceleration.y,
                        (float)Msg.linear_acceleration.z);

今回のサンプルで縦揺れを再現したいので、linearAcc.yを使います。ただし、拾ったデータのまま使用すると重力加速度が含まれているので、使う際にはその分を引きます。物体への揺れを反映するのには、RigidbodyのメソッドであるAddForceに力の成分として出力します。

//Force = mass * acceleration
 float F = Mass * (linearAcc.z - 9.8f);
//オブジェクトに力を伝える
 ROS_Object.AddForce(new Vector3(0f, -F, 0f));

パブリック変数として設けているMassは物体の重さの計数になります。各々の実機によって変更を加えてください。

このサンプルでは、Unityから実機へ動きをパブリッシュする機構はないので、TeleopKeyなどで動かしてみてUnityでの動きをぜひ確認してみてください。

以下デモ映像

リファレンス