Bridging Gazebo/ROS 2 with Unity for Enhanced Simulation

Overview

This lesson covers techniques to connect Gazebo/ROS 2 simulations with Unity, enabling a hybrid approach that combines realistic physics with high-fidelity rendering and advanced human-robot interface capabilities. Learn to synchronize data between systems and create enhanced simulation experiences.

Learning Objectives

By the end of this lesson, you should be able to:

• Understand the benefits and challenges of bridging Gazebo and Unity
• Set up communication between Gazebo/ROS 2 and Unity
• Synchronize robot states and sensor data between systems
• Implement data transformation for coordinate system compatibility
• Create a hybrid simulation pipeline
• Optimize performance when bridging multiple systems

Why Bridge Gazebo and Unity?

Complementary Strengths

Gazebo and Unity offer different advantages:

• Gazebo: Accurate physics simulation, robust sensor modeling, mature ROS/ROS 2 integration
• Unity: High-fidelity rendering, advanced UI/UX capabilities, powerful visualization tools

Hybrid Simulation Benefits

• Leverage Gazebo's physics accuracy with Unity's visual quality
• Enable advanced human-robot interaction through Unity's UI system
• Use Unity for visualization while relying on Gazebo for physics calculations
• Facilitate mixed reality applications combining simulation with AR/VR

Architecture of Gazebo-Unity Bridge

High-Level Architecture

ROS 2 Network
�����������������
   Gazebo         �� Physics & Sensor Simulation
   (Server)      
�����������������
        
        �
�����������������
   Bridge Node    �� Data Transformation & Synchronization
   (Middleware)  
�����������������
        
        �
�����������������
   Unity          �� Visualization & Interaction
   (Client)      
�����������������

Communication Protocols

Several approaches exist for connecting these systems:

1. Direct ROS/ROS 2 Bridge: Using ROS# or similar packages
2. Custom TCP/IP Protocol: For optimized communication
3. Shared Memory: For high-performance applications
4. Message Queues: For reliable communication with buffering

Setting Up the Bridge

Option 1: ROS# Bridge (Recommended for beginners)

ROS# is a popular Unity package for ROS communication.

Installation Steps:

1. Download and import the ROS# Unity package
2. Set up a ROS master (or use ROS bridge server)
3. Configure network settings for communication

Basic Bridge Node Example:

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;

public class GazeboUnityBridge : MonoBehaviour
{
    [Header("ROS Connection")]
    public string rosIP = "127.0.0.1";
    public int rosPort = 10000;

    [Header("Topics Configuration")]
    public string robotStateTopic = "/robot_state";
    public string jointStatesTopic = "/joint_states";
    public string tfTopic = "/tf";
    public string sensorDataTopic = "/sensor_data";

    [Header("Robot Configuration")]
    public GameObject robotModel;
    public Transform[] robotJoints; // Match order with ROS joint names

    private ROSConnection ros;
    private Dictionary<string, Transform> jointMap = new Dictionary<string, Transform>();
    private Dictionary<string, float> jointPositions = new Dictionary<string, float>();

    void Start()
    {
        // Initialize ROS connection
        ros = ROSConnection.GetOrCreateInstance();
        ros.Initialize(rosIP, rosPort);

        // Subscribe to topics
        ros.Subscribe<JointStateMsg>(jointStatesTopic, OnJointStatesReceived);
        ros.Subscribe<TFMessage>(tfTopic, OnTFReceived);

        // Map joint names to transforms
        MapJoints();
    }

    void MapJoints()
    {
        // Assuming joint names in ROS match GameObject names
        foreach (Transform joint in robotJoints)
        {
            jointMap[joint.name] = joint;
        }
    }

    void OnJointStatesReceived(JointStateMsg jointState)
    {
        // Update joint positions from ROS message
        for (int i = 0; i < jointState.name.Count; i++)
        {
            string jointName = jointState.name[i];
            float position = jointState.position[i];

            if (jointMap.ContainsKey(jointName))
            {
                jointPositions[jointName] = position;
                UpdateJoint(jointMap[jointName], position);
            }
        }
    }

    void OnTFReceived(TFMessage tfMsg)
    {
        // Update robot pose based on TF transforms
        foreach (var transform in tfMsg.transforms)
        {
            if (transform.child_frame_id == "base_link") // Adjust to your robot's base frame
            {
                // Convert ROS transform to Unity coordinates
                Vector3 position = RosToUnityPosition(transform.transform.translation);
                Quaternion rotation = RosToUnityRotation(transform.transform.rotation);

                robotModel.transform.position = position;
                robotModel.transform.rotation = rotation;
            }
        }
    }

