「ROS2 For Unity」を用いたシミュレーション環境の構築について

B4の渡邉です。 今回は「ROS2 For Unity」を用いたシミュレーション環境の構築手順を紹介します。

想定環境としては以下の通りです。

・Ubuntu 22.04 ・Unity 2022.3.53f1 ・Unityに「ROS2 For Unity」と「ROS-TCP-Connector」を導入済 ・Unityにロボットモデルをインポート済

Unityに関しては2022であれば細かくは問われないはずです。

ROS2 For Unityの導入方法は以下のリンクを参照してください。（同研究室の松田くんの記事） リンク

ROS-TCP-Connectorの導入は「ROS2とPythonで作って学ぶAIロボット入門」を参考にして行いましたが、インターネットにも記載あると思うのでそちらからでもOK。

ロボットモデルに関してはどのモデルでも理論上動作可能なはずなので、先人方が作成しているモデルをダウンロードします。（筆者は同研究室の花田くんが作成したモデルを使用） もし、急発進・急停止したときに車体が浮き上がってしまうのが気になるようであれば、画像のように車体後方におもりを設定するといいです。

以上で準備が完了したのでそれぞれのスクリプトを作成していきます。

まずロボットを操作するためのコントローラースクリプトです。UnityのインスペクターウィンドウでROS2による操作か、キーボードによる操作（方向キー）かを選択できます。 以下のAGVController_ROS2ForUnity.csを作成します。

using UnityEngine;
using ROS2;

public class AGVController_ROS2ForUnity : MonoBehaviour
{
    public enum ControlMode { Keyboard, ROS };
    [Tooltip("キーボード操作とROSからの指令を切り替えます")]
    public ControlMode mode = ControlMode.Keyboard;

    [Header("Robot Components")]
    public GameObject wheel1; // 左車輪のゲームオブジェクト
    public GameObject wheel2; // 右車輪のゲームオブジェクト

    [Header("Movement Parameters")]
    public float maxLinearSpeed = 2f;    // m/s
    public float maxRotationalSpeed = 1f; // rad/s
    public float wheelRadius = 0.033f;   // m
    public float trackWidth = 0.288f;    // m

    [Header("Physics Settings")]
    public float forceLimit = 10f;
    public float damping = 10f;

    [Header("ROS Settings")]
    public float ROSTimeout = 0.5f;

    private ArticulationBody wA1;
    private ArticulationBody wA2;
    private float lastCmdReceivedTimestamp = 0f;

    private ROS2Node node;
    private ISubscription<geometry_msgs.msg.Twist> twistSubscriber;
    private volatile float rosLinear = 0f;
    private volatile float rosAngular = 0f;
    // --------------------------------

    void Start()
    {
        wA1 = wheel1.GetComponent<ArticulationBody>();
        wA2 = wheel2.GetComponent<ArticulationBody>();
        SetParameters(wA1);
        SetParameters(wA2);

        ROS2UnityComponent ros2UnityComponent = FindObjectOfType<ROS2UnityComponent>();
        node = ros2UnityComponent.CreateNode("agv_controller_ros2");
        twistSubscriber = node.CreateSubscription<geometry_msgs.msg.Twist>(
            "cmd_vel", ReceiveROSCmd);
    }

    void ReceiveROSCmd(geometry_msgs.msg.Twist cmdVel)
    {
        rosLinear = (float)cmdVel.Linear.X;
        rosAngular = (float)cmdVel.Angular.Z;
    }

    void FixedUpdate()
    {
        float currentLinear = rosLinear;
        float currentAngular = rosAngular;

        if (Mathf.Abs(currentLinear) > 0.001f || Mathf.Abs(currentAngular) > 0.001f)
        {
            lastCmdReceivedTimestamp = Time.time;
        }

        if (mode == ControlMode.Keyboard)
        {
            KeyBoardUpdate();
        }
        else if (mode == ControlMode.ROS)
        {
            ROSUpdate(currentLinear, currentAngular);
        }     
    }
    
