소개

이번에는 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
우분투 데스크톱 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로 새롭게 스크립트를 작성해 이하의 코드를 기술해 주세요.

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에서 얻은 정보는 3축의 가속도와 3축의 각속도가 되어 있고, 이 함수 중에서는 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에서의 움직임을 꼭 확인해보십시오.

아래 데모 영상

참조

• Windows에서 Unity와 ROS2를 연계시킨다 (1) ~환경 구축편~
• Windows에서 Unity와 ROS2를 연계시키는 (2) ~간이형 시뮬레이터 작성편~
• Unity Documentation Rigidbody.AddForce