    void UpdateJoint(Transform joint, float position)
    {
        // Apply joint position to Unity transform
        // This is a simplified example - actual implementation depends on joint type
        joint.localEulerAngles = new Vector3(position * Mathf.Rad2Deg, 0, 0);
    }

    Vector3 RosToUnityPosition(geometry_msgs.Vector3 rosPos)
    {
        // ROS uses right-handed coordinate system (X-forward, Y-left, Z-up)
        // Unity uses left-handed coordinate system (X-right, Y-up, Z-forward)
        return new Vector3(rosPos.y, rosPos.z, rosPos.x);
    }

    Quaternion RosToUnityRotation(geometry_msgs.Quaternion rosQuat)
    {
        // Convert ROS quaternion to Unity quaternion
        // Account for coordinate system differences
        return new Quaternion(rosQuat.y, rosQuat.z, rosQuat.x, rosQuat.w);
    }

    void SendRobotCommands()
    {
        // Example: Send commands back to Gazebo
        var jointCmd = new JointStateMsg();
        jointCmd.name = new List<string>(jointMap.Keys);
        jointCmd.position = new List<float>();

        foreach (string jointName in jointMap.Keys)
        {
            jointCmd.position.Add(jointPositions[jointName]);
        }

        ros.Publish("/desired_joint_positions", jointCmd);
    }

    void Update()
    {
        // Periodically send commands if needed
        if (Time.frameCount % 60 == 0) // Every 60 frames (approx. 1 Hz if 60 FPS)
        {
            SendRobotCommands();
        }
    }
}

Option 2: Custom TCP/IP Bridge

For more control and performance optimization:

using System;
using System.Collections;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
using System.Text;
using UnityEngine;

public class CustomTCPPBridge : MonoBehaviour
{
    [Header("TCP Connection")]
    public string serverIP = "127.0.0.1";
    public int serverPort = 8080;

    [Header("Robot Configuration")]
    public GameObject robotModel;
    public Transform[] robotJoints;

    private TcpClient tcpClient;
    private NetworkStream stream;
    private bool isConnected = false;

    void Start()
    {
        StartCoroutine(ConnectToServer());
    }

    IEnumerator ConnectToServer()
    {
        yield return new WaitForSeconds(1); // Allow other systems to initialize

        try
        {
            tcpClient = new TcpClient();
            tcpClient.Connect(serverIP, serverPort);
            stream = tcpClient.GetStream();
            isConnected = true;

            Debug.Log("Connected to Gazebo bridge server");

            // Start receiving data
            StartCoroutine(ReceiveData());
        }
        catch (Exception e)
        {
            Debug.LogError("Failed to connect to bridge server: " + e.Message);
        }
    }

    IEnumerator ReceiveData()
    {
        byte[] buffer = new byte[1024];

        while (isConnected)
        {
            try
            {
                if (stream.DataAvailable)
                {
                    int bytesRead = stream.Read(buffer, 0, buffer.Length);
                    string receivedData = Encoding.UTF8.GetString(buffer, 0, bytesRead);

                    // Parse and handle received data
                    HandleReceivedData(receivedData);
                }

                yield return new WaitForSeconds(0.01f); // 100 Hz update rate
            }
            catch (Exception e)
            {
                Debug.LogError("Error receiving data: " + e.Message);
                isConnected = false;
                break;
            }
        }
    }

    void HandleReceivedData(string data)
    {
        // Example data format: "JOINT:joint1_name:1.23,JOINT:joint2_name:-0.45,POSE:0.1,0.2,0.3,0.0,0.0,0.0"
        string[] entries = data.Split(',');

        foreach (string entry in entries)
        {
            if (entry.StartsWith("JOINT:"))
            {
                // Parse joint data: JOINT:joint_name:value
                string[] parts = entry.Substring(6).Split(':');
                if (parts.Length == 2)
                {
                    string jointName = parts[0];
                    float position = float.Parse(parts[1]);

                    UpdateJointByName(jointName, position);
                }
            }
            else if (entry.StartsWith("POSE:"))
            {
                // Parse pose data: POSE:pos_x,pos_y,pos_z,rot_x,rot_y,rot_z
                string[] values = entry.Substring(5).Split(':');
                if (values.Length == 2)
                {
                    string[] poseValues = values[1].Split(';');
                    if (poseValues.Length == 6)
                    {
                        float posX = float.Parse(poseValues[0]);
                        float posY = float.Parse(poseValues[1]);
                        float posZ = float.Parse(poseValues[2]);
                        float rotX = float.Parse(poseValues[3]);
                        float rotY = float.Parse(poseValues[4]);
                        float rotZ = float.Parse(poseValues[5]);

                        UpdateRobotPose(new Vector3(posX, posY, posZ),
                                      new Vector3(rotX, rotY, rotZ));
                    }
                }
            }
        }
    }

