ROS2のIMUの情報をもとにUnityのオブジェクトを縦揺れさせる
はじめに
今回は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にチェック)。
これで配置の設定は以上です。
スクリプトの記述
Create→C#Srciptで新しくスクリプトを作成し以下のコードを記述してください。
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での動きをぜひ確認してみてください。
以下デモ映像
リファレンス
Author And Source
この問題について(ROS2のIMUの情報をもとにUnityのオブジェクトを縦揺れさせる), 我々は、より多くの情報をここで見つけました https://qiita.com/sfc_nakanishi_lab/items/d8ce70157003a1721c58著者帰属:元の著者の情報は、元のURLに含まれています。著作権は原作者に属する。
Content is automatically searched and collected through network algorithms . If there is a violation . Please contact us . We will adjust (correct author information ,or delete content ) as soon as possible .