    private void SetParameters(ArticulationBody joint)
    {
        if (joint == null) return;
        ArticulationDrive drive = joint.xDrive;
        drive.forceLimit = forceLimit;
        drive.damping = damping;
        joint.xDrive = drive;
    }

    private void SetSpeed(ArticulationBody joint, float wheelSpeed_deg_s)
    {
        if (joint == null) return;
        ArticulationDrive drive = joint.xDrive;
        drive.targetVelocity = wheelSpeed_deg_s;
        joint.xDrive = drive;
    }

    private void KeyBoardUpdate()
    {
        float moveDirection = Input.GetAxis("Vertical");
        float inputSpeed = moveDirection * maxLinearSpeed;
        float turnDirection = Input.GetAxis("Horizontal");
        float inputRotationSpeed = turnDirection * maxRotationalSpeed;
        RobotInput(inputSpeed, inputRotationSpeed);
    }
    
    private void ROSUpdate(float linear, float angular)
    {
        if (Time.time - lastCmdReceivedTimestamp > ROSTimeout)
        {
            linear = 0f;
            angular = 0f;
        }
        RobotInput(linear, -3.5f * angular);
    }
    
    private void RobotInput(float speed, float rotSpeed)
    {
        speed = Mathf.Clamp(speed, -maxLinearSpeed, maxLinearSpeed);
        rotSpeed = Mathf.Clamp(rotSpeed, -maxRotationalSpeed, maxRotationalSpeed);

        float wheel1Rotation = (speed / wheelRadius);
        float wheel2Rotation = wheel1Rotation;
        float wheelSpeedDiff = (rotSpeed * trackWidth) / wheelRadius;
        
        wheel1Rotation = (wheel1Rotation + wheelSpeedDiff) * Mathf.Rad2Deg;
        wheel2Rotation = (wheel2Rotation - wheelSpeedDiff) * Mathf.Rad2Deg;

        SetSpeed(wA1, wheel1Rotation);
        SetSpeed(wA2, wheel2Rotation);
    }
}

次に/clockをパブリッシュするスクリプトです。ここではROS-TCP-Connectorのライブラリより一部流用しています。このためにROS2 For UnityとROS-TCP-Connectorを共存させる必要がありました。（ROS2 For Unityだけで完結する方法もあるかと思いますが） 以下のRos2ClockPublisher.csを作成します。

using UnityEngine;
using ROS2;
using Unity.Robotics.Core;

public class Ros2ClockPublisher : MonoBehaviour
{
    [Tooltip("時刻メッセージを配信する頻度 (Hz)")]
    public float publishRateHz = 100f;
    
    private ROS2Node node;
    private IPublisher<rosgraph_msgs.msg.Clock> publisher;
    
    private rosgraph_msgs.msg.Clock clockMessage;
    private double lastPublishTimeSeconds;
    private double PublishPeriodSeconds => 1.0 / publishRateHz;

    private bool ShouldPublishMessage => Unity.Robotics.Core.Clock.Now > lastPublishTimeSeconds + PublishPeriodSeconds;

    void Start()
    {
        ROS2UnityComponent ros2UnityComponent = FindObjectOfType<ROS2UnityComponent>();
        node = ros2UnityComponent.CreateNode("unity_clock_publisher_ros2");
        publisher = node.CreatePublisher<rosgraph_msgs.msg.Clock>("/clock");
        
        clockMessage = new rosgraph_msgs.msg.Clock();
        clockMessage.Clock_ = new builtin_interfaces.msg.Time();
        
        lastPublishTimeSeconds = Unity.Robotics.Core.Clock.Now;
    }

    void Update()
    {
        if (ShouldPublishMessage)
        {
            PublishMessage();
        }
    }