    void UpdateJointByName(string jointName, float position)
    {
        foreach (Transform joint in robotJoints)
        {
            if (joint.name == jointName)
            {
                // Apply position to joint transform
                joint.localEulerAngles = new Vector3(position * Mathf.Rad2Deg, 0, 0);
                break;
            }
        }
    }

    void UpdateRobotPose(Vector3 position, Vector3 rotation)
    {
        robotModel.transform.position = position;
        robotModel.transform.eulerAngles = rotation;
    }

    void SendCommand(string command)
    {
        if (isConnected && stream != null)
        {
            byte[] data = Encoding.UTF8.GetBytes(command);
            stream.Write(data, 0, data.Length);
        }
    }

    void OnDestroy()
    {
        if (tcpClient != null)
        {
            tcpClient.Close();
        }
    }
}

Data Synchronization Techniques

Coordinate System Conversion

ROS and Unity use different coordinate systems:

• ROS: Right-handed (X-forward, Y-left, Z-up)
• Unity: Left-handed (X-right, Y-up, Z-forward)

Conversion functions:

public static class CoordinateConverter
{
    public static Vector3 RosToUnityPosition(Vector3 rosPos)
    {
        // Swap axes and negate as needed
        return new Vector3(rosPos.y, rosPos.z, rosPos.x);
    }

    public static Vector3 UnityToRosPosition(Vector3 unityPos)
    {
        // Reverse the conversion
        return new Vector3(unityPos.z, unityPos.x, unityPos.y);
    }

    public static Quaternion RosToUnityRotation(Quaternion rosQuat)
    {
        // Convert quaternion components for coordinate system change
        return new Quaternion(rosQuat.y, rosQuat.z, rosQuat.x, rosQuat.w);
    }

    public static Quaternion UnityToRosRotation(Quaternion unityQuat)
    {
        // Reverse the conversion
        return new Quaternion(unityQuat.z, unityQuat.x, unityQuat.y, unityQuat.w);
    }
}

Time Synchronization

Ensure both systems use synchronized time:

public class TimeSynchronizer : MonoBehaviour
{
    [Header("Time Synchronization")]
    public float gazeboTimeOffset = 0.0f;
    public float unityTimeOffset = 0.0f;

    private float lastSyncTime = 0.0f;

    public float GetSynchronizedTime()
    {
        // Get current time from Gazebo via ROS time topic
        // This is a simplified example
        float gazeboTime = Time.time + gazeboTimeOffset;
        float unityTime = Time.time + unityTimeOffset;

        // Return the most recent synchronized time
        return Mathf.Max(gazeboTime, unityTime);
    }

    void SyncTimeWithGazebo()
    {
        // Subscribe to /clock topic in ROS to get simulation time
        // Update time offsets accordingly
    }
}

Performance Optimization Strategies

Data Filtering and Rate Limiting

Not all data needs to be synchronized at full frequency:

using System.Collections.Generic;
using UnityEngine;

public class DataRateLimiter : MonoBehaviour
{
    [Header("Rate Limiting")]
    public float highFreqRate = 100.0f;  // 100 Hz for critical data (poses)
    public float medFreqRate = 30.0f;    // 30 Hz for important data (joints)
    public float lowFreqRate = 10.0f;    // 10 Hz for less critical data (sensors)

    private Dictionary<string, float> lastUpdateTime = new Dictionary<string, float>();
    private Dictionary<string, float> updateIntervals = new Dictionary<string, float>();

    void Start()
    {
        // Initialize update intervals
        updateIntervals["pose"] = 1.0f / highFreqRate;
        updateIntervals["joints"] = 1.0f / medFreqRate;
        updateIntervals["sensors"] = 1.0f / lowFreqRate;
    }

    public bool ShouldUpdate(string dataType)
    {
        float currentTime = Time.time;
        float interval = updateIntervals.ContainsKey(dataType) ? updateIntervals[dataType] : 0.1f;

        if (!lastUpdateTime.ContainsKey(dataType))
        {
            lastUpdateTime[dataType] = currentTime;
            return true;
        }

        if (currentTime - lastUpdateTime[dataType] >= interval)
        {
            lastUpdateTime[dataType] = currentTime;
            return true;
        }

        return false;
    }
}

Predictive Synchronization

Use interpolation to smooth data transmission:

using System.Collections.Generic;
using UnityEngine;

[System.Serializable]
public class RobotState
{
    public Vector3 position;
    public Quaternion rotation;
    public float timestamp;
}

public class PredictiveSync : MonoBehaviour
{
    [Header("Prediction Settings")]
    public int maxBufferSize = 10;
    public float predictionHorizon = 0.1f; // Predict 100ms ahead

    private Queue<RobotState> stateBuffer = new Queue<RobotState>();

    public void AddRobotState(Vector3 pos, Quaternion rot, float time)
    {
        RobotState newState = new RobotState
        {
            position = pos,
            rotation = rot,
            timestamp = time
        };

        stateBuffer.Enqueue(newState);

        // Keep buffer size manageable
        if (stateBuffer.Count > maxBufferSize)
        {
            stateBuffer.Dequeue();
        }
    }

    public RobotState PredictState(float futureTime)
    {
        if (stateBuffer.Count < 2)
        {
            // Not enough data for prediction, return latest
            if (stateBuffer.Count > 0)
                return stateBuffer.Peek();
            else
                return new RobotState();
        }

        // Simple linear prediction based on last two states
        RobotState recent = stateBuffer.ToArray()[stateBuffer.Count - 1];
        RobotState prev = stateBuffer.ToArray()[stateBuffer.Count - 2];

        float deltaTime = recent.timestamp - prev.timestamp;
        if (deltaTime <= 0) return recent;

        Vector3 velocity = (recent.position - prev.position) / deltaTime;
        Quaternion rotationDelta = Quaternion.Inverse(prev.rotation) * recent.rotation;
        float rotationSpeed = rotationDelta.eulerAngles.magnitude / deltaTime;

        // Predict position
        Vector3 predictedPos = recent.position + velocity * (futureTime - recent.timestamp);

        // Predict rotation (simplified)
        Vector3 rotationAxis = rotationDelta.eulerAngles.normalized;
        float predictedAngle = rotationSpeed * (futureTime - recent.timestamp);
        Quaternion predictedRot = recent.rotation * Quaternion.Euler(rotationAxis * predictedAngle * Mathf.Rad2Deg);

        return new RobotState
        {
            position = predictedPos,
            rotation = predictedRot,
            timestamp = futureTime
        };
    }
}

Implementation Patterns

Pattern 1: Visual-Only Bridge

Use Gazebo for physics and Unity for visualization:

• Gazebo calculates physics and sensor data
• Bridge transfers robot states to Unity
• Unity renders and handles user interaction

Pattern 2: Control-Feedback Loop

Close the loop between systems:

• Gazebo handles physics simulation
• Unity processes user input and high-level commands
• Bridge synchronizes states bidirectionally

Pattern 3: Distributed Simulation

Divide responsibilities:

• Gazebo: Physics, sensor simulation, low-level control
• Unity: Rendering, interaction, high-level visualization
• Bridge: Data synchronization and coordination

Troubleshooting Common Issues

Coordinate System Problems

• Verify coordinate system conversions
• Check if transformations are applied correctly
• Use visualization tools to debug pose mismatches

Timing Issues

• Ensure both systems are synchronized
• Account for network latency in real-time applications
• Use interpolation to smooth temporal discrepancies

Performance Bottlenecks

• Limit data transfer rate to necessary minimum
• Use compression for large data (e.g., point clouds)
• Optimize Unity rendering for real-time performance

Hands-On Exercise

1. Set up a simple robot model in both Gazebo and Unity
2. Implement a basic bridge using ROS# to synchronize robot pose
3. Add joint position synchronization
4. Implement a simple user interface in Unity to send commands to the Gazebo robot
5. Add predictive synchronization to improve responsiveness
6. Test the system with a simple navigation task

Example ROS command to check topics:

# Check available topics
ros2 topic list | grep -E "(joint|tf|robot)"

# Echo robot state
ros2 topic echo /joint_states sensor_msgs/msg/JointState

# Echo TF transforms
ros2 topic echo /tf tf2_msgs/msg/TFMessage

Advanced Topics

• Implementing haptic feedback in Unity based on Gazebo physics
• Using Unity for VR/AR teleoperation of Gazebo robots
• Integrating Unity ML-Agents with Gazebo physics for reinforcement learning
• Implementing distributed simulation across multiple machines

Summary

This lesson covered techniques for bridging Gazebo and Unity to create enhanced simulation experiences. You learned about different communication approaches, data synchronization techniques, and performance optimization strategies. The next module will explore NVIDIA Isaac Sim for photorealistic simulation and synthetic data generation.