    private void PublishMessage()
    {
        var publishTime = Unity.Robotics.Core.Clock.Now;
        var timestamp = new TimeStamp(publishTime);

        clockMessage.Clock_.Sec = timestamp.Seconds;
        clockMessage.Clock_.Nanosec = timestamp.NanoSeconds;
        
        publisher.Publish(clockMessage);
        
        lastPublishTimeSeconds = publishTime;
    }
}

次に/tfをパブリッシュするスクリプトです。 以下のRos2TransformTreePublisher.csを作成します。

using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using ROS2;
using Unity.Robotics.Core;

public static class RosGeometry
{
    public static geometry_msgs.msg.Transform ToRosTransform(this Transform unityTransform)
    {
        return new geometry_msgs.msg.Transform
        {
            Translation = unityTransform.position.ToRos(),
            Rotation = unityTransform.rotation.ToRos()
        };
    }
    
    public static geometry_msgs.msg.Vector3 ToRos(this Vector3 vector3)
    {
        return new geometry_msgs.msg.Vector3
        {
            X = vector3.z,
            Y = -vector3.x,
            Z = vector3.y
        };
    }

    public static geometry_msgs.msg.Quaternion ToRos(this Quaternion quaternion)
    {
        return new geometry_msgs.msg.Quaternion
        {
            X = -quaternion.z,
            Y = quaternion.x,
            Z = -quaternion.y,
            W = quaternion.w
        };
    }
    
    public static geometry_msgs.msg.Transform ToTransform(this geometry_msgs.msg.Vector3 translation, geometry_msgs.msg.Quaternion rotation)
    {
        return new geometry_msgs.msg.Transform
        {
            Translation = translation,
            Rotation = rotation
        };
    }
}

public class TransformTreeNode
{
    public Transform Transform { get; private set; }
    public string name => Transform.name;
    public TransformTreeNode Parent { get; private set; }
    public List<TransformTreeNode> Children { get; private set; }
    public bool IsALeafNode => Children.Count == 0;

    public TransformTreeNode(GameObject go, TransformTreeNode parent = null)
    {
        Transform = go.transform;
        Parent = parent;
        Children = new List<TransformTreeNode>();
        foreach (Transform child in go.transform)
        {
            if (child.GetComponent<ArticulationBody>() != null)
            {
                Children.Add(new TransformTreeNode(child.gameObject, this));
            }
        }
    }
    
    public static geometry_msgs.msg.TransformStamped ToTransformStamped(TransformTreeNode node)
    {
        var timestamp = new Unity.Robotics.Core.TimeStamp(Unity.Robotics.Core.Clock.Now);
        return new geometry_msgs.msg.TransformStamped
        {
            Header = new std_msgs.msg.Header
            {
                Frame_id = node.Parent.name,
                Stamp = new builtin_interfaces.msg.Time
                {
                    Sec = timestamp.Seconds,
                    Nanosec = timestamp.NanoSeconds
                }
            },
            Child_frame_id = node.name,
            Transform = node.Transform.localPosition.ToRos().ToTransform(node.Transform.localRotation.ToRos())
        };
    }
}

public class Ros2TransformTreePublisher : MonoBehaviour
{
    const string k_TfTopic = "/tf";

    [SerializeField]
    double m_PublishRateHz = 20f;
    [SerializeField]
    List<string> m_GlobalFrameIds = new List<string> { "odom" };
    [SerializeField]
    GameObject m_RootGameObject;
    
    double m_LastPublishTimeSeconds;

    TransformTreeNode m_TransformRoot;
    
    ROS2Node node;
    IPublisher<tf2_msgs.msg.TFMessage> publisher;

    double PublishPeriodSeconds => 1.0f / m_PublishRateHz;
    bool ShouldPublishMessage => Unity.Robotics.Core.Clock.Now > m_LastPublishTimeSeconds + PublishPeriodSeconds;

    void Start()
    {
        if (m_RootGameObject == null)
        {
            Debug.LogWarning($"No GameObject explicitly defined as {nameof(m_RootGameObject)}, so using this GameObject as root.");
            m_RootGameObject = gameObject;
        }

        ROS2UnityComponent ros2UnityComponent = FindObjectOfType<ROS2UnityComponent>();
        node = ros2UnityComponent.CreateNode("transform_tree_publisher_ros2");
        publisher = node.CreatePublisher<tf2_msgs.msg.TFMessage>(k_TfTopic);
        
        m_TransformRoot = new TransformTreeNode(m_RootGameObject);
        m_LastPublishTimeSeconds = Unity.Robotics.Core.Clock.Now;
    }

    void Update()
    {
        if (ShouldPublishMessage)
        {
            PublishMessage();
        }
    }

    void PublishMessage()
    {
        var tfMessageList = new List<geometry_msgs.msg.TransformStamped>();
        var timestamp = new Unity.Robotics.Core.TimeStamp(Unity.Robotics.Core.Clock.Now);

        if (m_GlobalFrameIds.Count > 0)
        {
            var tfRootToGlobal = new geometry_msgs.msg.TransformStamped
            {
                Header = new std_msgs.msg.Header
                {
                    Frame_id = m_GlobalFrameIds.Last(),
                    Stamp = new builtin_interfaces.msg.Time { Sec = timestamp.Seconds, Nanosec = timestamp.NanoSeconds }
                },
                Child_frame_id = m_TransformRoot.name,
                Transform = m_TransformRoot.Transform.gameObject.transform.ToRosTransform()
            };
            tfMessageList.Add(tfRootToGlobal);
        }
        else
        {
            Debug.LogWarning($"No GlobalFrameIds specified, transform tree will be entirely local coordinates.");
        }
        
        for (var i = 1; i < m_GlobalFrameIds.Count; ++i)
        {
            var tfGlobalToGlobal = new geometry_msgs.msg.TransformStamped
            {
                Header = new std_msgs.msg.Header
                {
                    Frame_id = m_GlobalFrameIds[i - 1],
                    Stamp = new builtin_interfaces.msg.Time { Sec = timestamp.Seconds, Nanosec = timestamp.NanoSeconds }
                },
                Child_frame_id = m_GlobalFrameIds[i],
                Transform = new geometry_msgs.msg.Transform()
            };
            tfMessageList.Add(tfGlobalToGlobal);
        }

        PopulateTFList(tfMessageList, m_TransformRoot);

        var tfMessage = new tf2_msgs.msg.TFMessage { Transforms = tfMessageList.ToArray() };
        publisher.Publish(tfMessage);
        
        m_LastPublishTimeSeconds = Unity.Robotics.Core.Clock.Now;
    }
    
    static void PopulateTFList(List<geometry_msgs.msg.TransformStamped> tfList, TransformTreeNode tfNode)
    {
        foreach (var childTf in tfNode.Children)
        {
            tfList.Add(TransformTreeNode.ToTransformStamped(childTf));
            if (!childTf.IsALeafNode)
            {
                PopulateTFList(tfList, childTf);
            }
        }
    }
}

次に/scanをパブリッシュするスクリプトです。 以下のRos2LaserScanPublisher_3.csを作成します。

using System.Collections.Generic;
using UnityEngine;
using ROS2;
using Unity.Robotics.Core;

public class Ros2LaserScanPublisher_3 : MonoBehaviour
{
    [Header("ROS2 Settings")]
    public string topic = "/scan";
    public string frameId = "base_scan";

    [Header("Scan Parameters")]
    public double publishPeriodSeconds = 0.1;
    public float rangeMetersMin = 0.1f;
    public float rangeMetersMax = 10f;
    public float scanAngleStartDegrees = 135f;
    public float scanAngleEndDegrees = -135f;
    public int numMeasurementsPerScan = 750;

    private ROS2Node node;
    private IPublisher<sensor_msgs.msg.LaserScan> publisher;
    private List<float> ranges;
    private double timeNextScanSeconds;

    void Start()
    {
        ROS2UnityComponent ros2UnityComponent = FindObjectOfType<ROS2UnityComponent>();
        node = ros2UnityComponent.CreateNode("laser_scan_publisher_final");
        publisher = node.CreatePublisher<sensor_msgs.msg.LaserScan>(topic);
        ranges = new List<float>(numMeasurementsPerScan);
        timeNextScanSeconds = Unity.Robotics.Core.Clock.Now;
    }

    void Update()
    {
        if (Unity.Robotics.Core.Clock.Now < timeNextScanSeconds)
        {
            return;
        }

        PerformScan();
        PublishScan();

        timeNextScanSeconds = Unity.Robotics.Core.Clock.Now + publishPeriodSeconds;
    }

    private void PerformScan()
    {
        ranges.Clear();
        var yawBaseDegrees = transform.rotation.eulerAngles.y;

        for (int i = 0; i < numMeasurementsPerScan; i++)
        {
            var t = (numMeasurementsPerScan > 1) ? (float)i / (numMeasurementsPerScan - 1) : 0f;
            
            var yawSensorDegrees = Mathf.Lerp(scanAngleStartDegrees, scanAngleEndDegrees, t);
            
            var yawDegrees = yawBaseDegrees - yawSensorDegrees;
            
            var directionVector = Quaternion.Euler(0f, yawDegrees, 0f) * Vector3.forward;
            var measurementStart = transform.position + (directionVector * rangeMetersMin);
            var measurementRay = new Ray(measurementStart, directionVector);

            if (Physics.Raycast(measurementRay, out var hit, rangeMetersMax))
            {
                ranges.Add(hit.distance);
            }
            else
            {
                ranges.Add(float.PositiveInfinity);
            }
        }
    }

    private void PublishScan()
    {
        var timestamp = new TimeStamp(Unity.Robotics.Core.Clock.Now);
        
        float angleStartRos = -scanAngleEndDegrees * Mathf.Deg2Rad;
        float angleEndRos = -scanAngleStartDegrees * Mathf.Deg2Rad;
        
        if (angleStartRos > angleEndRos)
        {
            var temp = angleEndRos;
            angleEndRos = angleStartRos;
            angleStartRos = temp;
            ranges.Reverse();
        }

        var msg = new sensor_msgs.msg.LaserScan
        {
            Header = new std_msgs.msg.Header
            {
                Frame_id = frameId,
                Stamp = new builtin_interfaces.msg.Time { Sec = timestamp.Seconds, Nanosec = timestamp.NanoSeconds }
            },
            Angle_min = angleStartRos,
            Angle_max = angleEndRos,
            Angle_increment = (numMeasurementsPerScan > 1) ? (angleEndRos - angleStartRos) / (numMeasurementsPerScan - 1) : 0,
            Time_increment = 0f,
            Scan_time = (float)publishPeriodSeconds,
            Range_min = rangeMetersMin,
            Range_max = rangeMetersMax,
            Ranges = ranges.ToArray(),
            Intensities = new float[ranges.Count]
        };

        publisher.Publish(msg);
    }
}

各スクリプトを作成できたら、それぞれゲームオブジェクトにアタッチします。アタッチする場所は画像を参照してください。

まずAGVController_ROS2ForUnity.csは以下の場所です。

次にRos2ClockPublisher.csとRos2TransformTreePublisher.csは以下の場所です。

次にRos2LaserScanPublisher_3.csは以下の場所です。

以上の作業を完遂した状態で、ターミナルからrviz2を起動させると画像のようにセンサーの情報が表示されるはずです。

これで基本のシミュレーション環境は構築できました。 これからROS2 For Unityを研究に使おうと思っている人はよかったら参考にしてみてください。 何か問題が生じたら可能な範囲で助けられるかもしれないのでSlackなどで連絡ください.