Visualizations package (#196)

CONTRIBUTING.md

@@ -52,7 +52,8 @@ We run continuous integration on all PRs; all tests must be passing before the P
 
 All Python code should follow the [PEP 8 style guidelines](https://pep8.org/).
 
-All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). (Code may be reformatted to Unity coding standards where applicable.)
+All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions).
+(Code may be reformatted to Unity coding standards where applicable.)
 
 Please note that even if the code you are changing does not follow these conventions,
 we expect your submissions to do so.

README.md

@@ -1,4 +1,4 @@
-# ROS-TCP-Connector
+# ROS TCP Connector
 
 [![Version](https://img.shields.io/github/v/tag/Unity-Technologies/ROS-TCP-Connector)](https://github.com/Unity-Technologies/ROS-TCP-Connector/releases)
 [![License](https://img.shields.io/badge/license-Apache--2.0-green.svg)](LICENSE.md)
@@ -7,24 +7,37 @@
 ![ROS](https://img.shields.io/badge/ros2-foxy-brightgreen)
 ![Unity](https://img.shields.io/badge/unity-2020.2+-brightgreen)
 
-## Installation
-1. Using Unity 2020.2 or later, open the package manager from `Window` -> `Package Manager` and select "Add package from git URL..."
-![image](https://user-images.githubusercontent.com/29758400/110989310-8ea36180-8326-11eb-8318-f67ee200a23d.png)
-2. Enter the following URL. If you don't want to use the latest version, substitute your desired version tag where we've put `v0.5.0` in this example:
-`https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.5.0`
-3. Click `Add`.
+## Introduction
 
+This repository contains two Unity packages: the ROS TCP Connector, for sending/receiving messages from ROS, and the Visualizations Package, for adding visualizations of incoming and outgoing messages in the Unity scene.
+
+## Installation
+1. Using Unity 2020.2 or later, open the Package Manager from `Window` -> `Package Manager`.
+2. In the Package Manager window, find and click the + button in the upper lefthand corner of the window. Select `Add package from git URL....`
+
+    ![image](https://user-images.githubusercontent.com/29758400/110989310-8ea36180-8326-11eb-8318-f67ee200a23d.png)
+
+3. Enter the git URL for the desired package. Note: you can append a version tag to the end of the git url, like `#v0.4.0` or `#v0.5.0`, to declare a specific package version, or exclude the tag to get the latest from the package's `main` branch.
+    1. For the ROS-TCP-Connector, enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector`.
+    2. For Visualizations, enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.visualizations`.
+4. Click `Add`.
+
+To install from a local clone of the repository, see [installing a local package](https://docs.unity3d.com/Manual/upm-ui-local.html) in the Unity manual.
 
 ## Tutorials
-Scripts used to send [ROS](https://www.ros.org/) messages to an [TCP endpoint](https://github.com/Unity-Technologies/ROS_TCP_Endpoint) running as a ROS node.
+<!-- Scripts used to send [ROS](https://www.ros.org/) messages to an [TCP endpoint](https://github.com/Unity-Technologies/ROS_TCP_Endpoint) running as a ROS node. -->
 
-This Unity package provides three main features:
+This Unity package provides four main features:
 
-- ROSConnection: See the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/README.md) repository for information and tutorials on how to use this component.
+- ROSConnection: A component that sets up communication between ROS and Unity. See the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/tutorials/ros_unity_integration/README.md) for information and tutorials.
 
-- [Message Generation](MessageGeneration.md)
+- [Message Generation](MessageGeneration.md): A tool to generate C# classes for ROS message types.
 
-- [ROSGeometry](ROSGeometry.md)
+- Visualizations: A suite of default configurations and APIs to visualize incoming and outgoing information from ROS.
+    - See the [Nav2 SLAM Example](https://github.com/Unity-Technologies/Robotics-Nav2-SLAM-Example) for tutorials on using the Visualizations Package!
+    - You can also view the package's [Usage Information](com.unity.robotics.visualizations/Documentation~/README.md) for more details on using the package in your own project.
+
+- [ROSGeometry](ROSGeometry.md): A set of extensions that convert geometries between Unity and other coordinate frames.
 
 ## ROS#
 

com.unity.robotics.ros-tcp-connector/CHANGELOG.md

@@ -24,6 +24,7 @@ Upgrade the TestRosTcpConnector project to use Unity LTS version 2020.3.11f1
 
 - Added an optional pooling system for ros publishers
 - Implemented a queueing and latching system to mimic the ROS implementation in Unity
+- Add support for visualizations
 
 ### Changed
 - Publishing a message to an unregistered topic will show an error.

com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ActionAutoGen.cs

@@ -21,6 +21,7 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
     public class ActionAutoGen
     {
         private static readonly string[] types = { "Goal", "Result", "Feedback" };
+        private static readonly MessageSubtopic[] subtopics = { MessageSubtopic.Goal, MessageSubtopic.Result, MessageSubtopic.Feedback };
 
         public static string[] GetActionClassPaths(string inFilePath, string outPath)
         {
@@ -76,7 +77,16 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
                 string className = inFileName + types[i] + MsgAutoGenUtilities.ActionClassSuffix;
 
                 // Parse and generate goal, result, feedback messages
-                MessageParser parser = new MessageParser(tokens, outPath, rosPackageName, "action", MsgAutoGenUtilities.builtInTypesMapping, MsgAutoGenUtilities.builtInTypesDefaultInitialValues, className);
+                MessageParser parser = new MessageParser(
+                    tokens,
+                    outPath,
+                    rosPackageName,
+                    "action",
+                    MsgAutoGenUtilities.builtInTypesMapping,
+                    MsgAutoGenUtilities.builtInTypesDefaultInitialValues,
+                    className,
+                    subtopic: subtopics[i]
+                );
                 parser.Parse();
                 warnings.AddRange(parser.GetWarnings());
 

com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/MessageParser.cs

@@ -48,10 +48,11 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
         private HashSet<string> defaultValues = new HashSet<string>();
 
         private string body = "";
+        MessageSubtopic subtopic;
 
         private List<string> warnings = new List<string>();
 
-        public MessageParser(List<MessageToken> tokens, string outPath, string rosPackageName, string type, Dictionary<string, string> builtInTypeMapping, Dictionary<string, string> builtInTypesDefaultInitialValues, string className = "", string rosMsgName = "")
+        public MessageParser(List<MessageToken> tokens, string outPath, string rosPackageName, string type, Dictionary<string, string> builtInTypeMapping, Dictionary<string, string> builtInTypesDefaultInitialValues, string className = "", string rosMsgName = "", MessageSubtopic subtopic = MessageSubtopic.Default)
         {
             this.tokens = tokens;
 
@@ -61,6 +62,8 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
             this.rosPackageName = rosPackageName;
             this.rosPackageNamespace = MsgAutoGenUtilities.ResolvePackageName(rosPackageName);
 
+            this.subtopic = subtopic;
+
             if (className.Equals(""))
             {
                 this.className = MsgAutoGenUtilities.CapitalizeFirstLetter(inFileName) + MsgAutoGenUtilities.MessageClassSuffix;
@@ -151,6 +154,8 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
                 // Write ToString override
                 writer.Write(GenerateToString());
 
+                string subtopicParameter = subtopic == MessageSubtopic.Default ? "" : $", MessageSubtopic.{subtopic}";
+
                 writer.Write(
                     "\n" +
                     "#if UNITY_EDITOR\n" +
@@ -160,7 +165,7 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
                     "#endif\n" +
                     TWO_TABS + "public static void Register()\n" +
                     TWO_TABS + "{\n" +
-                    THREE_TABS + "MessageRegistry.Register(k_RosMessageName, Deserialize);\n" +
+                    THREE_TABS + $"MessageRegistry.Register(k_RosMessageName, Deserialize{subtopicParameter});\n" +
                     TWO_TABS + "}\n"
                 );
 

com.unity.robotics.ros-tcp-connector/Editor/MessageGeneration/ServiceAutoGen.cs

@@ -72,7 +72,15 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
                 // Service is made up of request and response
                 string className = inFileName + MsgAutoGenUtilities.ServiceClassSuffix + types[i];
 
-                MessageParser parser = new MessageParser(tokens, outPath, rosPackageName, "srv", MsgAutoGenUtilities.builtInTypesMapping, MsgAutoGenUtilities.builtInTypesDefaultInitialValues, className);
+                MessageParser parser = new MessageParser(
+                    tokens,
+                    outPath,
+                    rosPackageName,
+                    "srv",
+                    MsgAutoGenUtilities.builtInTypesMapping,
+                    MsgAutoGenUtilities.builtInTypesDefaultInitialValues,
+                    className,
+                    subtopic: i == 0 ? MessageSubtopic.Default : MessageSubtopic.Response);
                 parser.Parse();
                 warnings.AddRange(parser.GetWarnings());
             }

com.unity.robotics.ros-tcp-connector/Editor/ROSSettingsEditor.cs

@@ -14,12 +14,12 @@ namespace Unity.Robotics.ROSTCPConnector.Editor
         {
             ROSSettingsEditor window = GetWindow<ROSSettingsEditor>(false, "ROS Settings", true);
             window.minSize = new Vector2(300, 65);
-            window.maxSize = new Vector2(600, 250);
+            window.maxSize = new Vector2(600, 500);
             window.Show();
         }
 
         GameObject prefabObj;
-        ROSConnection prefab;
+        Unity.Robotics.ROSTCPConnector.ROSConnection prefab;
 
         public enum RosProtocol
         {
@@ -35,6 +35,9 @@ namespace Unity.Robotics.ROSTCPConnector.Editor
         const RosProtocol k_AlternateProtocol = RosProtocol.ROS2;
 #endif
 
+        [SerializeField] string[] m_TFTopics;
+        UnityEditor.Editor editor;
+
         protected virtual void OnGUI()
         {
             if (prefab == null)
@@ -49,8 +52,7 @@ namespace Unity.Robotics.ROSTCPConnector.Editor
                     sceneObj.AddComponent<ROSConnection>();
                     if (!Directory.Exists("Assets/Resources"))
                         Directory.CreateDirectory("Assets/Resources");
-                    prefabObj = PrefabUtility.SaveAsPrefabAsset(sceneObj,
-                        "Assets/Resources/ROSConnectionPrefab.prefab");
+                    prefabObj = PrefabUtility.SaveAsPrefabAsset(sceneObj, "Assets/Resources/ROSConnectionPrefab.prefab");
                     if (prefabObj != null)
                         prefab = prefabObj.GetComponent<ROSConnection>();
                     DestroyImmediate(sceneObj);
@@ -96,8 +98,8 @@ namespace Unity.Robotics.ROSTCPConnector.Editor
             prefab.RosPort = EditorGUILayout.IntField("ROS Port", prefab.RosPort);
 
             // Also set the player prefs, for users who hit play in the editor: they will expect the last-used IP address to appear in the hud
-            HUDPanel.SetIPPref(prefab.RosIPAddress);
-            HUDPanel.SetPortPref(prefab.RosPort);
+            ROSConnection.SetIPPref(prefab.RosIPAddress);
+            ROSConnection.SetPortPref(prefab.RosPort);
 
             EditorGUILayout.Space();
 
@@ -127,10 +129,27 @@ namespace Unity.Robotics.ROSTCPConnector.Editor
                     "Sleep this long before checking for new network messages. (Decreasing this time will make it respond faster, but consume more CPU)."),
                 prefab.SleepTimeSeconds);
 
+            // TODO: make the settings input update the prefab
+            // EditorGUILayout.Space();
+            // if (!editor) { editor = UnityEditor.Editor.CreateEditor(this); }
+            // if (editor) { editor.OnInspectorGUI(); }
+
             if (GUI.changed)
             {
                 PrefabUtility.SavePrefabAsset(prefab.gameObject);
             }
         }
+
+        void OnInspectorUpdate() { Repaint(); }
+    }
+
+    [CustomEditor(typeof(ROSSettingsEditor), true)]
+    public class TFTopicsEditorDrawer : UnityEditor.Editor
+    {
+        public override void OnInspectorGUI()
+        {
+            var list = serializedObject.FindProperty("m_TFTopics");
+            EditorGUILayout.PropertyField(list, new GUIContent("TF Topics"), true);
+        }
     }
 }

com.unity.robotics.ros-tcp-connector/Editor/Unity.Robotics.ROSTCPConnector.Editor.asmdef

@@ -1,5 +1,6 @@
 {
     "name": "Unity.Robotics.ROSTCPConnector.Editor",
+    "rootNamespace": "",
     "references": [
         "GUID:625bfc588fb96c74696858f2c467e978"
     ],

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs

@@ -0,0 +1,200 @@
+using RosMessageTypes.Sensor;
+using RosMessageTypes.Std;
+using RosMessageTypes.Geometry;
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
+{
+    // Source: https://jamesmccaffrey.wordpress.com/2015/03/06/inverting-a-matrix-using-c/
+    public static class MatrixExtensions
+    {
+        public static double[][] MatrixCreate(int rows, int cols)
+        {
+            double[][] result = new double[rows][];
+            for (int i = 0; i < rows; ++i)
+                result[i] = new double[cols];
+            return result;
+        }
+
+        public static double[][] MatrixDecompose(double[][] matrix, out int[] perm, out int toggle)
+        {
+            // Doolittle LUP decomposition with partial pivoting.
+            // rerturns: result is L (with 1s on diagonal) and U;
+            // perm holds row permutations; toggle is +1 or -1 (even or odd)
+            int rows = matrix.Length;
+            int cols = matrix[0].Length; // assume square
+            if (rows != cols)
+                throw new Exception("Attempt to decompose a non-square m");
+
+            int n = rows; // convenience
+
+            double[][] result = MatrixDuplicate(matrix);
+
+            perm = new int[n]; // set up row permutation result
+            for (int i = 0; i < n; ++i) { perm[i] = i; }
+
+            toggle = 1; // toggle tracks row swaps.
+            // +1 -> even, -1 -> odd. used by MatrixDeterminant
+
+            for (int j = 0; j < n - 1; ++j) // each column
+            {
+                double colMax = Math.Abs(result[j][j]); // find largest val in col
+                int pRow = j;
+                //for (int i = j + 1; i < n; ++i)
+                //{
+                //  if (result[i][j] > colMax)
+                //  {
+                //    colMax = result[i][j];
+                //    pRow = i;
+                //  }
+                //}
+
+                // reader Matt V needed this:
+                for (int i = j + 1; i < n; ++i)
+                {
+                    if (Math.Abs(result[i][j]) > colMax)
+                    {
+                        colMax = Math.Abs(result[i][j]);
+                        pRow = i;
+                    }
+                }
+                // Not sure if this approach is needed always, or not.
+
+                if (pRow != j) // if largest value not on pivot, swap rows
+                {
+                    double[] rowPtr = result[pRow];
+                    result[pRow] = result[j];
+                    result[j] = rowPtr;
+
+                    int tmp = perm[pRow]; // and swap perm info
+                    perm[pRow] = perm[j];
+                    perm[j] = tmp;
+
+                    toggle = -toggle; // adjust the row-swap toggle
+                }
+
+                // --------------------------------------------------
+                // This part added later (not in original)
+                // and replaces the 'return null' below.
+                // if there is a 0 on the diagonal, find a good row
+                // from i = j+1 down that doesn't have
+                // a 0 in column j, and swap that good row with row j
+                // --------------------------------------------------
+
+                if (result[j][j] == 0.0)
+                {
+                    // find a good row to swap
+                    int goodRow = -1;
+                    for (int row = j + 1; row < n; ++row)
+                    {
+                        if (result[row][j] != 0.0)
+                            goodRow = row;
+                    }
+
+                    if (goodRow == -1)
+                        throw new Exception("Cannot use Doolittle's method");
+
+                    // swap rows so 0.0 no longer on diagonal
+                    double[] rowPtr = result[goodRow];
+                    result[goodRow] = result[j];
+                    result[j] = rowPtr;
+
+                    int tmp = perm[goodRow]; // and swap perm info
+                    perm[goodRow] = perm[j];
+                    perm[j] = tmp;
+
+                    toggle = -toggle; // adjust the row-swap toggle
+                }
+                // --------------------------------------------------
+                // if diagonal after swap is zero . .
+                //if (Math.Abs(result[j][j]) < 1.0E-20) 
+                //  return null; // consider a throw
+
+                for (int i = j + 1; i < n; ++i)
+                {
+                    result[i][j] /= result[j][j];
+                    for (int k = j + 1; k < n; ++k)
+                    {
+                        result[i][k] -= result[i][j] * result[j][k];
+                    }
+                }
+
+
+            } // main j column loop
+
+            return result;
+        }
+
+        public static double[][] MatrixDuplicate(double[][] matrix)
+        {
+            // allocates/creates a duplicate of a matrix.
+            double[][] result = MatrixCreate(matrix.Length, matrix[0].Length);
+            for (int i = 0; i < matrix.Length; ++i) // copy the values
+                for (int j = 0; j < matrix[i].Length; ++j)
+                    result[i][j] = matrix[i][j];
+            return result;
+        }
+
+        public static double[][] MatrixInverse(double[][] matrix)
+        {
+            int n = matrix.Length;
+            double[][] result = MatrixDuplicate(matrix);
+
+            int[] perm;
+            int toggle;
+            double[][] lum = MatrixDecompose(matrix, out perm,
+                out toggle);
+            if (lum == null)
+                throw new Exception("Unable to compute inverse");
+
+            double[] b = new double[n];
+            for (int i = 0; i < n; ++i)
+            {
+                for (int j = 0; j < n; ++j)
+                {
+                    if (i == perm[j])
+                        b[j] = 1.0;
+                    else
+                        b[j] = 0.0;
+                }
+
+                double[] x = HelperSolve(lum, b); // 
+
+                for (int j = 0; j < n; ++j)
+                    result[j][i] = x[j];
+            }
+            return result;
+        }
+
+        public static double[] HelperSolve(double[][] luMatrix, double[] b)
+        {
+            // before calling this helper, permute b using the perm array
+            // from MatrixDecompose that generated luMatrix
+            int n = luMatrix.Length;
+            double[] x = new double[n];
+            b.CopyTo(x, 0);
+
+            for (int i = 1; i < n; ++i)
+            {
+                double sum = x[i];
+                for (int j = 0; j < i; ++j)
+                    sum -= luMatrix[i][j] * x[j];
+                x[i] = sum;
+            }
+
+            x[n - 1] /= luMatrix[n - 1][n - 1];
+            for (int i = n - 2; i >= 0; --i)
+            {
+                double sum = x[i];
+                for (int j = i + 1; j < n; ++j)
+                    sum -= luMatrix[i][j] * x[j];
+                x[i] = sum / luMatrix[i][i];
+            }
+
+            return x;
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MatrixExtensions.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 59e6ede11dca14588816d491dbf9d848
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageDeserializer.cs

@@ -13,6 +13,12 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
         int alignmentCorrection;
 #endif
 
+        public Message DeserializeMessage(string rosMessageName, byte[] data, MessageSubtopic subtopic = MessageSubtopic.Default)
+        {
+            InitWithBuffer(data);
+            return MessageRegistry.GetDeserializeFunction(rosMessageName, subtopic)(this);
+        }
+
         public T DeserializeMessage<T>(byte[] data) where T : Message
         {
             InitWithBuffer(data);

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs

@@ -0,0 +1,757 @@
+using RosMessageTypes.Sensor;
+using RosMessageTypes.Std;
+using RosMessageTypes.Geometry;
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using RosMessageTypes.BuiltinInterfaces;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
+{
+    public enum BatteryState_Status_Constants
+    {
+        UNKNOWN = 0,
+        CHARGING = 1,
+        DISCHARGING = 2,
+        NOT_CHARGING = 3,
+        FULL = 4
+    };
+
+    public enum BatteryState_Health_Constants
+    {
+        UNKNOWN = 0,
+        GOOD = 1,
+        OVERHEAT = 2,
+        DEAD = 3,
+        OVERVOLTAGE = 4,
+        UNSPEC_FAILURE = 5,
+        COLD = 6,
+        WATCHDOG_TIMER_EXPIRE = 7,
+        SAFETY_TIMER_EXPIRE = 8
+    };
+
+    public enum BatteryState_Technology_Constants
+    {
+        UNKNOWN = 0,
+        NIMH = 1,
+        LION = 2,
+        LIPO = 3,
+        LIFE = 4,
+        NICD = 5,
+        LIMN = 6
+    };
+
+    public enum JoyFeedback_Type_Constants
+    {
+        LED = 0,
+        RUMBLE = 1,
+        BUZZER = 2,
+    };
+
+    public enum JoyRegion
+    {
+        BSouth = 0,
+        BEast = 1,
+        BWest = 2,
+        BNorth = 3,
+        LB = 4,
+        RB = 5,
+        Back = 6,
+        Start = 7,
+        Power = 8,
+        LPress = 9,
+        RPress = 10,
+        LStick, RStick, LT, RT, DPad, lAxisX, lAxisY,
+        rAxisX, rAxisY, ltAxis, rtAxis, dAxisX, dAxisY
+    };
+
+    public enum NavSatStatus_Type_Constants
+    {
+        NO_FIX = -1,        // unable to fix position
+        FIX = 0,        // unaugmented fix
+        SBAS_FIX = 1,        // with satellite-based augmentation
+        GBAS_FIX = 2         // with ground-based augmentation
+    };
+
+    public enum NavSatStatus_Service_Constants
+    {
+        GPS = 1,
+        GLONASS = 2,
+        COMPASS = 4,      // includes BeiDou.
+        GALILEO = 8
+    };
+
+    public enum NavSatFix_Covariance_Constants
+    {
+        UNKNOWN = 0,
+        APPROXIMATED = 1,
+        DIAGONAL_KNOWN = 2,
+        KNOWN = 3
+    };
+
+    public enum Range_RadiationType_Constants
+    {
+        ULTRASOUND = 0,
+        INFRARED = 1
+    };
+
+    public enum PointField_Format_Constants
+    {
+        INT8 = 1,
+        UINT8 = 2,
+        INT16 = 3,
+        UINT16 = 4,
+        INT32 = 5,
+        UINT32 = 6,
+        FLOAT32 = 7,
+        FLOAT64 = 8
+    }
+
+    // Convenience functions for built-in message types
+    public static class MessageExtensions
+    {
+        public static string ToTimestampString(this TimeMsg message)
+        {
+            // G: format using short date and long time
+            return message.ToDateTime().ToString("G") + $"(+{message.nanosec})";
+        }
+
+        public static long ToLongTime(this TimeMsg message)
+        {
+            return (long)message.sec << 32 | message.nanosec;
+        }
+
+        public static DateTime ToDateTime(this TimeMsg message)
+        {
+            DateTime time = new DateTime(message.sec);
+            time = time.AddMilliseconds(message.nanosec / 1E6);
+            return time;
+        }
+
+        // public static TimeMsg TimeMsg(this DateTime dateTime)
+        // {
+        //     return new TimeMsg { sec = Convert.ToUInt32(dateTime.Ticks), nanosec = Convert.ToUInt32(dateTime.Millisecond * 1E6) };
+        // }
+
+        public static Color ToUnityColor(this ColorRGBAMsg message)
+        {
+            return new Color(message.r, message.g, message.b, message.a);
+        }
+
+        public static ColorRGBAMsg ColorRGBAMsg(this Color color)
+        {
+            return new ColorRGBAMsg(color.r, color.g, color.b, color.a);
+        }
+
+        /// <summary>
+        /// Converts a byte array from BGR to RGB.
+        /// </summary>
+        static byte[] EncodingConversion(ImageMsg image, bool convertBGR = true, bool flipY = true)
+        {
+            // Number of channels in this encoding
+            int channels = image.GetNumChannels();
+
+            if (!image.EncodingRequiresBGRConversion())
+                convertBGR = false;
+
+            // If no modifications are necessary, return original array
+            if (!convertBGR && !flipY)
+                return image.data;
+
+            int channelStride = image.GetBytesPerChannel();
+            int pixelStride = channelStride * channels;
+            int rowStride = pixelStride * (int)image.width;
+
+            if (flipY)
+            {
+                ReverseInBlocks(image.data, rowStride, (int)image.height);
+            }
+
+            if (convertBGR)
+            {
+                // given two channels, we swap R with G (distance = 1).
+                // given three or more channels, we swap R with B (distance = 2).
+                int swapDistance = channels == 2 ? channelStride : channelStride * 2;
+                int dataLength = (int)image.width * (int)image.height * pixelStride;
+
+                if (channelStride == 1)
+                {
+                    // special case for the 1-byte-per-channel formats: avoid the inner loop
+                    for (int pixelIndex = 0; pixelIndex < dataLength; pixelIndex += pixelStride)
+                    {
+                        int swapB = pixelIndex + swapDistance;
+                        byte temp = image.data[pixelIndex];
+                        image.data[pixelIndex] = image.data[swapB];
+                        image.data[swapB] = temp;
+                    }
+                }
+                else
+                {
+                    for (int pixelIndex = 0; pixelIndex < dataLength; pixelIndex += pixelStride)
+                    {
+                        int channelEndByte = pixelIndex + channelStride;
+                        for (int byteIndex = pixelIndex; byteIndex < channelEndByte; byteIndex++)
+                        {
+                            int swapB = byteIndex + swapDistance;
+                            byte temp = image.data[byteIndex];
+                            image.data[byteIndex] = image.data[swapB];
+                            image.data[swapB] = temp;
+                        }
+                    }
+                }
+            }
+            return image.data;
+        }
+
+        static byte[] s_ScratchSpace;
+
+        static void ReverseInBlocks(byte[] array, int blockSize, int numBlocks)
+        {
+            if (blockSize * numBlocks > array.Length)
+            {
+                Debug.LogError($"Invalid ReverseInBlocks, array length is {array.Length}, should be at least {blockSize * numBlocks}");
+                return;
+            }
+
+            if (s_ScratchSpace == null || s_ScratchSpace.Length < blockSize)
+                s_ScratchSpace = new byte[blockSize];
+
+            int startBlockIndex = 0;
+            int endBlockIndex = ((int)numBlocks - 1) * blockSize;
+
+            while (startBlockIndex < endBlockIndex)
+            {
+                Buffer.BlockCopy(array, startBlockIndex, s_ScratchSpace, 0, blockSize);
+                Buffer.BlockCopy(array, endBlockIndex, array, startBlockIndex, blockSize);
+                Buffer.BlockCopy(s_ScratchSpace, 0, array, endBlockIndex, blockSize);
+                startBlockIndex += blockSize;
+                endBlockIndex -= blockSize;
+            }
+        }
+
+        static void ReverseInBlocks<T1, T2>(T1[] fromArray, T2[] toArray, int blockSize, int numBlocks)
+        {
+            int startBlockIndex = 0;
+            int endBlockIndex = ((int)numBlocks - 1) * blockSize;
+
+            while (startBlockIndex < endBlockIndex)
+            {
+                Buffer.BlockCopy(fromArray, startBlockIndex, toArray, endBlockIndex, blockSize);
+                Buffer.BlockCopy(fromArray, endBlockIndex, toArray, startBlockIndex, blockSize);
+                startBlockIndex += blockSize;
+                endBlockIndex -= blockSize;
+            }
+        }
+
+        public static void DebayerConvert(this ImageMsg image, bool flipY = true)
+        {
+            int channelStride = image.GetBytesPerChannel();
+            int width = (int)image.width;
+            int height = (int)image.height;
+            int rowStride = width * channelStride;
+            int dataSize = rowStride * height;
+            int finalPixelStride = channelStride * 4;
+
+            int[] reorderIndices;
+            switch (image.encoding)
+            {
+                case "bayer_rggb8":
+                    reorderIndices = new int[] { 0, 1, width + 1 };
+                    break;
+                case "bayer_bggr8":
+                    reorderIndices = new int[] { width + 1, 1, 0 };
+                    break;
+                case "bayer_gbrg8":
+                    reorderIndices = new int[] { width, 0, 1 };
+                    break;
+                case "bayer_grbg8":
+                    reorderIndices = new int[] { 1, 0, width };
+                    break;
+                case "bayer_rggb16":
+                    reorderIndices = new int[] { 0, 1, 2, 3, rowStride + 2, rowStride + 3 };
+                    break;
+                case "bayer_bggr16":
+                    reorderIndices = new int[] { rowStride + 2, rowStride + 3, 2, 3, 0, 1 };
+                    break;
+                case "bayer_gbrg16":
+                    reorderIndices = new int[] { rowStride, rowStride + 1, 0, 1, 2, 3 };
+                    break;
+                case "bayer_grbg16":
+                    reorderIndices = new int[] { 2, 3, 0, 1, rowStride, rowStride + 1 };
+                    break;
+                default:
+                    return;
+            }
+
+            if (flipY)
+            {
+                ReverseInBlocks(image.data, rowStride * 2, (int)image.height / 2);
+            }
+
+            if (s_ScratchSpace == null || s_ScratchSpace.Length < rowStride * 2)
+                s_ScratchSpace = new byte[rowStride * 2];
+
+            int rowStartIndex = 0;
+            while (rowStartIndex < dataSize)
+            {
+                Buffer.BlockCopy(image.data, rowStartIndex, s_ScratchSpace, 0, rowStride * 2);
+                int pixelReadIndex = 0;
+                int pixelWriteIndex = rowStartIndex;
+                while (pixelReadIndex < rowStride)
+                {
+                    for (int Idx = 0; Idx < reorderIndices.Length; ++Idx)
+                    {
+                        image.data[pixelWriteIndex + Idx] = s_ScratchSpace[pixelReadIndex + reorderIndices[Idx]];
+                    }
+                    image.data[pixelWriteIndex + reorderIndices.Length] = 255;
+                    if (channelStride == 2)
+                        image.data[pixelWriteIndex + reorderIndices.Length + 1] = 255;
+                    pixelReadIndex += channelStride * 2;
+                    pixelWriteIndex += finalPixelStride;
+                }
+                rowStartIndex += rowStride * 2;
+            }
+
+            image.width = image.width / 2;
+            image.height = image.height / 2;
+            image.encoding = channelStride == 1 ? "rgba8" : "rgba16";
+            image.step = (uint)(channelStride * image.width);
+        }
+
+        public static int GetNumChannels(this ImageMsg image)
+        {
+            switch (image.encoding)
+            {
+                case "8SC1":
+                case "8UC1":
+                case "16SC1":
+                case "16UC1":
+                case "32FC1":
+                case "32SC1":
+                case "64FC1":
+                case "mono8":
+                case "mono16":
+                case "bayer_rggb8":
+                case "bayer_bggr8":
+                case "bayer_gbrg8":
+                case "bayer_grbg8":
+                case "bayer_rggb16":
+                case "bayer_bggr16":
+                case "bayer_gbrg16":
+                case "bayer_grbg16":
+                    return 1;
+                case "8SC2":
+                case "8UC2":
+                case "16SC2":
+                case "16UC2":
+                case "32FC2":
+                case "32SC2":
+                case "64FC2":
+                    return 2;
+                case "8SC3":
+                case "8UC3":
+                case "16SC3":
+                case "16UC3":
+                case "32FC3":
+                case "32SC3":
+                case "64FC3":
+                case "bgr8":
+                case "rgb8":
+                    return 3;
+                case "8SC4":
+                case "8UC4":
+                case "16SC4":
+                case "16UC4":
+                case "32FC4":
+                case "32SC4":
+                case "64FC4":
+                case "bgra8":
+                case "rgba8":
+                    return 4;
+            }
+            return 4;
+        }
+
+        public static bool IsBayerEncoded(this ImageMsg image)
+        {
+            switch (image.encoding)
+            {
+                case "bayer_rggb8":
+                case "bayer_bggr8":
+                case "bayer_gbrg8":
+                case "bayer_grbg8":
+                case "bayer_rggb16":
+                case "bayer_bggr16":
+                case "bayer_gbrg16":
+                case "bayer_grbg16":
+                    return true;
+                default:
+                    return false;
+            }
+        }
+
+        public static bool EncodingRequiresBGRConversion(this ImageMsg image)
+        {
+            switch (image.encoding)
+            {
+                case "8SC1":
+                case "8UC1":
+                case "16SC1":
+                case "16UC1":
+                case "32FC1":
+                case "32SC1":
+                case "64FC1":
+                case "mono8":
+                case "mono16":
+                    // single channel = nothing to swap
+                    return false;
+                case "8UC4":
+                case "8SC4":
+                case "bgra8":
+                    return false; // raw BGRA32 texture format
+                case "rgb8":
+                    return false; // raw RGB24 texture format
+                case "rgba8":
+                    return false; // raw RGB32 texture format
+                case "bayer_rggb8":
+                case "bayer_bggr8":
+                case "bayer_gbrg8":
+                case "bayer_grbg8":
+                    return false; // bayer has its own conversions needed
+                default:
+                    return true;
+            }
+        }
+
+        public static int GetBytesPerChannel(this ImageMsg image)
+        {
+            switch (image.encoding)
+            {
+                case "8SC1":
+                case "8SC2":
+                case "8SC3":
+                case "8SC4":
+                case "8UC1":
+                case "8UC2":
+                case "8UC3":
+                case "8UC4":
+                case "mono8":
+                case "bgr8":
+                case "rgb8":
+                case "bgra8":
+                case "rgba8":
+                case "bayer_rggb8":
+                case "bayer_bggr8":
+                case "bayer_gbrg8":
+                case "bayer_grbg8":
+                    return 1;
+                case "16SC1":
+                case "16SC2":
+                case "16SC3":
+                case "16SC4":
+                case "16UC1":
+                case "16UC2":
+                case "16UC3":
+                case "16UC4":
+                case "mono16":
+                case "bayer_rggb16":
+                case "bayer_bggr16":
+                case "bayer_gbrg16":
+                case "bayer_grbg16":
+                    return 2;
+                case "32FC1":
+                case "32SC1":
+                case "32FC2":
+                case "32SC2":
+                case "32FC3":
+                case "32SC3":
+                case "32FC4":
+                case "32SC4":
+                    return 4;
+                case "64FC1":
+                case "64FC2":
+                case "64FC3":
+                case "64FC4":
+                    return 8;
+            }
+            return 1;
+        }
+
+        public static TextureFormat GetTextureFormat(this ImageMsg image)
+        {
+            switch (image.encoding)
+            {
+                case "8UC1":
+                case "8SC1":
+                    return TextureFormat.R8;
+                case "8UC2":
+                case "8SC2":
+                    return TextureFormat.RG16;
+                case "8UC3":
+                case "8SC3":
+                    return TextureFormat.RGB24;
+                case "8UC4":
+                case "8SC4":
+                case "bgra8":
+                    return TextureFormat.BGRA32; // unity supports these natively
+                case "16UC1":
+                case "16SC1":
+                    return TextureFormat.R16;
+                case "16UC2":
+                case "16SC2":
+                    return TextureFormat.RG32;
+                case "16UC3":
+                case "16SC3":
+                    return TextureFormat.RGB48;
+                case "16UC4":
+                case "16SC4":
+                    return TextureFormat.RGBA64;
+                case "32SC1":
+                case "32SC2":
+                case "32SC3":
+                case "32SC4":
+                    throw new NotImplementedException("32 bit integer texture formats are not supported");
+                case "32FC1":
+                    return TextureFormat.RFloat;
+                case "32FC2":
+                    return TextureFormat.RGFloat;
+                case "32FC3":
+                    throw new NotImplementedException("32FC3 texture format is not supported");
+                case "32FC4":
+                    return TextureFormat.RGBAFloat;
+                case "64FC1":
+                case "64FC2":
+                case "64FC3":
+                case "64FC4":
+                    throw new NotImplementedException("Double precision texture formats are not supported");
+                case "mono8":
+                    return TextureFormat.R8;
+                case "mono16":
+                    return TextureFormat.R16;
+                case "bgr8":
+                    return TextureFormat.RGB24;
+                case "rgb8":
+                    return TextureFormat.RGB24; // unity supports this natively
+                case "rgba8":
+                    return TextureFormat.RGBA32; // unity supports this natively
+                case "bayer_rggb8":
+                case "bayer_bggr8":
+                case "bayer_gbrg8":
+                case "bayer_grbg8":
+                    return TextureFormat.R8;
+                case "bayer_rggb16":
+                case "bayer_bggr16":
+                case "bayer_gbrg16":
+                case "bayer_grbg16":
+                    return TextureFormat.R16;
+            }
+            return TextureFormat.RGB24;
+        }
+
+        public static Texture2D ToTexture2D(this CompressedImageMsg message)
+        {
+            var tex = new Texture2D(1, 1);
+            tex.LoadImage(message.data);
+            return tex;
+        }
+
+        public static Texture2D ToTexture2D(this ImageMsg message, bool debayer = false, bool convertBGR = true, bool flipY = true)
+        {
+            Texture2D tex;
+            byte[] data;
+            if (debayer && message.IsBayerEncoded())
+            {
+                tex = new Texture2D((int)message.width / 2, (int)message.height / 2, TextureFormat.RGBA32, false);
+                message.DebayerConvert(flipY);
+                data = message.data;
+            }
+            else
+            {
+                tex = new Texture2D((int)message.width, (int)message.height, message.GetTextureFormat(), false);
+                data = EncodingConversion(message, convertBGR, flipY);
+            }
+
+            tex.LoadRawTextureData(data);
+            tex.Apply();
+            return tex;
+        }
+
+        /// <summary>
+        /// Finds the world coordinates of an image coordinate based on the CameraInfo matrices
+        /// K matrix     P matrix
+        /// 0 1 2        0 1 2 3
+        /// 3 4 5        4 5 6 7
+        /// 6 7 8        8 9 10 11
+        /// </summary>
+        /// <param name="x">x coord to map</param>
+        /// <param name="y">y coord to map</param>
+        /// <param name="P">Projection matrix (3x4)</param>
+        /// <param name="K">Camera intrinsics (3x3)</param>
+        /// <returns></returns>
+        static PointMsg FindPixelProjection(int x, int y, double[] P, double[][] invK)
+        {
+            PointMsg worldCoord = new PointMsg(x, y, 1);
+
+            return new PointMsg((float)invK[0][0] * worldCoord.x + (float)invK[0][1] * worldCoord.y + (float)invK[0][2] * worldCoord.z, (float)invK[1][0] * worldCoord.x + (float)invK[1][1] * worldCoord.y + (float)invK[1][2] * worldCoord.z, (float)invK[2][0] * worldCoord.x + (float)invK[2][1] * worldCoord.y + (float)invK[2][2] * worldCoord.z);
+        }
+
+        public static PointMsg[] GetPixelsInWorld(this CameraInfoMsg cameraInfo)
+        {
+            List<PointMsg> res = new List<PointMsg>();
+
+            double[][] formatK = new double[][] {
+                new double[] { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2] },
+                new double[] { cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5] },
+                new double[] { cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8] },
+            };
+
+            var inverseK = MatrixExtensions.MatrixInverse(formatK);
+
+            for (int i = 0; i < cameraInfo.width; i++)
+            {
+                for (int j = 0; j < cameraInfo.height; j++)
+                {
+                    res.Add(FindPixelProjection(i, j, cameraInfo.p, inverseK));
+                }
+            }
+
+            return res.ToArray();
+        }
+
+        public static Texture2D ApplyCameraInfoProjection(this Texture2D tex, CameraInfoMsg cameraInfo)
+        {
+            var newTex = new Texture2D(tex.width, tex.height);
+
+            double[][] formatK = new double[][] {
+                new double[] { cameraInfo.k[0], cameraInfo.k[1], cameraInfo.k[2] },
+                new double[] { cameraInfo.k[3], cameraInfo.k[4], cameraInfo.k[5] },
+                new double[] { cameraInfo.k[6], cameraInfo.k[7], cameraInfo.k[8] },
+            };
+
+            var inverseK = MatrixExtensions.MatrixInverse(formatK);
+            for (int i = 0; i < tex.width; i++)
+            {
+                for (int j = 0; j < tex.height; j++)
+                {
+                    var newPix = FindPixelProjection(i, j, cameraInfo.p, inverseK);
+                    var color = tex.GetPixel((int)newPix.x, (int)newPix.y);
+                    newTex.SetPixel(i, j, color);
+                }
+            }
+            newTex.Apply();
+
+            return newTex;
+        }
+
+        public static CompressedImageMsg ToCompressedImageMsg_JPG(this Texture2D tex, HeaderMsg header)
+        {
+            return new CompressedImageMsg(header, "jpeg", tex.EncodeToJPG());
+        }
+
+        public static CompressedImageMsg ToCompressedImageMsg_PNG(this Texture2D tex, HeaderMsg header)
+        {
+            return new CompressedImageMsg(header, "png", tex.EncodeToPNG());
+        }
+
+        /// <summary>
+        /// Creates a new Texture2D that grabs a region of interest of a given texture, if applicable. Otherwise, an approximated empty texture with a highlighted region is returned.
+        /// </summary>
+        public static Texture2D RegionOfInterestTexture(this RegionOfInterestMsg message, Texture2D rawTex, int height = 0, int width = 0)
+        {
+            int mWidth = (int)message.width;
+            int mHeight = (int)message.height;
+            int x_off = ((int)message.x_offset == mWidth) ? 0 : (int)message.x_offset;
+            int y_off = ((int)message.y_offset == mHeight) ? 0 : (int)message.y_offset;
+
+            Texture2D overlay;
+            if (rawTex == null)
+            {
+                // No texture provided, just return approximation
+                if (width == 0 || height == 0)
+                {
+                    overlay = new Texture2D(x_off + mWidth + 10, y_off + mHeight + 10);
+                }
+                else
+                {
+                    overlay = new Texture2D(width, height);
+                }
+
+                // Initialize ROI color block
+                Color[] colors = new Color[mHeight * mWidth];
+                for (int i = 0; i < colors.Length; i++)
+                {
+                    colors[i] = Color.red;
+                }
+
+                overlay.SetPixels(x_off, y_off, mWidth, mHeight, colors);
+            }
+            else
+            {
+                // Crop out ROI from input texture
+                overlay = new Texture2D(mWidth - x_off, mHeight - y_off, rawTex.format, true);
+                overlay.SetPixels(0, 0, overlay.width, overlay.height, rawTex.GetPixels(x_off, 0, mWidth - x_off, mHeight - y_off));
+            }
+            overlay.Apply();
+            return overlay;
+        }
+
+        public static ImageMsg ToImageMsg(this Texture2D tex, HeaderMsg header)
+        {
+            byte[] data = null;
+            string encoding = "rgba8";
+            switch (tex.format)
+            {
+                case TextureFormat.RGB24:
+                    data = new byte[tex.width * tex.height * 3];
+                    tex.GetPixelData<byte>(0).CopyTo(data);
+                    encoding = "rgb8";
+                    ReverseInBlocks(data, tex.width * 3, tex.height);
+                    break;
+                case TextureFormat.RGBA32:
+                    data = new byte[tex.width * tex.height * 4];
+                    tex.GetPixelData<byte>(0).CopyTo(data);
+                    encoding = "rgba8";
+                    ReverseInBlocks(data, tex.width * 4, tex.height);
+                    break;
+                case TextureFormat.R8:
+                    data = new byte[tex.width * tex.height];
+                    tex.GetPixelData<byte>(0).CopyTo(data);
+                    encoding = "8UC1";
+                    ReverseInBlocks(data, tex.width, tex.height);
+                    break;
+                case TextureFormat.R16:
+                    data = new byte[tex.width * tex.height * 2];
+                    tex.GetPixelData<byte>(0).CopyTo(data);
+                    encoding = "16UC1";
+                    ReverseInBlocks(data, tex.width * 2, tex.height);
+                    break;
+                default:
+                    Color32[] pixels = tex.GetPixels32();
+                    data = new byte[pixels.Length * 4];
+                    // although this is painfully slow, it does work... Surely there's a better way
+                    int writeIdx = 0;
+                    for (int Idx = 0; Idx < pixels.Length; ++Idx)
+                    {
+                        Color32 p = pixels[Idx];
+                        data[writeIdx] = p.r;
+                        data[writeIdx + 1] = p.g;
+                        data[writeIdx + 2] = p.b;
+                        data[writeIdx + 3] = p.a;
+                        writeIdx += 4;
+                    }
+                    ReverseInBlocks(data, tex.width * 4, tex.height);
+                    encoding = "rgba8";
+                    break;
+            }
+            return new ImageMsg(header, height: (uint)tex.height, width: (uint)tex.width, encoding: encoding, is_bigendian: 0, step: 4, data: data);
+        }
+
+        public static string ToLatLongString(this NavSatFixMsg message)
+        {
+            string lat = (message.latitude > 0) ? "ºN" : "ºS";
+            string lon = (message.longitude > 0) ? "ºE" : "ºW";
+            return $"{message.latitude}{lat} {message.longitude}{lon}";
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageExtensions.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7085dfe7a1d55a644b1cbfa3f0cf6770
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageRegistry.cs

@@ -7,24 +7,32 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
 {
     public static class MessageRegistry
     {
-        static Dictionary<string, Func<MessageDeserializer, Message>> s_DeserializeFunctionsByName = new Dictionary<string, Func<MessageDeserializer, Message>>();
+        static Dictionary<string, Func<MessageDeserializer, Message>>[] s_DeserializeFunctionsByName = new Dictionary<string, Func<MessageDeserializer, Message>>[]{
+            new Dictionary<string, Func<MessageDeserializer, Message>>(), // default
+            new Dictionary<string, Func<MessageDeserializer, Message>>(), // response
+            new Dictionary<string, Func<MessageDeserializer, Message>>(), // goal
+            new Dictionary<string, Func<MessageDeserializer, Message>>(), // feedback
+            new Dictionary<string, Func<MessageDeserializer, Message>>(), // result
+        };
         class RegistryEntry<T>
         {
             public static string s_RosMessageName;
             public static Func<MessageDeserializer, T> s_DeserializeFunction;
         }
 
-        public static void Register<T>(string rosMessageName, Func<MessageDeserializer, T> deserialize) where T : Message
+        public static void Register<T>(string rosMessageName, Func<MessageDeserializer, T> deserialize, MessageSubtopic subtopic = MessageSubtopic.Default) where T : Message
         {
             RegistryEntry<T>.s_RosMessageName = rosMessageName;
             RegistryEntry<T>.s_DeserializeFunction = deserialize;
-            s_DeserializeFunctionsByName[rosMessageName] = deserialize;
+            if (s_DeserializeFunctionsByName[(int)subtopic].ContainsKey(rosMessageName))
+                Debug.LogWarning($"More than one message was registered as \"{rosMessageName}\" \"{subtopic}\"");
+            s_DeserializeFunctionsByName[(int)subtopic][rosMessageName] = deserialize;
         }
 
-        public static Func<MessageDeserializer, Message> GetDeserializeFunction(string rosMessageName)
+        public static Func<MessageDeserializer, Message> GetDeserializeFunction(string rosMessageName, MessageSubtopic subtopic = MessageSubtopic.Default)
         {
             Func<MessageDeserializer, Message> result;
-            s_DeserializeFunctionsByName.TryGetValue(rosMessageName, out result);
+            s_DeserializeFunctionsByName[(int)subtopic].TryGetValue(rosMessageName, out result);
             return result;
         }
 

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSerializer.cs

@@ -71,6 +71,8 @@ namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
             int writeIndex = 0;
             foreach (byte[] statement in m_ListOfSerializations)
             {
+                if (statement == null)
+                    continue;
                 statement.CopyTo(serializedMessage, writeIndex);
                 writeIndex += statement.Length;
             }

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs

@@ -0,0 +1,11 @@
+namespace Unity.Robotics.ROSTCPConnector.MessageGeneration
+{
+    public enum MessageSubtopic
+    {
+        Default = 0,
+        Response = 1,
+        Goal = 2,
+        Feedback = 3,
+        Result = 4,
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/MessageGeneration/MessageSubtopic.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7d6200914621dd54e9c17a6e65308726
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/AddDiagnosticsResponse.cs

@@ -61,7 +61,7 @@ namespace RosMessageTypes.Diagnostic
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Diagnostic/srv/SelfTestResponse.cs

@@ -63,7 +63,7 @@ namespace RosMessageTypes.Diagnostic
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Geometry/msg/Point32Msg.cs

@@ -15,9 +15,9 @@ namespace RosMessageTypes.Geometry
 
         //  This contains the position of a point in free space(with 32 bits of precision).
         //  It is recommended to use Point wherever possible instead of Point32.
-        // 
+        //
         //  This recommendation is to promote interoperability.
-        // 
+        //
         //  This message is designed to take up less space when sending
         //  lots of points at once, as in the case of a PointCloud.
         public float x;

com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/msg/HeaderMsg.cs

@@ -17,7 +17,7 @@ namespace RosMessageTypes.Std
         //  in a particular coordinate frame.
 
 #if !ROS2
-        //  sequence ID: consecutively increasing ID 
+        //  sequence ID: consecutively increasing ID
         public uint seq;
 #endif
 
@@ -84,7 +84,7 @@ namespace RosMessageTypes.Std
             return "Header: " +
 #if !ROS2
             "\nseq: " + seq.ToString() +
-#endif          
+#endif
             "\nstamp: " + stamp.ToString() +
             "\nframe_id: " + frame_id.ToString();
         }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/HandwrittenMessages/srv.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 7616fee199c3ce545a29adf052c60af4
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapAction.cs

@@ -9,6 +9,7 @@ namespace RosMessageTypes.Nav
         public const string k_RosMessageName = "nav_msgs/GetMapAction";
         public override string RosMessageName => k_RosMessageName;
 
+
         public GetMapAction() : base()
         {
             this.action_goal = new GetMapActionGoal();

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionFeedback.cs

@@ -10,6 +10,7 @@ namespace RosMessageTypes.Nav
         public const string k_RosMessageName = "nav_msgs/GetMapActionFeedback";
         public override string RosMessageName => k_RosMessageName;
 
+
         public GetMapActionFeedback() : base()
         {
             this.feedback = new GetMapFeedback();

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionGoal.cs

@@ -10,6 +10,7 @@ namespace RosMessageTypes.Nav
         public const string k_RosMessageName = "nav_msgs/GetMapActionGoal";
         public override string RosMessageName => k_RosMessageName;
 
+
         public GetMapActionGoal() : base()
         {
             this.goal = new GetMapGoal();

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapActionResult.cs

@@ -10,6 +10,7 @@ namespace RosMessageTypes.Nav
         public const string k_RosMessageName = "nav_msgs/GetMapActionResult";
         public override string RosMessageName => k_RosMessageName;
 
+
         public GetMapActionResult() : base()
         {
             this.result = new GetMapResult();

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapFeedback.cs

@@ -40,7 +40,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Feedback);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapGoal.cs

@@ -40,7 +40,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Goal);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/action/GetMapResult.cs

@@ -50,7 +50,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Result);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetMapResponse.cs

@@ -51,7 +51,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/GetPlanResponse.cs

@@ -51,7 +51,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/LoadMapResponse.cs

@@ -63,7 +63,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Nav/srv/SetMapResponse.cs

@@ -51,7 +51,7 @@ namespace RosMessageTypes.Nav
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionFeedback.cs

@@ -40,7 +40,7 @@ namespace RosMessageTypes.ObjectRecognition
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Feedback);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionGoal.cs

@@ -58,7 +58,7 @@ namespace RosMessageTypes.ObjectRecognition
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Goal);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/action/ObjectRecognitionResult.cs

@@ -51,7 +51,7 @@ namespace RosMessageTypes.ObjectRecognition
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Result);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/ObjectRecognition/srv/GetObjectInformationResponse.cs

@@ -51,7 +51,7 @@ namespace RosMessageTypes.ObjectRecognition
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/BoundingBoxQueryResponse.cs

@@ -39,7 +39,7 @@ namespace RosMessageTypes.Octomap
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Octomap/srv/GetOctomapResponse.cs

@@ -50,7 +50,7 @@ namespace RosMessageTypes.Octomap
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: e5962ccc8da3648479afd21afdfb74a5
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 1549a2716409b4ac38a8bbb432fcbcdb
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs

@@ -0,0 +1,60 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.BuiltinInterfaces;
+
+namespace RosMessageTypes.Rosgraph
+{
+    [Serializable]
+    public class ClockMsg : Message
+    {
+        public const string k_RosMessageName = "rosgraph_msgs/Clock";
+        public override string RosMessageName => k_RosMessageName;
+
+        //  roslib/Clock is used for publishing simulated time in ROS. 
+        //  This message simply communicates the current time.
+        //  For more information, see http://www.ros.org/wiki/Clock
+        public TimeMsg clock;
+
+        public ClockMsg()
+        {
+            this.clock = new TimeMsg();
+        }
+
+        public ClockMsg(TimeMsg clock)
+        {
+            this.clock = clock;
+        }
+
+        public static ClockMsg Deserialize(MessageDeserializer deserializer) => new ClockMsg(deserializer);
+
+        private ClockMsg(MessageDeserializer deserializer)
+        {
+            this.clock = TimeMsg.Deserialize(deserializer);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.clock);
+        }
+
+        public override string ToString()
+        {
+            return "ClockMsg: " +
+            "\nclock: " + clock.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/ClockMsg.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7f0e00b0615af4fcc8b1640447e53ed3
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs

@@ -0,0 +1,117 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.Std;
+
+namespace RosMessageTypes.Rosgraph
+{
+    [Serializable]
+    public class LogMsg : Message
+    {
+        public const string k_RosMessageName = "rosgraph_msgs/Log";
+        public override string RosMessageName => k_RosMessageName;
+
+        // #
+        // # Severity level constants
+        // #
+        public const sbyte DEBUG = 1; // debug level
+        public const sbyte INFO = 2; // general level
+        public const sbyte WARN = 4; // warning level
+        public const sbyte ERROR = 8; // error level
+        public const sbyte FATAL = 16; // fatal/critical level
+        // #
+        // # Fields
+        // #
+        public HeaderMsg header;
+        public sbyte level;
+        public string name;
+        //  name of the node
+        public string msg;
+        //  message 
+        public string file;
+        //  file the message came from
+        public string function;
+        //  function the message came from
+        public uint line;
+        //  line the message came from
+        public string[] topics;
+        //  topic names that the node publishes
+
+        public LogMsg()
+        {
+            this.header = new HeaderMsg();
+            this.level = 0;
+            this.name = "";
+            this.msg = "";
+            this.file = "";
+            this.function = "";
+            this.line = 0;
+            this.topics = new string[0];
+        }
+
+        public LogMsg(HeaderMsg header, sbyte level, string name, string msg, string file, string function, uint line, string[] topics)
+        {
+            this.header = header;
+            this.level = level;
+            this.name = name;
+            this.msg = msg;
+            this.file = file;
+            this.function = function;
+            this.line = line;
+            this.topics = topics;
+        }
+
+        public static LogMsg Deserialize(MessageDeserializer deserializer) => new LogMsg(deserializer);
+
+        private LogMsg(MessageDeserializer deserializer)
+        {
+            this.header = HeaderMsg.Deserialize(deserializer);
+            deserializer.Read(out this.level);
+            deserializer.Read(out this.name);
+            deserializer.Read(out this.msg);
+            deserializer.Read(out this.file);
+            deserializer.Read(out this.function);
+            deserializer.Read(out this.line);
+            deserializer.Read(out this.topics, deserializer.ReadLength());
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.header);
+            serializer.Write(this.level);
+            serializer.Write(this.name);
+            serializer.Write(this.msg);
+            serializer.Write(this.file);
+            serializer.Write(this.function);
+            serializer.Write(this.line);
+            serializer.WriteLength(this.topics);
+            serializer.Write(this.topics);
+        }
+
+        public override string ToString()
+        {
+            return "LogMsg: " +
+            "\nheader: " + header.ToString() +
+            "\nlevel: " + level.ToString() +
+            "\nname: " + name.ToString() +
+            "\nmsg: " + msg.ToString() +
+            "\nfile: " + file.ToString() +
+            "\nfunction: " + function.ToString() +
+            "\nline: " + line.ToString() +
+            "\ntopics: " + System.String.Join(", ", topics.ToList());
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/LogMsg.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 1f2a11cd0d2954866b174eff1c2ab43c
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs

@@ -0,0 +1,146 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.BuiltinInterfaces;
+
+namespace RosMessageTypes.Rosgraph
+{
+    [Serializable]
+    public class TopicStatisticsMsg : Message
+    {
+        public const string k_RosMessageName = "rosgraph_msgs/TopicStatistics";
+        public override string RosMessageName => k_RosMessageName;
+
+        //  name of the topic
+        public string topic;
+        //  node id of the publisher
+        public string node_pub;
+        //  node id of the subscriber
+        public string node_sub;
+        //  the statistics apply to this time window
+        public TimeMsg window_start;
+        public TimeMsg window_stop;
+        //  number of messages delivered during the window
+        public int delivered_msgs;
+        //  numbers of messages dropped during the window
+        public int dropped_msgs;
+        //  traffic during the window, in bytes
+        public int traffic;
+        //  mean/stddev/max period between two messages
+        public DurationMsg period_mean;
+        public DurationMsg period_stddev;
+        public DurationMsg period_max;
+        //  mean/stddev/max age of the message based on the
+        //  timestamp in the message header. In case the
+        //  message does not have a header, it will be 0.
+        public DurationMsg stamp_age_mean;
+        public DurationMsg stamp_age_stddev;
+        public DurationMsg stamp_age_max;
+
+        public TopicStatisticsMsg()
+        {
+            this.topic = "";
+            this.node_pub = "";
+            this.node_sub = "";
+            this.window_start = new TimeMsg();
+            this.window_stop = new TimeMsg();
+            this.delivered_msgs = 0;
+            this.dropped_msgs = 0;
+            this.traffic = 0;
+            this.period_mean = new DurationMsg();
+            this.period_stddev = new DurationMsg();
+            this.period_max = new DurationMsg();
+            this.stamp_age_mean = new DurationMsg();
+            this.stamp_age_stddev = new DurationMsg();
+            this.stamp_age_max = new DurationMsg();
+        }
+
+        public TopicStatisticsMsg(string topic, string node_pub, string node_sub, TimeMsg window_start, TimeMsg window_stop, int delivered_msgs, int dropped_msgs, int traffic, DurationMsg period_mean, DurationMsg period_stddev, DurationMsg period_max, DurationMsg stamp_age_mean, DurationMsg stamp_age_stddev, DurationMsg stamp_age_max)
+        {
+            this.topic = topic;
+            this.node_pub = node_pub;
+            this.node_sub = node_sub;
+            this.window_start = window_start;
+            this.window_stop = window_stop;
+            this.delivered_msgs = delivered_msgs;
+            this.dropped_msgs = dropped_msgs;
+            this.traffic = traffic;
+            this.period_mean = period_mean;
+            this.period_stddev = period_stddev;
+            this.period_max = period_max;
+            this.stamp_age_mean = stamp_age_mean;
+            this.stamp_age_stddev = stamp_age_stddev;
+            this.stamp_age_max = stamp_age_max;
+        }
+
+        public static TopicStatisticsMsg Deserialize(MessageDeserializer deserializer) => new TopicStatisticsMsg(deserializer);
+
+        private TopicStatisticsMsg(MessageDeserializer deserializer)
+        {
+            deserializer.Read(out this.topic);
+            deserializer.Read(out this.node_pub);
+            deserializer.Read(out this.node_sub);
+            this.window_start = TimeMsg.Deserialize(deserializer);
+            this.window_stop = TimeMsg.Deserialize(deserializer);
+            deserializer.Read(out this.delivered_msgs);
+            deserializer.Read(out this.dropped_msgs);
+            deserializer.Read(out this.traffic);
+            this.period_mean = DurationMsg.Deserialize(deserializer);
+            this.period_stddev = DurationMsg.Deserialize(deserializer);
+            this.period_max = DurationMsg.Deserialize(deserializer);
+            this.stamp_age_mean = DurationMsg.Deserialize(deserializer);
+            this.stamp_age_stddev = DurationMsg.Deserialize(deserializer);
+            this.stamp_age_max = DurationMsg.Deserialize(deserializer);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.topic);
+            serializer.Write(this.node_pub);
+            serializer.Write(this.node_sub);
+            serializer.Write(this.window_start);
+            serializer.Write(this.window_stop);
+            serializer.Write(this.delivered_msgs);
+            serializer.Write(this.dropped_msgs);
+            serializer.Write(this.traffic);
+            serializer.Write(this.period_mean);
+            serializer.Write(this.period_stddev);
+            serializer.Write(this.period_max);
+            serializer.Write(this.stamp_age_mean);
+            serializer.Write(this.stamp_age_stddev);
+            serializer.Write(this.stamp_age_max);
+        }
+
+        public override string ToString()
+        {
+            return "TopicStatisticsMsg: " +
+            "\ntopic: " + topic.ToString() +
+            "\nnode_pub: " + node_pub.ToString() +
+            "\nnode_sub: " + node_sub.ToString() +
+            "\nwindow_start: " + window_start.ToString() +
+            "\nwindow_stop: " + window_stop.ToString() +
+            "\ndelivered_msgs: " + delivered_msgs.ToString() +
+            "\ndropped_msgs: " + dropped_msgs.ToString() +
+            "\ntraffic: " + traffic.ToString() +
+            "\nperiod_mean: " + period_mean.ToString() +
+            "\nperiod_stddev: " + period_stddev.ToString() +
+            "\nperiod_max: " + period_max.ToString() +
+            "\nstamp_age_mean: " + stamp_age_mean.ToString() +
+            "\nstamp_age_stddev: " + stamp_age_stddev.ToString() +
+            "\nstamp_age_max: " + stamp_age_max.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Rosgraph/msg/TopicStatisticsMsg.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 9c0a4e1f9f89e4ef0af5bb52dfcf6b74
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/msg/ImuMsg.cs

@@ -14,14 +14,14 @@ namespace RosMessageTypes.Sensor
         public override string RosMessageName => k_RosMessageName;
 
         //  This is a message to hold data from an IMU (Inertial Measurement Unit)
-        // 
+        //
         //  Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
-        // 
+        //
         //  If the covariance of the measurement is known, it should be filled in (if all you know is the
         //  variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
         //  A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
         //  data a covariance will have to be assumed or gotten from some other source
-        // 
+        //
         //  If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an
         //  orientation estimate), please set element 0 of the associated covariance matrix to -1
         //  If you are interpreting this message, please check for a value of -1 in the first element of each

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Sensor/srv/SetCameraInfoResponse.cs

@@ -58,7 +58,7 @@ namespace RosMessageTypes.Sensor
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/EmptyResponse.cs

@@ -39,7 +39,7 @@ namespace RosMessageTypes.Std
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/SetBoolResponse.cs

@@ -58,7 +58,7 @@ namespace RosMessageTypes.Std
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Std/srv/TriggerResponse.cs

@@ -58,7 +58,7 @@ namespace RosMessageTypes.Std
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: a5e8e35917a9e0949a00177203401e5e
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 1dbabffde68444d8ba7a9fc06b3730f4
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs

@@ -0,0 +1,37 @@
+using System.Collections.Generic;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+
+namespace RosMessageTypes.Tf2
+{
+    public class LookupTransformAction : Action<LookupTransformActionGoal, LookupTransformActionResult, LookupTransformActionFeedback, LookupTransformGoal, LookupTransformResult, LookupTransformFeedback>
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransformAction";
+        public override string RosMessageName => k_RosMessageName;
+
+
+        public LookupTransformAction() : base()
+        {
+            this.action_goal = new LookupTransformActionGoal();
+            this.action_result = new LookupTransformActionResult();
+            this.action_feedback = new LookupTransformActionFeedback();
+        }
+
+        public static LookupTransformAction Deserialize(MessageDeserializer deserializer) => new LookupTransformAction(deserializer);
+
+        LookupTransformAction(MessageDeserializer deserializer)
+        {
+            this.action_goal = LookupTransformActionGoal.Deserialize(deserializer);
+            this.action_result = LookupTransformActionResult.Deserialize(deserializer);
+            this.action_feedback = LookupTransformActionFeedback.Deserialize(deserializer);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.action_goal);
+            serializer.Write(this.action_result);
+            serializer.Write(this.action_feedback);
+        }
+
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformAction.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: dc5f7f906e8ff4450945b23dfaf6c658
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs

@@ -0,0 +1,37 @@
+using System.Collections.Generic;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.Std;
+using RosMessageTypes.Actionlib;
+
+namespace RosMessageTypes.Tf2
+{
+    public class LookupTransformActionFeedback : ActionFeedback<LookupTransformFeedback>
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransformActionFeedback";
+        public override string RosMessageName => k_RosMessageName;
+
+
+        public LookupTransformActionFeedback() : base()
+        {
+            this.feedback = new LookupTransformFeedback();
+        }
+
+        public LookupTransformActionFeedback(HeaderMsg header, GoalStatusMsg status, LookupTransformFeedback feedback) : base(header, status)
+        {
+            this.feedback = feedback;
+        }
+        public static LookupTransformActionFeedback Deserialize(MessageDeserializer deserializer) => new LookupTransformActionFeedback(deserializer);
+
+        LookupTransformActionFeedback(MessageDeserializer deserializer) : base(deserializer)
+        {
+            this.feedback = LookupTransformFeedback.Deserialize(deserializer);
+        }
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.header);
+            serializer.Write(this.status);
+            serializer.Write(this.feedback);
+        }
+
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionFeedback.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e2c6074f2dd51401ea42e73f07b17420
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs

@@ -0,0 +1,37 @@
+using System.Collections.Generic;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.Std;
+using RosMessageTypes.Actionlib;
+
+namespace RosMessageTypes.Tf2
+{
+    public class LookupTransformActionGoal : ActionGoal<LookupTransformGoal>
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransformActionGoal";
+        public override string RosMessageName => k_RosMessageName;
+
+
+        public LookupTransformActionGoal() : base()
+        {
+            this.goal = new LookupTransformGoal();
+        }
+
+        public LookupTransformActionGoal(HeaderMsg header, GoalIDMsg goal_id, LookupTransformGoal goal) : base(header, goal_id)
+        {
+            this.goal = goal;
+        }
+        public static LookupTransformActionGoal Deserialize(MessageDeserializer deserializer) => new LookupTransformActionGoal(deserializer);
+
+        LookupTransformActionGoal(MessageDeserializer deserializer) : base(deserializer)
+        {
+            this.goal = LookupTransformGoal.Deserialize(deserializer);
+        }
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.header);
+            serializer.Write(this.goal_id);
+            serializer.Write(this.goal);
+        }
+
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionGoal.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: c978cedffbc704ce2be299d52ecd58c0
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs

@@ -0,0 +1,37 @@
+using System.Collections.Generic;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.Std;
+using RosMessageTypes.Actionlib;
+
+namespace RosMessageTypes.Tf2
+{
+    public class LookupTransformActionResult : ActionResult<LookupTransformResult>
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransformActionResult";
+        public override string RosMessageName => k_RosMessageName;
+
+
+        public LookupTransformActionResult() : base()
+        {
+            this.result = new LookupTransformResult();
+        }
+
+        public LookupTransformActionResult(HeaderMsg header, GoalStatusMsg status, LookupTransformResult result) : base(header, status)
+        {
+            this.result = result;
+        }
+        public static LookupTransformActionResult Deserialize(MessageDeserializer deserializer) => new LookupTransformActionResult(deserializer);
+
+        LookupTransformActionResult(MessageDeserializer deserializer) : base(deserializer)
+        {
+            this.result = LookupTransformResult.Deserialize(deserializer);
+        }
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.header);
+            serializer.Write(this.status);
+            serializer.Write(this.result);
+        }
+
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformActionResult.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e20032c52a2f2439384f243b410c1e38
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs

@@ -0,0 +1,45 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class LookupTransformFeedback : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransform";
+        public override string RosMessageName => k_RosMessageName;
+
+
+        public LookupTransformFeedback()
+        {
+        }
+        public static LookupTransformFeedback Deserialize(MessageDeserializer deserializer) => new LookupTransformFeedback(deserializer);
+
+        private LookupTransformFeedback(MessageDeserializer deserializer)
+        {
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+        }
+
+        public override string ToString()
+        {
+            return "LookupTransformFeedback: ";
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Feedback);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformFeedback.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a0732d6f56146447eb7aff397d53c66d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs

@@ -0,0 +1,96 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using RosMessageTypes.BuiltinInterfaces;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class LookupTransformGoal : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransform";
+        public override string RosMessageName => k_RosMessageName;
+
+        // Simple API
+        public string target_frame;
+        public string source_frame;
+        public TimeMsg source_time;
+        public DurationMsg timeout;
+        // Advanced API
+        public TimeMsg target_time;
+        public string fixed_frame;
+        // Whether or not to use the advanced API
+        public bool advanced;
+
+        public LookupTransformGoal()
+        {
+            this.target_frame = "";
+            this.source_frame = "";
+            this.source_time = new TimeMsg();
+            this.timeout = new DurationMsg();
+            this.target_time = new TimeMsg();
+            this.fixed_frame = "";
+            this.advanced = false;
+        }
+
+        public LookupTransformGoal(string target_frame, string source_frame, TimeMsg source_time, DurationMsg timeout, TimeMsg target_time, string fixed_frame, bool advanced)
+        {
+            this.target_frame = target_frame;
+            this.source_frame = source_frame;
+            this.source_time = source_time;
+            this.timeout = timeout;
+            this.target_time = target_time;
+            this.fixed_frame = fixed_frame;
+            this.advanced = advanced;
+        }
+
+        public static LookupTransformGoal Deserialize(MessageDeserializer deserializer) => new LookupTransformGoal(deserializer);
+
+        private LookupTransformGoal(MessageDeserializer deserializer)
+        {
+            deserializer.Read(out this.target_frame);
+            deserializer.Read(out this.source_frame);
+            this.source_time = TimeMsg.Deserialize(deserializer);
+            this.timeout = DurationMsg.Deserialize(deserializer);
+            this.target_time = TimeMsg.Deserialize(deserializer);
+            deserializer.Read(out this.fixed_frame);
+            deserializer.Read(out this.advanced);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.target_frame);
+            serializer.Write(this.source_frame);
+            serializer.Write(this.source_time);
+            serializer.Write(this.timeout);
+            serializer.Write(this.target_time);
+            serializer.Write(this.fixed_frame);
+            serializer.Write(this.advanced);
+        }
+
+        public override string ToString()
+        {
+            return "LookupTransformGoal: " +
+            "\ntarget_frame: " + target_frame.ToString() +
+            "\nsource_frame: " + source_frame.ToString() +
+            "\nsource_time: " + source_time.ToString() +
+            "\ntimeout: " + timeout.ToString() +
+            "\ntarget_time: " + target_time.ToString() +
+            "\nfixed_frame: " + fixed_frame.ToString() +
+            "\nadvanced: " + advanced.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Goal);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformGoal.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: dbdd5b79defed454a927d42f2828103f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs

@@ -0,0 +1,62 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class LookupTransformResult : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/LookupTransform";
+        public override string RosMessageName => k_RosMessageName;
+
+        public Geometry.TransformStampedMsg transform;
+        public TF2ErrorMsg error;
+
+        public LookupTransformResult()
+        {
+            this.transform = new Geometry.TransformStampedMsg();
+            this.error = new TF2ErrorMsg();
+        }
+
+        public LookupTransformResult(Geometry.TransformStampedMsg transform, TF2ErrorMsg error)
+        {
+            this.transform = transform;
+            this.error = error;
+        }
+
+        public static LookupTransformResult Deserialize(MessageDeserializer deserializer) => new LookupTransformResult(deserializer);
+
+        private LookupTransformResult(MessageDeserializer deserializer)
+        {
+            this.transform = Geometry.TransformStampedMsg.Deserialize(deserializer);
+            this.error = TF2ErrorMsg.Deserialize(deserializer);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.transform);
+            serializer.Write(this.error);
+        }
+
+        public override string ToString()
+        {
+            return "LookupTransformResult: " +
+            "\ntransform: " + transform.ToString() +
+            "\nerror: " + error.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Result);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/action/LookupTransformResult.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 5f645ff91c32c4059aebd7e2444407f8
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: dce99d42918c044d9b04c887e1d5e42b
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs

@@ -0,0 +1,69 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class TF2ErrorMsg : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/TF2Error";
+        public override string RosMessageName => k_RosMessageName;
+
+        public const byte NO_ERROR = 0;
+        public const byte LOOKUP_ERROR = 1;
+        public const byte CONNECTIVITY_ERROR = 2;
+        public const byte EXTRAPOLATION_ERROR = 3;
+        public const byte INVALID_ARGUMENT_ERROR = 4;
+        public const byte TIMEOUT_ERROR = 5;
+        public const byte TRANSFORM_ERROR = 6;
+        public byte error;
+        public string error_string;
+
+        public TF2ErrorMsg()
+        {
+            this.error = 0;
+            this.error_string = "";
+        }
+
+        public TF2ErrorMsg(byte error, string error_string)
+        {
+            this.error = error;
+            this.error_string = error_string;
+        }
+
+        public static TF2ErrorMsg Deserialize(MessageDeserializer deserializer) => new TF2ErrorMsg(deserializer);
+
+        private TF2ErrorMsg(MessageDeserializer deserializer)
+        {
+            deserializer.Read(out this.error);
+            deserializer.Read(out this.error_string);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.error);
+            serializer.Write(this.error_string);
+        }
+
+        public override string ToString()
+        {
+            return "TF2ErrorMsg: " +
+            "\nerror: " + error.ToString() +
+            "\nerror_string: " + error_string.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TF2ErrorMsg.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 48281dc85fc2b4d67bfc4df260db0089
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs

@@ -0,0 +1,57 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class TFMessageMsg : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/TFMessage";
+        public override string RosMessageName => k_RosMessageName;
+
+        public Geometry.TransformStampedMsg[] transforms;
+
+        public TFMessageMsg()
+        {
+            this.transforms = new Geometry.TransformStampedMsg[0];
+        }
+
+        public TFMessageMsg(Geometry.TransformStampedMsg[] transforms)
+        {
+            this.transforms = transforms;
+        }
+
+        public static TFMessageMsg Deserialize(MessageDeserializer deserializer) => new TFMessageMsg(deserializer);
+
+        private TFMessageMsg(MessageDeserializer deserializer)
+        {
+            deserializer.Read(out this.transforms, Geometry.TransformStampedMsg.Deserialize, deserializer.ReadLength());
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.WriteLength(this.transforms);
+            serializer.Write(this.transforms);
+        }
+
+        public override string ToString()
+        {
+            return "TFMessageMsg: " +
+            "\ntransforms: " + System.String.Join(", ", transforms.ToList());
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/msg/TFMessageMsg.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 8ccdd0a385d6a4f7bb1b7d5b855a3fbb
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: b1af9364b9b8d4d359a3bfd9db3ffd0d
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs

@@ -0,0 +1,45 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class FrameGraphRequest : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/FrameGraph";
+        public override string RosMessageName => k_RosMessageName;
+
+
+        public FrameGraphRequest()
+        {
+        }
+        public static FrameGraphRequest Deserialize(MessageDeserializer deserializer) => new FrameGraphRequest(deserializer);
+
+        private FrameGraphRequest(MessageDeserializer deserializer)
+        {
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+        }
+
+        public override string ToString()
+        {
+            return "FrameGraphRequest: ";
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphRequest.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 612710bb2e09848eaa17623543cd3bae
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs

@@ -0,0 +1,56 @@
+//Do not edit! This file was generated by Unity-ROS MessageGeneration.
+using System;
+using System.Linq;
+using System.Collections.Generic;
+using System.Text;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+
+namespace RosMessageTypes.Tf2
+{
+    [Serializable]
+    public class FrameGraphResponse : Message
+    {
+        public const string k_RosMessageName = "tf2_msgs/FrameGraph";
+        public override string RosMessageName => k_RosMessageName;
+
+        public string frame_yaml;
+
+        public FrameGraphResponse()
+        {
+            this.frame_yaml = "";
+        }
+
+        public FrameGraphResponse(string frame_yaml)
+        {
+            this.frame_yaml = frame_yaml;
+        }
+
+        public static FrameGraphResponse Deserialize(MessageDeserializer deserializer) => new FrameGraphResponse(deserializer);
+
+        private FrameGraphResponse(MessageDeserializer deserializer)
+        {
+            deserializer.Read(out this.frame_yaml);
+        }
+
+        public override void SerializeTo(MessageSerializer serializer)
+        {
+            serializer.Write(this.frame_yaml);
+        }
+
+        public override string ToString()
+        {
+            return "FrameGraphResponse: " +
+            "\nframe_yaml: " + frame_yaml.ToString();
+        }
+
+#if UNITY_EDITOR
+        [UnityEditor.InitializeOnLoadMethod]
+#else
+        [UnityEngine.RuntimeInitializeOnLoadMethod]
+#endif
+        public static void Register()
+        {
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Tf2/srv/FrameGraphResponse.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 7da27b37dd62f4f2ea969cd3ec68dfdb
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/Messages/Visualization/srv/GetInteractiveMarkersResponse.cs

@@ -62,7 +62,7 @@ namespace RosMessageTypes.Visualization
 #endif
         public static void Register()
         {
-            MessageRegistry.Register(k_RosMessageName, Deserialize);
+            MessageRegistry.Register(k_RosMessageName, Deserialize, MessageSubtopic.Response);
         }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HUDPanel.cs

@@ -1,167 +1,39 @@
 using System;
 using System.Collections.Generic;
+using System.IO;
 using Unity.Robotics.ROSTCPConnector.MessageGeneration;
 using UnityEngine;
 
 namespace Unity.Robotics.ROSTCPConnector
 {
-    public class HUDPanel : MonoBehaviour
+    public interface IHudTab
     {
-        // GUI variables
-        GUIStyle m_LabelStyle;
-        GUIStyle m_ConnectionArrowStyle;
-        GUIStyle m_ContentStyle;
-        GUIStyle m_MessageStyle;
-        bool m_ViewSent = false;
-        bool m_ViewRecv = false;
-        bool m_ViewSrvs = false;
-        Rect m_ScrollRect;
+        string Label { get; }
+        void OnGUI(HudPanel hud);
+        void OnSelected();
+        void OnDeselected();
+    }
+
+    public class HudPanel : MonoBehaviour
+    {
+        public static GUIStyle s_BoldStyle;
+
+        // these are static so that anyone can register tabs and windows without needing to worry about whether the hud has been initialized
+        static SortedList<int, IHudTab> s_HUDTabs = new SortedList<int, IHudTab>();
+        static SortedList<int, Action> s_HeaderContents = new SortedList<int, Action>();
+        static List<HudWindow> s_ActiveWindows = new List<HudWindow>();
+        static int s_NextWindowID = 101;
 
         // ROS Message variables
         internal bool isEnabled;
-        internal string host;
 
-        MessageViewState m_LastMessageSent;
-        string m_LastMessageSentMeta = "None";
-        float m_LastMessageSentRealtime;
+        HudWindow m_DraggingWindow;
 
-        // For the Hud's IP address field, we store the IP address and port in PlayerPrefs.
-        // This is used to remember the last IP address the player typed into the HUD, in builds where ConnectOnStart is not checked
-        public const string PlayerPrefsKey_ROS_IP = "ROS_IP";
-        public const string PlayerPrefsKey_ROS_TCP_PORT = "ROS_TCP_PORT";
-
-        public static string RosIPAddressPref
-        {
-            get => PlayerPrefs.GetString(PlayerPrefsKey_ROS_IP, "127.0.0.1");
-        }
-
-        public static int RosPortPref
-        {
-            get => PlayerPrefs.GetInt(PlayerPrefsKey_ROS_TCP_PORT, 10000);
-        }
-
-        public static void SetIPPref(string ipAddress)
-        {
-            PlayerPrefs.SetString(PlayerPrefsKey_ROS_IP, ipAddress);
-        }
-
-        public static void SetPortPref(int port)
-        {
-            PlayerPrefs.SetInt(PlayerPrefsKey_ROS_TCP_PORT, port);
-        }
-
-
-        public void SetLastMessageSent(string topic, Message message)
-        {
-            m_LastMessageSent = new MessageViewState() { label = "Last Message Sent:", message = message };
-            m_LastMessageSentMeta = $"{topic} (time: {System.DateTime.Now.TimeOfDay})";
-            m_LastMessageSentRealtime = ROSConnection.s_RealTimeSinceStartup;
-        }
-
-        MessageViewState m_LastMessageReceived;
-        string m_LastMessageReceivedMeta = "None";
-        float m_LastMessageReceivedRealtime;
-
-        public void SetLastMessageReceived(string topic, Message message)
-        {
-            m_LastMessageReceived = new MessageViewState() { label = "Last Message Received:", message = message };
-            m_LastMessageReceivedMeta = $"{topic} (time: {System.DateTime.Now.TimeOfDay})";
-            m_LastMessageReceivedRealtime = Time.realtimeSinceStartup;
-        }
-
-        List<MessageViewState> activeServices = new List<MessageViewState>();
-        MessageViewState lastCompletedServiceRequest = null;
-        MessageViewState lastCompletedServiceResponse = null;
-        int nextServiceID = 101;
-
-        public int AddServiceRequest(string topic, Message request)
-        {
-            int serviceID = nextServiceID;
-            nextServiceID++;
-
-            activeServices.Add(new MessageViewState()
-            {
-                serviceID = serviceID,
-                timestamp = Time.time,
-                topic = topic,
-                message = request,
-                label = $"{topic} Service Requested",
-            });
-
-            return serviceID;
-        }
-
-        public void AddServiceResponse(int serviceID, Message response)
-        {
-            lastCompletedServiceRequest = activeServices.Find(s => s.serviceID == serviceID);
-            activeServices.Remove(lastCompletedServiceRequest);
-
-            lastCompletedServiceResponse = new MessageViewState()
-            {
-                serviceID = serviceID,
-                timestamp = Time.time,
-                topic = lastCompletedServiceRequest.topic,
-                message = response,
-                label = $"{lastCompletedServiceRequest.topic} Service Response",
-            };
-        }
+        IHudTab m_SelectedTab;
 
         void Awake()
         {
-            // Define font styles
-            m_LabelStyle = new GUIStyle
-            {
-                alignment = TextAnchor.MiddleLeft,
-                normal = { textColor = Color.white },
-                fontStyle = FontStyle.Bold,
-                fixedWidth = 250
-            };
-
-            m_ConnectionArrowStyle = new GUIStyle
-            {
-                alignment = TextAnchor.MiddleLeft,
-                normal = { textColor = Color.white },
-                fontSize = 22,
-                fontStyle = FontStyle.Bold,
-                fixedWidth = 250
-            };
-
-            m_ContentStyle = new GUIStyle
-            {
-                alignment = TextAnchor.MiddleLeft,
-                padding = new RectOffset(10, 0, 0, 5),
-                normal = { textColor = Color.white },
-                fixedWidth = 300
-            };
-
-            m_MessageStyle = new GUIStyle
-            {
-                alignment = TextAnchor.MiddleLeft,
-                padding = new RectOffset(10, 0, 5, 5),
-                normal = { textColor = Color.white },
-                fixedWidth = 300,
-                wordWrap = true
-            };
-
-            m_ScrollRect = new Rect();
-        }
-
-        Color GetConnectionColor(float elapsedTime)
-        {
-            var bright = new Color(1, 1, 0.5f);
-            var mid = new Color(0, 1, 1);
-            var dark = new Color(0, 0.5f, 1);
-            const float brightDuration = 0.03f;
-            const float fadeToDarkDuration = 1.0f;
-
-            if (!ROSConnection.instance.HasConnectionThread)
-                return Color.gray;
-            if (ROSConnection.instance.HasConnectionError)
-                return Color.red;
-            if (elapsedTime <= brightDuration)
-                return bright;
-            else
-                return Color.Lerp(mid, dark, elapsedTime / fadeToDarkDuration);
+            InitStyles();
         }
 
         void OnGUI()
@@ -172,153 +44,184 @@ namespace Unity.Robotics.ROSTCPConnector
             // Initialize main HUD
             GUILayout.BeginVertical(GUI.skin.box, GUILayout.Width(300));
 
-            // ROS IP Setup
+            foreach (Action element in s_HeaderContents.Values)
+            {
+                element();
+            }
+
             GUILayout.BeginHorizontal();
-            Color baseColor = GUI.color;
-            GUI.color = Color.white;
-            GUI.Label(new Rect(4, 5, 25, 15), "I", m_ConnectionArrowStyle);
-            GUI.color = GetConnectionColor(Time.realtimeSinceStartup - m_LastMessageReceivedRealtime);
-            GUI.Label(new Rect(8, 6, 25, 15), "\u2190", m_ConnectionArrowStyle);
-            GUI.color = GetConnectionColor(Time.realtimeSinceStartup - m_LastMessageSentRealtime);
-            GUI.Label(new Rect(8, 0, 25, 15), "\u2192", m_ConnectionArrowStyle);
-            GUI.color = baseColor;
 
-#if ROS2
-            string protocolName = "ROS2";
-#else
-            string protocolName = "ROS";
+            foreach (IHudTab tab in s_HUDTabs.Values)
+            {
+                var wasSelected = tab == m_SelectedTab;
+                var selected = GUILayout.Toggle(wasSelected, tab.Label, GUI.skin.button);
+                if (selected != wasSelected)
+                {
+                    if (m_SelectedTab != null)
+                        m_SelectedTab.OnDeselected();
+
+                    m_SelectedTab = selected ? tab : null;
+
+                    if (m_SelectedTab != null)
+                        m_SelectedTab.OnSelected();
+                }
+            }
+
+            GUILayout.EndHorizontal();
+
+            if (m_SelectedTab != null)
+                m_SelectedTab.OnGUI(this);
+
+            GUILayout.EndVertical();
+
+            // Draggable windows
+            var current = Event.current;
+            if (current.type == EventType.MouseDown)
+            {
+                for (var Idx = s_ActiveWindows.Count - 1; Idx >= 0; --Idx)
+                {
+                    var window = s_ActiveWindows[Idx];
+                    if (s_ActiveWindows[Idx].TryDragWindow(current))
+                    {
+                        m_DraggingWindow = s_ActiveWindows[Idx];
+                        break;
+                    }
+                }
+            }
+            else if (current.type == EventType.MouseDrag && m_DraggingWindow != null)
+            {
+                m_DraggingWindow.UpdateDragWindow(current);
+            }
+            else if (current.type == EventType.MouseUp && m_DraggingWindow != null)
+            {
+                m_DraggingWindow.EndDragWindow();
+                m_DraggingWindow = null;
+            }
+
+            foreach (var window in s_ActiveWindows)
+                window.DrawWindow(this);
+        }
+
+        public static void RegisterTab(IHudTab tab, int index = 0)
+        {
+            if (s_HUDTabs.ContainsKey(index))
+            {
+                Debug.LogWarning($"HUDPanel already contains a tab registered at index {index}! Registering at index {s_HUDTabs.Count} instead.");
+                index = s_HUDTabs.Count;
+            }
+
+            s_HUDTabs.Add(index, tab);
+        }
+
+        public static void RegisterHeader(Action headerContent, int index = 0)
+        {
+            if (s_HeaderContents.ContainsKey(index))
+            {
+                Debug.LogWarning($"HUDPanel already contains a header registered at index {index}! Registering at index {s_HeaderContents.Count} instead.");
+                index = s_HeaderContents.Count;
+            }
+
+            s_HeaderContents.Add(index, headerContent);
+        }
+
+        public static void AddWindow(HudWindow window)
+        {
+            s_ActiveWindows.Add(window);
+        }
+
+        public static void RemoveWindow(HudWindow window)
+        {
+            s_ActiveWindows.Remove(window);
+        }
+
+        public static int GetNextWindowID()
+        {
+            var result = s_NextWindowID;
+            s_NextWindowID++;
+            return result;
+        }
+
+        void InitStyles()
+        {
+            // Define font styles
+            s_BoldStyle = new GUIStyle
+            {
+                alignment = TextAnchor.MiddleLeft,
+                normal = { textColor = Color.white },
+                fontStyle = FontStyle.Bold,
+            };
+        }
+        public static Rect GetDefaultWindowRect()
+        {
+            return new Rect(450, 0, 300, 200);
+        }
+
+        public Rect GetFreeWindowRect()
+        {
+            var xQueue = new Queue<Rect>();
+            var yQueue = new Queue<Rect>();
+            yQueue.Enqueue(GetDefaultWindowRect());
+
+            while (yQueue.Count > 0 || xQueue.Count > 0)
+            {
+                var testRect = xQueue.Count > 0 ? xQueue.Dequeue() : yQueue.Dequeue();
+                if (testRect.xMax > Screen.width || testRect.yMax > Screen.height)
+                    continue;
+
+                float maxX, maxY;
+                if (IsFreeWindowRect(testRect, out maxX, out maxY))
+                    return testRect;
+
+                xQueue.Enqueue(new Rect(maxX, testRect.y, testRect.width, testRect.height));
+                yQueue.Enqueue(new Rect(testRect.x, maxY, testRect.width, testRect.height));
+            }
+
+            return GetDefaultWindowRect();
+        }
+
+        public bool IsFreeWindowRect(Rect rect)
+        {
+            foreach (var window in s_ActiveWindows)
+                if (window.WindowRect.Overlaps(rect))
+                    return false;
+            return true;
+        }
+
+        public bool IsFreeWindowRect(Rect rect, out float maxX, out float maxY)
+        {
+            maxX = 0;
+            maxY = 0;
+            var result = true;
+            foreach (var window in s_ActiveWindows)
+            {
+                if (window.WindowRect.Overlaps(rect))
+                {
+                    maxX = Mathf.Max(maxX, window.WindowRect.xMax);
+                    maxY = Mathf.Max(maxY, window.WindowRect.yMax);
+                    result = false;
+                }
+            }
+
+            return result;
+        }
+
+#if UNITY_EDITOR
+        // automatically clear all static content on pressing play
+        [UnityEditor.InitializeOnLoadMethod]
+        static void InitializeOnLoad()
+        {
+            UnityEditor.EditorApplication.playModeStateChanged += OnPlayMode;
+        }
+
+        static void OnPlayMode(UnityEditor.PlayModeStateChange change)
+        {
+            if (change == UnityEditor.PlayModeStateChange.ExitingEditMode)
+            {
+                s_HUDTabs.Clear();
+                s_HeaderContents.Clear();
+                s_ActiveWindows.Clear();
+            }
+        }
 #endif
-
-            GUILayout.Space(30);
-            GUILayout.Label($"{protocolName} IP: ", m_LabelStyle, GUILayout.Width(100));
-
-            if (!ROSConnection.instance.HasConnectionThread)
-            {
-                // if you've never run a build on this machine before, initialize the playerpref settings to the ones from the RosConnection
-                if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_IP))
-                    SetIPPref(ROSConnection.instance.RosIPAddress);
-                if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_TCP_PORT))
-                    SetPortPref(ROSConnection.instance.RosPort);
-
-                // NB, here the user is editing the PlayerPrefs values, not the ones in the RosConnection.
-                // (So that the hud remembers what IP you used last time you ran this build.)
-                // The RosConnection receives the edited values when you click Connect.
-                SetIPPref(GUILayout.TextField(RosIPAddressPref));
-                SetPortPref(Convert.ToInt32(GUILayout.TextField(RosPortPref.ToString())));
-
-                GUILayout.EndHorizontal();
-                GUILayout.Label("(Not connected)");
-                if (GUILayout.Button("Connect"))
-                    ROSConnection.instance.Connect(RosIPAddressPref, RosPortPref);
-            }
-            else
-            {
-                GUILayout.Label(host, m_ContentStyle);
-                GUILayout.EndHorizontal();
-            }
-
-            // Last message sent
-            GUILayout.Label("Last Message Sent:", m_LabelStyle);
-            GUILayout.Label(m_LastMessageSentMeta, m_ContentStyle);
-            if (m_LastMessageSent != null)
-                m_ViewSent = GUILayout.Toggle(m_ViewSent, "View contents");
-
-            // Last message received
-            GUILayout.Label("Last Message Received:", m_LabelStyle);
-            GUILayout.Label(m_LastMessageReceivedMeta, m_ContentStyle);
-            if (m_LastMessageReceived != null)
-                m_ViewRecv = GUILayout.Toggle(m_ViewRecv, "View contents");
-
-            GUILayout.Label($"{activeServices.Count} Active Service Requests:", m_LabelStyle);
-            if (activeServices.Count > 0)
-            {
-                var dots = new String('.', (int)Time.time % 4);
-                GUILayout.Label($"Waiting for service response{dots}", m_ContentStyle);
-            }
-
-            m_ViewSrvs = GUILayout.Toggle(m_ViewSrvs, "View services status");
-
-            GUILayout.EndVertical();
-
-            // Update length of scroll
-            if (GUILayoutUtility.GetLastRect().height > 1 && GUILayoutUtility.GetLastRect().width > 1)
-                m_ScrollRect = GUILayoutUtility.GetLastRect();
-
-            // Optionally show message contents
-            float y = m_ScrollRect.yMax;
-            if (m_ViewSent)
-            {
-                y = ShowMessage(m_LastMessageSent, y);
-            }
-
-            if (m_ViewRecv)
-            {
-                y = ShowMessage(m_LastMessageReceived, y);
-            }
-
-            if (m_ViewSrvs)
-            {
-                foreach (MessageViewState service in activeServices)
-                {
-                    y = ShowMessage(service, y, showElapsedTime: true);
-                }
-
-                if (lastCompletedServiceRequest != null && lastCompletedServiceResponse != null)
-                {
-                    y = ShowMessage(lastCompletedServiceRequest, y);
-                    y = ShowMessage(lastCompletedServiceResponse, y);
-                }
-            }
-        }
-
-        /// <summary>
-        /// All the information necessary to display a message and remember its scroll position
-        /// </summary>
-        class MessageViewState
-        {
-            public string label;
-            public int serviceID;
-            public float timestamp;
-            public string topic;
-            public Message message;
-            public Rect contentRect;
-            public Vector2 scrollPosition;
-        }
-
-        /// <summary>
-        /// Displays a MessageViewState
-        /// </summary>
-        /// <param name="msgView">The message view to draw</param>
-        /// <param name="y">The Y position to draw at</param>
-        /// <param name="showElapsedTime">Whether to add elapsed time to the title</param>
-        /// <returns>The new Y position to draw at</returns>
-        float ShowMessage(MessageViewState msgView, float y, bool showElapsedTime = false)
-        {
-            if (msgView == null)
-                return y;
-
-            // Start scrollviews
-            float height = msgView.contentRect.height > 0 ? Mathf.Min(msgView.contentRect.height, 200) : 200;
-            Rect panelRect = new Rect(0, y + 5, 325, height);
-            msgView.scrollPosition = GUI.BeginScrollView(panelRect, msgView.scrollPosition, msgView.contentRect);
-
-            GUILayout.BeginVertical("box");
-
-            // Paste contents of message
-            if (showElapsedTime)
-                GUILayout.Label($"{msgView.label} ({Time.time - msgView.timestamp})", m_LabelStyle);
-            else
-                GUILayout.Label(msgView.label, m_LabelStyle);
-            GUILayout.Label(msgView.message.ToString(), m_MessageStyle);
-
-            GUILayout.EndVertical();
-            GUI.EndScrollView();
-
-            // Update size of internal rect view
-            if (GUILayoutUtility.GetLastRect().height > 1 && GUILayoutUtility.GetLastRect().width > 1)
-                msgView.contentRect = GUILayoutUtility.GetLastRect();
-
-            return panelRect.yMax;
-        }
     }
 }

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs

@@ -0,0 +1,159 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector
+{
+    public interface IMenuFiller
+    {
+        void AddItem(GUIContent name, bool selected, System.Action callback);
+    }
+
+    public class HudWindow
+    {
+        const float c_DraggableSize = 8;
+        Rect m_WindowRect;
+        public Rect WindowRect => m_WindowRect;
+        string m_Title;
+        bool m_IsActive;
+        public bool IsActive => m_IsActive;
+        bool m_AutoLayout;
+        bool m_DraggingBottom;
+        bool m_DraggingLeft;
+        bool m_DraggingRight;
+        bool m_DraggingTitle;
+        Vector2 m_DragMouseOffset;
+        Vector2 m_ContentScrollPosition;
+        System.Action m_GuiDrawer;
+        System.Action<IMenuFiller> m_GuiMenu;
+        int m_WindowID;
+
+        public HudWindow(string title, Rect rect)
+        {
+            m_Title = title;
+            m_WindowID = HudPanel.GetNextWindowID();
+            m_IsActive = true;
+            m_AutoLayout = false;
+            m_WindowRect = rect;
+        }
+
+        public HudWindow(string title)
+        {
+            m_Title = title;
+            m_WindowID = HudPanel.GetNextWindowID();
+            m_IsActive = true;
+            m_AutoLayout = true;
+        }
+
+        public void SetActive(bool active)
+        {
+            m_IsActive = active;
+        }
+
+        public bool HasActiveRect => m_IsActive && !m_AutoLayout;
+
+        public void SetOnGUI(Action guiDrawer)
+        {
+            m_GuiDrawer = guiDrawer;
+        }
+
+        public void SetGUIMenu(Action<IMenuFiller> guiMenu)
+        {
+            m_GuiMenu = guiMenu;
+        }
+
+        public void DrawWindow(HudPanel hud)
+        {
+            if (m_IsActive && m_GuiDrawer != null)
+            {
+                if (m_AutoLayout)
+                {
+                    m_WindowRect = hud.GetFreeWindowRect();
+                    m_AutoLayout = false;
+                }
+                GUI.Window(m_WindowID, m_WindowRect, DrawWindowContents, m_Title);
+            }
+        }
+
+
+#if UNITY_EDITOR
+        class EditorMenuBuilder : IMenuFiller
+        {
+            UnityEditor.GenericMenu m_Menu;
+            public EditorMenuBuilder()
+            {
+                m_Menu = new UnityEditor.GenericMenu();
+            }
+
+            public void AddItem(GUIContent name, bool selected, Action callback)
+            {
+                m_Menu.AddItem(name, selected, ()=>callback());
+            }
+
+            public void Show(Vector2 position)
+            {
+                m_Menu.DropDown(new Rect(position.x, position.y, 0f, 0f));
+            }
+        }
+#endif
+
+        void DrawWindowContents(int id)
+        {
+            m_ContentScrollPosition = GUILayout.BeginScrollView(m_ContentScrollPosition);
+            m_GuiDrawer();
+            GUILayout.EndScrollView();
+            if (m_GuiMenu != null)
+            {
+                Rect scrollRect = GUILayoutUtility.GetLastRect();
+                if (GUI.Button(new Rect(scrollRect.xMin - 5, scrollRect.yMin - 17, 20, 20), "\u2630"))
+                {
+#if UNITY_EDITOR
+                    EditorMenuBuilder menuBuilder = new EditorMenuBuilder();
+                    m_GuiMenu(menuBuilder);
+                    menuBuilder.Show(new Vector2(scrollRect.xMin, scrollRect.yMin+70));
+#endif
+                }
+            }
+        }
+
+        public bool TryDragWindow(Event current)
+        {
+            var expandedWindowMenu = new Rect(m_WindowRect.x - c_DraggableSize, m_WindowRect.y, m_WindowRect.width + c_DraggableSize * 2, m_WindowRect.height + c_DraggableSize);
+            if (expandedWindowMenu.Contains(current.mousePosition))
+            {
+                m_DraggingTitle = current.mousePosition.y < m_WindowRect.yMin + c_DraggableSize * 2;
+                m_DraggingLeft = current.mousePosition.x < m_WindowRect.xMin + c_DraggableSize;
+                m_DraggingRight = current.mousePosition.x > m_WindowRect.xMax - c_DraggableSize;
+                m_DraggingBottom = current.mousePosition.y > m_WindowRect.yMax - c_DraggableSize;
+            }
+
+            m_DragMouseOffset = current.mousePosition - new Vector2(m_WindowRect.xMin, m_WindowRect.yMin);
+            return m_DraggingTitle || m_DraggingLeft || m_DraggingRight || m_DraggingBottom;
+        }
+
+        public void UpdateDragWindow(Event current)
+        {
+            if (m_DraggingTitle)
+            {
+                m_WindowRect.x = current.mousePosition.x - m_DragMouseOffset.x;
+                m_WindowRect.y = current.mousePosition.y - m_DragMouseOffset.y;
+            }
+            else
+            {
+                if (m_DraggingLeft)
+                    m_WindowRect.xMin = current.mousePosition.x;
+                if (m_DraggingBottom)
+                    m_WindowRect.yMax = current.mousePosition.y;
+                if (m_DraggingRight)
+                    m_WindowRect.xMax = current.mousePosition.x;
+            }
+        }
+
+        public void EndDragWindow()
+        {
+            m_DraggingTitle = m_DraggingLeft = m_DraggingRight = m_DraggingBottom = false;
+        }
+
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/HudWindow.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 079abb065905cb142b6daf13e36a8c5b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/OutgoingMessageSender.cs

@@ -25,11 +25,11 @@ namespace Unity.Robotics.ROSTCPConnector
      */
     public class SysCommandSender : OutgoingMessageSender
     {
-        private List<byte[]> m_ListOfSerializations;
+        List<byte[]> m_ListOfSerializations;
 
-        public SysCommandSender(List<byte[]> m_ListOfSerializations)
+        public SysCommandSender(List<byte[]> listOfSerializations)
         {
-            this.m_ListOfSerializations = m_ListOfSerializations;
+            m_ListOfSerializations = listOfSerializations;
         }
 
         public override SendToState SendInternal(MessageSerializer m_MessageSerializer, Stream stream)

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSConnection.cs

@@ -9,6 +9,7 @@ using UnityEngine;
 using UnityEngine.Serialization;
 using System.Collections.Concurrent;
 using System.Threading;
+using System.Linq;
 
 namespace Unity.Robotics.ROSTCPConnector
 {
@@ -44,20 +45,26 @@ namespace Unity.Robotics.ROSTCPConnector
         float m_SleepTimeSeconds = 0.01f;
         public float SleepTimeSeconds { get => m_SleepTimeSeconds; set => m_SleepTimeSeconds = value; }
 
+
         [SerializeField]
         [FormerlySerializedAs("showHUD")]
         bool m_ShowHUD = true;
         public bool ShowHud { get => m_ShowHUD; set => m_ShowHUD = value; }
 
+        [SerializeField]
+        string[] m_TFTopics = { "/tf" };
+        public string[] TFTopics { get => m_TFTopics; set => m_TFTopics = value; }
+
         const int k_DefaultPublisherQueueSize = 10;
         const bool k_DefaultPublisherLatch = false;
 
         // GUI window variables
-        internal HUDPanel m_HudPanel = null;
+        internal HudPanel m_HudPanel = null;
+        public HudPanel HUDPanel => m_HudPanel;
 
         class OutgoingMessageQueue
         {
-            private ConcurrentQueue<OutgoingMessageSender> m_OutgoingMessageQueue;
+            ConcurrentQueue<OutgoingMessageSender> m_OutgoingMessageQueue;
             public readonly ManualResetEvent NewMessageReadyToSendEvent;
 
             public OutgoingMessageQueue()
@@ -78,11 +85,10 @@ namespace Unity.Robotics.ROSTCPConnector
             }
         }
 
-        private OutgoingMessageQueue m_OutgoingMessageQueue = new OutgoingMessageQueue();
+        OutgoingMessageQueue m_OutgoingMessageQueue = new OutgoingMessageQueue();
 
         ConcurrentQueue<Tuple<string, byte[]>> m_IncomingMessages = new ConcurrentQueue<Tuple<string, byte[]>>();
         CancellationTokenSource m_ConnectionThreadCancellation;
-
         public bool HasConnectionThread => m_ConnectionThreadCancellation != null;
 
         static bool m_HasConnectionError = false;
@@ -92,77 +98,163 @@ namespace Unity.Robotics.ROSTCPConnector
         public static float s_RealTimeSinceStartup = 0.0f;
 
         readonly object m_ServiceRequestLock = new object();
+
         int m_NextSrvID = 101;
         Dictionary<int, TaskPauser> m_ServicesWaiting = new Dictionary<int, TaskPauser>();
 
-        struct SubscriberCallback
+        public bool listenForTFMessages = true;
+
+        float m_LastMessageReceivedRealtime;
+        float m_LastMessageSentRealtime;
+        public float LastMessageReceivedRealtime => m_LastMessageReceivedRealtime;
+        public float LastMessageSentRealtime => m_LastMessageSentRealtime;
+
+        // For the IP address field we show in the hud, we store the IP address and port in PlayerPrefs.
+        // This is used to remember the last IP address the player typed into the HUD, in builds where ConnectOnStart is not checked
+        public const string PlayerPrefsKey_ROS_IP = "ROS_IP";
+        public const string PlayerPrefsKey_ROS_TCP_PORT = "ROS_TCP_PORT";
+        public static string RosIPAddressPref => PlayerPrefs.GetString(PlayerPrefsKey_ROS_IP, "127.0.0.1");
+        public static int RosPortPref => PlayerPrefs.GetInt(PlayerPrefsKey_ROS_TCP_PORT, 10000);
+
+        public bool HasSubscriber(string topic)
         {
-            public Func<MessageDeserializer, Message> deserialize;
-            public string rosMessageName;
-            public List<Action<Message>> callbacks;
+            RosTopicState info;
+            return m_Topics.TryGetValue(topic, out info) && info.HasSubscriberCallback;
         }
 
-        Dictionary<string, SubscriberCallback> m_Subscribers = new Dictionary<string, SubscriberCallback>();
-
-        struct UnityServiceImplementation
-        {
-            public Func<MessageDeserializer, Message> deserialize;
-            public string rosMessageName;
-            public Func<Message, Message> callback;
-        }
-
-        object m_DictionaryLock = new object();
-        Dictionary<string, UnityServiceImplementation> m_UnityServices = new Dictionary<string, UnityServiceImplementation>();
-        Dictionary<string, ROSPublisherBase> m_Publishers = new Dictionary<string, ROSPublisherBase>();
-        Dictionary<string, string> m_RosServices = new Dictionary<string, string>();
         MessageSerializer m_MessageSerializer = new MessageSerializer();
         MessageDeserializer m_MessageDeserializer = new MessageDeserializer();
         List<Action<string[]>> m_TopicsListCallbacks = new List<Action<string[]>>();
         List<Action<Dictionary<string, string>>> m_TopicsAndTypesListCallbacks = new List<Action<Dictionary<string, string>>>();
+        List<Action<RosTopicState>> m_NewTopicCallbacks = new List<Action<RosTopicState>>();
+
+        Dictionary<string, RosTopicState> m_Topics = new Dictionary<string, RosTopicState>();
+
+        public void ListenForTopics(Action<RosTopicState> callback, bool notifyAllExistingTopics = false)
+        {
+            m_NewTopicCallbacks.Add(callback);
+            if (notifyAllExistingTopics)
+            {
+                foreach (RosTopicState state in AllTopics)
+                {
+                    callback(state);
+                }
+            }
+        }
+
+        RosTopicState AddTopic(string topic, string rosMessageName)
+        {
+            RosTopicState newTopic = new RosTopicState(topic, rosMessageName, this, new InternalAPI(this));
+            lock (m_Topics)
+            {
+                m_Topics.Add(topic, newTopic);
+            }
+            return newTopic;
+        }
+
+        public RosTopicState GetTopic(string topic)
+        {
+            RosTopicState info;
+            m_Topics.TryGetValue(topic, out info);
+            return info;
+        }
+
+        public IEnumerable<RosTopicState> AllTopics => m_Topics.Values;
+
+        public RosTopicState GetOrCreateTopic(string topic, string rosMessageName)
+        {
+            RosTopicState state = GetTopic(topic);
+            if (state != null)
+            {
+                if (state.RosMessageName != rosMessageName)
+                {
+                    state.ChangeRosMessageName(rosMessageName);
+                }
+                return state;
+            }
+
+            RosTopicState result = AddTopic(topic, rosMessageName);
+            foreach (Action<RosTopicState> callback in m_NewTopicCallbacks)
+            {
+                callback(result);
+            }
+            return result;
+        }
 
         public void Subscribe<T>(string topic, Action<T> callback) where T : Message
         {
-            string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName<T>();
-            AddSubscriberInternal<T>(topic, rosMessageName, callback);
-
-            if (HasConnectionThread)
-                SendSubscriberRegistration(topic, rosMessageName);
-        }
-
-        void AddSubscriberInternal<T>(string topic, string rosMessageName, Action<T> callback) where T : Message
-        {
-            SubscriberCallback subCallbacks;
-            if (!m_Subscribers.TryGetValue(topic, out subCallbacks))
+            string rosMessageName = MessageRegistry.GetRosMessageName<T>();
+            AddSubscriberInternal(topic, rosMessageName, (Message msg) =>
             {
-                subCallbacks = new SubscriberCallback
+                if (msg.RosMessageName == rosMessageName)
                 {
-                    deserialize = MessageRegistry.GetDeserializeFunction<T>(),
-                    rosMessageName = rosMessageName,
-                    callbacks = new List<Action<Message>> { }
-                };
-                m_Subscribers.Add(topic, subCallbacks);
-            }
-
-            subCallbacks.callbacks.Add((Message msg) =>
-            {
-                callback((T)msg);
+                    callback((T)msg);
+                }
+                else
+                {
+                    Debug.LogError($"Subscriber to '{topic}' expected '{rosMessageName}' but received '{msg.RosMessageName}'!?");
+                }
             });
         }
 
-        public void ImplementService<REQUEST>(string topic, Func<REQUEST, Message> callback) where REQUEST : Message
+        public void Unsubscribe(string topic)
         {
-            string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName<REQUEST>();
-            m_UnityServices[topic] = new UnityServiceImplementation
-            {
-                deserialize = MessageRegistry.GetDeserializeFunction<REQUEST>(),
-                rosMessageName = rosMessageName,
-                callback = (Message msg) => callback((REQUEST)msg)
-            };
-
-            if (HasConnectionThread)
-                SendUnityServiceRegistration(topic, rosMessageName);
+            RosTopicState info = GetTopic(topic);
+            if (info != null)
+                info.UnsubscribeAll();
         }
 
+        // Version for when the message type is unknown at compile time
+        public void SubscribeByMessageName(string topic, string rosMessageName, Action<Message> callback)
+        {
+            var constructor = MessageRegistry.GetDeserializeFunction(rosMessageName);
+            if (constructor == null)
+            {
+                Debug.LogError($"Failed to subscribe to topic {topic} - no class has RosMessageName \"{rosMessageName}\"!");
+                return;
+            }
+
+            AddSubscriberInternal(topic, rosMessageName, callback);
+        }
+
+        void AddSubscriberInternal(string topic, string rosMessageName, Action<Message> callback)
+        {
+            RosTopicState info;
+            if (!m_Topics.TryGetValue(topic, out info))
+            {
+                info = AddTopic(topic, rosMessageName);
+            }
+
+            info.AddSubscriber(callback);
+
+            foreach (Action<RosTopicState> topicCallback in m_NewTopicCallbacks)
+            {
+                topicCallback(info);
+            }
+        }
+
+        // Implement a service in Unity
+        public void ImplementService<TRequest>(string topic, Func<TRequest, Message> callback, int? queueSize = null)
+            where TRequest : Message
+        {
+            string rosMessageName = rosMessageName = MessageRegistry.GetRosMessageName<TRequest>();
+
+            RosTopicState info;
+            if (!m_Topics.TryGetValue(topic, out info))
+            {
+                info = AddTopic(topic, rosMessageName);
+            }
+
+            int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize);
+            info.ImplementService((Message msg) => callback((TRequest)msg), resolvedQueueSize);
+
+            foreach (Action<RosTopicState> topicCallback in m_NewTopicCallbacks)
+            {
+                topicCallback(info);
+            }
+        }
+
+        // Send a request to a ros service
         public async void SendServiceMessage<RESPONSE>(string rosServiceName, Message serviceRequest, Action<RESPONSE> callback) where RESPONSE : Message, new()
         {
             RESPONSE response = await SendServiceMessage<RESPONSE>(rosServiceName, serviceRequest);
@@ -176,6 +268,7 @@ namespace Unity.Robotics.ROSTCPConnector
             }
         }
 
+        // Send a request to a ros service
         public async Task<RESPONSE> SendServiceMessage<RESPONSE>(string rosServiceName, Message serviceRequest) where RESPONSE : Message, new()
         {
             m_MessageSerializer.Clear();
@@ -190,8 +283,8 @@ namespace Unity.Robotics.ROSTCPConnector
                 m_ServicesWaiting.Add(srvID, pauser);
             }
 
-            SendSysCommand(SysCommand.k_SysCommand_ServiceRequest, new SysCommand_Service { srv_id = srvID });
-            Publish(rosServiceName, serviceRequest);
+            RosTopicState topicState = GetOrCreateTopic(rosServiceName, serviceRequest.RosMessageName);
+            topicState.SendServiceRequest(serviceRequest, srvID);
 
             byte[] rawResponse = (byte[])await pauser.PauseUntilResumed();
 
@@ -216,82 +309,33 @@ namespace Unity.Robotics.ROSTCPConnector
         {
         }
 
-        public bool TryGetPublisher<T>(out ROSPublisher<T> result, string rosTopicName,
+        public RosTopicState RegisterPublisher<T>(string rosTopicName,
             int? queue_size = null, bool? latch = null) where T : Message
         {
-            ROSPublisherBase publisher;
-            lock (m_DictionaryLock)
-            {
-                if (m_Publishers.TryGetValue(rosTopicName, out publisher))
-                {
-
-                    result = (ROSPublisher<T>)publisher;
-                    if (result == null)
-                    {
-                        Debug.LogError($"Existing publisher with topic {rosTopicName} is null!");
-                        return false;
-                    }
-
-                    string messageName = MessageRegistry.GetRosMessageName<T>();
-                    if (publisher.EquivalentTo(rosTopicName, messageName, queue_size, latch))
-                    {
-                        //We already have a valid existing publisher of the correct type.
-                        return true;
-                    }
-                    else
-                    {
-                        string errorMessage = $"Publisher on topic {rosTopicName} has changed type! " +
-                                              $"Do you have multiple publishers on the same topic?";
-                        Debug.LogError(errorMessage);
-                        return false;
-                    }
-                }
-                else
-                {
-                    //No publisher already exists.
-                    result = null;
-                    return false;
-                }
-            }
+            return RegisterPublisher(rosTopicName, MessageRegistry.GetRosMessageName<T>(), queue_size, latch);
         }
 
-        public ROSPublisher<T> RegisterPublisher<T>(string rosTopicName,
-            int? queue_size = null, bool? latch = null) where T : Message
+        public RosTopicState RegisterPublisher(string rosTopicName, string rosMessageName,
+            int? queueSize = null, bool? latch = null)
         {
-            bool correctPublisherAlreadyExists = TryGetPublisher(out ROSPublisher<T> result, rosTopicName, queue_size, latch);
-            if (correctPublisherAlreadyExists)
-            {
-                Debug.LogWarning($"Publisher for topic {rosTopicName} registered twice!");
-                return result;
-            }
-            else
-            {
-                if (result == null)
-                {
-                    //Create a new publisher.
-                    int resolvedQueueSize = queue_size.GetValueOrDefault(k_DefaultPublisherQueueSize);
-                    bool resolvedLatch = latch.GetValueOrDefault(k_DefaultPublisherLatch);
-                    result = new ROSPublisher<T>(rosTopicName, resolvedQueueSize, resolvedLatch);
-                    m_Publishers[rosTopicName] = result;
-                }
-                else
-                {
-                    throw new Exception("Failed to register publisher!");
-                }
-            }
-            return result;
+            RosTopicState topicState = GetOrCreateTopic(rosTopicName, rosMessageName);
+            //Create a new publisher.
+            int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize);
+            bool resolvedLatch = latch.GetValueOrDefault(k_DefaultPublisherLatch);
+            topicState.RegisterPublisher(resolvedQueueSize, resolvedLatch);
+            return topicState;
         }
 
-        public void RegisterRosService<T>(string topic) where T : Message
+        public void RegisterRosService<TRequest, TResponse>(string topic) where TRequest : Message where TResponse : Message
         {
-            RegisterRosService(topic, MessageRegistry.GetRosMessageName<T>());
+            RegisterRosService(topic, MessageRegistry.GetRosMessageName<TRequest>(), MessageRegistry.GetRosMessageName<TResponse>());
         }
 
-        public void RegisterRosService(string topic, string rosMessageName)
+        public void RegisterRosService(string topic, string requestMessageName, string responseMessageName, int? queueSize = null)
         {
-            m_RosServices[topic] = rosMessageName;
-            if (HasConnectionThread)
-                SendRosServiceRegistration(topic, rosMessageName);
+            RosTopicState info = GetOrCreateTopic(topic, requestMessageName);
+            int resolvedQueueSize = queueSize.GetValueOrDefault(k_DefaultPublisherQueueSize);
+            info.RegisterRosService(responseMessageName, resolvedQueueSize);
         }
 
         [Obsolete("Calling ImplementUnityService now implicitly registers it")]
@@ -299,48 +343,103 @@ namespace Unity.Robotics.ROSTCPConnector
         {
         }
 
-
-        void SendSubscriberRegistration(string topic, string rosMessageName, NetworkStream stream = null)
+        internal struct InternalAPI
         {
-            SendSysCommand(SysCommand.k_SysCommand_Subscribe, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream);
+            ROSConnection m_Self;
+
+            public InternalAPI(ROSConnection self) { m_Self = self; }
+
+            public MessageDeserializer Deserializer => m_Self.m_MessageDeserializer;
+
+            public void SendSubscriberRegistration(string topic, string rosMessageName, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_Subscribe, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream);
+            }
+
+            public void SendRosServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_RosService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream);
+            }
+
+            public void SendUnityServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_UnityService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream);
+            }
+
+            public void SendSubscriberUnregistration(string topic, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_RemoveSubscriber, new SysCommand_Topic { topic = topic }, stream);
+            }
+
+            public void SendPublisherUnregistration(string topic, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_RemovePublisher, new SysCommand_Topic { topic = topic }, stream);
+            }
+
+            public void SendRosServiceUnregistration(string topic, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_RemoveRosService, new SysCommand_Topic { topic = topic }, stream);
+            }
+
+            public void SendUnityServiceUnregistration(string topic, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_RemoveUnityService, new SysCommand_Topic { topic = topic }, stream);
+            }
+
+            public void SendUnityServiceResponse(int serviceId, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_ServiceResponse, new SysCommand_Service { srv_id = serviceId }, stream);
+            }
+
+            public void SendPublisherRegistration(string topic, string message_name, int queueSize, bool latch, NetworkStream stream = null)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_Publish,
+                    new SysCommand_PublisherRegistration { topic = topic, message_name = message_name, queue_size = queueSize, latch = latch }
+                );
+            }
+
+            public void SendServiceRequest(int serviceId)
+            {
+                m_Self.SendSysCommand(SysCommand.k_SysCommand_ServiceRequest, new SysCommand_Service { srv_id = serviceId });
+            }
+
+            public void AddSenderToQueue(OutgoingMessageSender sender)
+            {
+                m_Self.m_OutgoingMessageQueue.Enqueue(sender);
+            }
         }
 
-        void SendRosServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null)
+        static ROSConnection _instance;
+
+        public static ROSConnection GetOrCreateInstance()
         {
-            SendSysCommand(SysCommand.k_SysCommand_RosService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream);
+            if (_instance == null)
+            {
+                GameObject prefab = Resources.Load<GameObject>("ROSConnectionPrefab");
+                if (prefab == null)
+                {
+                    Debug.LogWarning("No settings for ROSConnection.instance! Open \"ROS Settings\" from the Robotics menu to configure it.");
+                    GameObject instance = new GameObject("ROSConnection");
+                    _instance = instance.AddComponent<ROSConnection>();
+                }
+                else
+                {
+                    _instance = Instantiate(prefab).GetComponent<ROSConnection>();
+                }
+            }
+            return _instance;
         }
 
-        void SendUnityServiceRegistration(string topic, string rosMessageName, NetworkStream stream = null)
-        {
-            SendSysCommand(SysCommand.k_SysCommand_UnityService, new SysCommand_TopicAndType { topic = topic, message_name = rosMessageName }, stream);
-        }
-
-        private static ROSConnection _instance;
+        [Obsolete("Please call ROSConnection.GetOrCreateInstance()")]
         public static ROSConnection instance
         {
             get
             {
-                if (_instance == null)
-                {
-                    GameObject prefab = Resources.Load<GameObject>("ROSConnectionPrefab");
-                    if (prefab == null)
-                    {
-                        Debug.LogWarning(
-                            "No settings for ROSConnection.instance! Open \"ROS Settings\" from the Robotics menu to configure it.");
-                        GameObject instance = new GameObject("ROSConnection");
-                        _instance = instance.AddComponent<ROSConnection>();
-                    }
-                    else
-                    {
-                        _instance = Instantiate(prefab).GetComponent<ROSConnection>();
-                    }
-                }
-
-                return _instance;
+                return GetOrCreateInstance();
             }
         }
 
-        void OnEnable()
+        void Awake()
         {
             if (_instance == null)
                 _instance = this;
@@ -350,10 +449,13 @@ namespace Unity.Robotics.ROSTCPConnector
         {
             InitializeHUD();
 
+            HudPanel.RegisterHeader(DrawHeaderGUI);
+
+            if (listenForTFMessages)
+                TFSystem.GetOrCreateInstance();
+
             if (ConnectOnStart)
-            {
                 Connect();
-            }
         }
 
         public void Connect(string ipAddress, int port)
@@ -368,9 +470,6 @@ namespace Unity.Robotics.ROSTCPConnector
             if (!IPFormatIsCorrect(RosIPAddress))
                 Debug.LogWarning("Invalid ROS IP address: " + RosIPAddress);
 
-            if (m_HudPanel != null)
-                m_HudPanel.host = $"{RosIPAddress}:{RosPort}";
-
             m_ConnectionThreadCancellation = new CancellationTokenSource();
 
             Task.Run(() => ConnectionThread(
@@ -379,62 +478,41 @@ namespace Unity.Robotics.ROSTCPConnector
                 m_NetworkTimeoutSeconds,
                 m_KeepaliveTime,
                 (int)(m_SleepTimeSeconds * 1000.0f),
-                RegisterAll,
-                DeregisterAll,
+                OnConnectionStartedCallback,
+                OnConnectionLostCallback,
                 m_OutgoingMessageQueue,
                 m_IncomingMessages,
                 m_ConnectionThreadCancellation.Token
             ));
         }
 
-        void RegisterAll(NetworkStream stream)
+        // NB this callback is not running on the main thread, be cautious about modifying data here
+        void OnConnectionStartedCallback(NetworkStream stream)
         {
-            lock (m_DictionaryLock)
+            RosTopicState[] topics;
+            lock (m_Topics)
             {
-                foreach (var keyValue in m_Subscribers)
-                {
-                    if (keyValue.Value.rosMessageName != null)
-                    {
-                        SendSubscriberRegistration(keyValue.Key, keyValue.Value.rosMessageName, stream);
-                    }
-                }
-
-                foreach (var keyValue in m_UnityServices)
-                {
-                    if (keyValue.Value.rosMessageName != null)
-                    {
-                        SendUnityServiceRegistration(keyValue.Key, keyValue.Value.rosMessageName, stream);
-                    }
-                }
-
-                foreach (var keyValue in m_Publishers)
-                {
-                    if (keyValue.Value != null)
-                    {
-                        keyValue.Value.OnConnectionEstablished(m_MessageSerializer, stream);
-                    }
-                }
-
-                foreach (var keyValue in m_RosServices)
-                {
-                    if (keyValue.Value != null)
-                    {
-                        SendRosServiceRegistration(keyValue.Key, keyValue.Value, stream);
-                    }
-                }
+                topics = AllTopics.ToArray();
             }
+
+            foreach (RosTopicState topicInfo in m_Topics.Values.ToArray())
+                topicInfo.OnConnectionEstablished(stream);
+
+            RefreshTopicsList();
         }
 
-        void DeregisterAll()
+        void OnConnectionLostCallback()
         {
-            lock (m_DictionaryLock)
+            RosTopicState[] topics;
+            lock (m_Topics)
             {
-                foreach (var keyValue in m_Publishers)
-                {
-                    //For all publishers, notify that they need to re-register.
-                    if (keyValue.Value != null)
-                        keyValue.Value.PublisherRegistered = false;
-                }
+                topics = AllTopics.ToArray();
+            }
+
+            foreach (RosTopicState topicInfo in topics)
+            {
+                //For all publishers, notify that they need to re-register.
+                topicInfo.OnConnectionLost();
             }
         }
 
@@ -448,21 +526,12 @@ namespace Unity.Robotics.ROSTCPConnector
 
         void OnValidate()
         {
-            InitializeHUD();
-        }
-
-        private void InitializeHUD()
-        {
-            if (!Application.isPlaying || (!m_ShowHUD && m_HudPanel == null))
+            // the prefab is not the instance!
+            if (gameObject.scene.name == null)
                 return;
 
-            if (m_HudPanel == null)
-            {
-                m_HudPanel = gameObject.AddComponent<HUDPanel>();
-                m_HudPanel.host = $"{RosIPAddress}:{RosPort}";
-            }
-
-            m_HudPanel.isEnabled = m_ShowHUD;
+            if (_instance == null)
+                _instance = this;
         }
 
         Action<string, byte[]> m_SpecialIncomingMessageHandler;
@@ -475,6 +544,7 @@ namespace Unity.Robotics.ROSTCPConnector
             while (m_IncomingMessages.TryDequeue(out data))
             {
                 (string topic, byte[] contents) = data;
+                m_LastMessageReceivedRealtime = Time.realtimeSinceStartup;
 
                 if (m_SpecialIncomingMessageHandler != null)
                 {
@@ -486,22 +556,38 @@ namespace Unity.Robotics.ROSTCPConnector
                 }
                 else
                 {
-                    // notify whatever is interested in this incoming message
-                    SubscriberCallback callback;
-                    if (m_Subscribers.TryGetValue(topic, out callback))
-                    {
-                        m_MessageDeserializer.InitWithBuffer(contents);
-                        Message message = callback.deserialize(m_MessageDeserializer);
-
-                        if (m_HudPanel != null && !topic.StartsWith("__"))
-                            m_HudPanel.SetLastMessageReceived(topic, message);
-
-                        callback.callbacks.ForEach(item => item(message));
-                    }
+                    RosTopicState topicInfo = GetTopic(topic);
+                    // if this is null, we have received a message on a topic we've never heard of...!?
+                    // all we can do is ignore it, we don't even know what type it is
+                    if (topicInfo != null)
+                        topicInfo.OnMessageReceived(contents);
                 }
             }
         }
 
+        float m_LastTopicsRequestRealtime = -1;
+        const float k_TimeBetweenTopicsUpdates = 5.0f;
+
+        public void RefreshTopicsList()
+        {
+            // cap the rate of requests
+            if (m_LastTopicsRequestRealtime != -1 && s_RealTimeSinceStartup - m_LastTopicsRequestRealtime <= k_TimeBetweenTopicsUpdates)
+                return;
+
+            m_LastTopicsRequestRealtime = s_RealTimeSinceStartup;
+            GetTopicAndTypeList((data) =>
+            {
+                foreach (KeyValuePair<string, string> kv in data)
+                {
+                    RosTopicState state = GetOrCreateTopic(kv.Key, kv.Value);
+                    if (state.RosMessageName != kv.Value)
+                    {
+                        state.ChangeRosMessageName(kv.Value);
+                    }
+                }
+            });
+        }
+
         void ReceiveSysCommand(string topic, string json)
         {
             switch (topic)
@@ -528,36 +614,26 @@ namespace Unity.Robotics.ROSTCPConnector
                     {
                         var serviceCommand = JsonUtility.FromJson<SysCommand_Service>(json);
 
-                        // the next incoming message will be a service request, so set a special callback to process it
+                        // the next incoming message will be a request for a Unity service, so set a special callback to process it
                         m_SpecialIncomingMessageHandler = (string serviceTopic, byte[] requestBytes) =>
                         {
                             m_SpecialIncomingMessageHandler = null;
 
-                            // find the service implementation
-                            UnityServiceImplementation service;
-                            if (!m_UnityServices.TryGetValue(serviceTopic, out service))
+                            RosTopicState topicState = GetTopic(serviceTopic);
+                            if (topicState == null)
                             {
                                 Debug.LogError($"Unity service {serviceTopic} has not been implemented!");
                                 return;
                             }
 
-                            // deserialize the request message
-                            m_MessageDeserializer.InitWithBuffer(requestBytes);
-                            Message requestMessage = service.deserialize(m_MessageDeserializer);
-
-                            // run the actual service
-                            Message responseMessage = service.callback(requestMessage);
-
-                            // send the response message back
-                            SendSysCommand(SysCommand.k_SysCommand_ServiceResponse, new SysCommand_Service { srv_id = serviceCommand.srv_id });
-                            Publish(serviceTopic, responseMessage);
+                            topicState.HandleUnityServiceRequest(requestBytes, serviceCommand.srv_id);
                         };
                     }
                     break;
 
                 case SysCommand.k_SysCommand_ServiceResponse:
                     {
-                        // it's a response from a ros service
+                        // the next incoming message will be a response from a ros service
                         var serviceCommand = JsonUtility.FromJson<SysCommand_Service>(json);
                         m_SpecialIncomingMessageHandler = (string serviceTopic, byte[] requestBytes) =>
                         {
@@ -585,8 +661,8 @@ namespace Unity.Robotics.ROSTCPConnector
                         if (m_TopicsAndTypesListCallbacks.Count > 0)
                         {
                             Dictionary<string, string> callbackParam = new Dictionary<string, string>();
-                            for (int Idx = 0; Idx < topicsResponse.topics.Length; ++Idx)
-                                callbackParam[topicsResponse.topics[Idx]] = topicsResponse.types[Idx];
+                            for (int idx = 0; idx < topicsResponse.topics.Length; ++idx)
+                                callbackParam[topicsResponse.topics[idx]] = topicsResponse.types[idx];
                             m_TopicsAndTypesListCallbacks.ForEach(a => a(callbackParam));
                             m_TopicsAndTypesListCallbacks.Clear();
                         }
@@ -620,7 +696,7 @@ namespace Unity.Robotics.ROSTCPConnector
             float networkTimeoutSeconds,
             float keepaliveTime,
             int sleepMilliseconds,
-            Action<NetworkStream> RegisterAll,
+            Action<NetworkStream> OnConnectionStartedCallback,
             Action DeregisterAll,
             OutgoingMessageQueue outgoingQueue,
             ConcurrentQueue<Tuple<string, byte[]>> incomingQueue,
@@ -647,7 +723,7 @@ namespace Unity.Robotics.ROSTCPConnector
                     networkStream.ReadTimeout = (int)(networkTimeoutSeconds * 1000);
 
                     SendKeepalive(networkStream);
-                    RegisterAll(networkStream);
+                    OnConnectionStartedCallback(networkStream);
 
                     readerCancellation = new CancellationTokenSource();
                     _ = Task.Run(() => ReaderThread(nextReaderIdx, networkStream, incomingQueue, sleepMilliseconds, readerCancellation.Token));
@@ -657,7 +733,6 @@ namespace Unity.Robotics.ROSTCPConnector
                     float waitingSinceRealTime = s_RealTimeSinceStartup;
                     while (true)
                     {
-
                         bool messageReadyEventWasSet = outgoingQueue.NewMessageReadyToSendEvent.WaitOne(sleepMilliseconds);
                         token.ThrowIfCancellationRequested();
 
@@ -676,7 +751,6 @@ namespace Unity.Robotics.ROSTCPConnector
 
                         while (outgoingQueue.TryDequeue(out OutgoingMessageSender sendsOutgoingMessages))
                         {
-
                             OutgoingMessageSender.SendToState sendToState = sendsOutgoingMessages.SendInternal(messageSerializer, networkStream);
                             switch (sendToState)
                             {
@@ -733,7 +807,7 @@ namespace Unity.Robotics.ROSTCPConnector
                 try
                 {
                     Tuple<string, byte[]> content = await ReadMessageContents(networkStream, sleepMilliseconds, token);
-                    //Debug.Log($"Message {content.Item1} received");
+                    // Debug.Log($"Message {content.Item1} received");
                     ROSConnection.m_HasConnectionError = false;
 
                     if (content.Item1 != "") // ignore keepalive messages
@@ -797,8 +871,6 @@ namespace Unity.Robotics.ROSTCPConnector
             Disconnect();
         }
 
-
-
         void SendSysCommand(string command, object param, NetworkStream stream = null)
         {
             if (stream != null)
@@ -807,7 +879,7 @@ namespace Unity.Robotics.ROSTCPConnector
                 QueueSysCommand(command, param);
         }
 
-        private static void PopulateSysCommand(MessageSerializer messageSerializer, string command, object param)
+        static void PopulateSysCommand(MessageSerializer messageSerializer, string command, object param)
         {
             messageSerializer.Clear();
             // syscommands are sent as:
@@ -836,13 +908,13 @@ namespace Unity.Robotics.ROSTCPConnector
             m_OutgoingMessageQueue.Enqueue(new SysCommandSender(m_MessageSerializer.GetBytesSequence()));
         }
 
-        [ObsoleteAttribute("Use Publish instead of Send", false)]
-        public void Send<T>(string rosTopicName, T message) where T : Message
+        [Obsolete("Use Publish instead of Send", false)]
+        public void Send(string rosTopicName, Message message)
         {
             Publish(rosTopicName, message);
         }
 
-        public void Publish<T>(string rosTopicName, T message) where T : Message
+        public void Publish(string rosTopicName, Message message)
         {
             if (rosTopicName.StartsWith("__"))
             {
@@ -850,31 +922,161 @@ namespace Unity.Robotics.ROSTCPConnector
             }
             else
             {
-                //Find the publisher and queue the message for sending.
-                if (TryGetPublisher(out ROSPublisher<T> existingPublisher, rosTopicName))
+                RosTopicState rosTopic = GetTopic(rosTopicName);
+                if (rosTopic == null || !rosTopic.IsPublisher)
                 {
-                    existingPublisher.PublishInternal(message);
-                    m_OutgoingMessageQueue.Enqueue(existingPublisher);
+                    throw new Exception($"No registered publisher on topic {rosTopicName} for type {message.RosMessageName}!");
+                }
 
-                    if (m_HudPanel != null)
-                        m_HudPanel.SetLastMessageSent(rosTopicName, message);
-                }
-                else
-                {
-                    throw new Exception($"No registered publisher on topic {rosTopicName} of type {MessageRegistry.GetRosMessageName<T>()}!");
-                }
+                rosTopic.Publish(message);
             }
         }
 
-        public static T GetFromPool<T>(string rosTopicName) where T : Message
+        public T GetFromPool<T>(string rosTopicName) where T : Message
         {
-            if (instance.TryGetPublisher(out ROSPublisher<T> rosPublisher, rosTopicName))
+            RosTopicState topicState = GetTopic(rosTopicName);
+            if (topicState != null)
             {
-                return rosPublisher.GetMessageFromPool();
+                return (T)topicState.GetMessageFromPool();
             }
             throw new Exception($"No publisher on topic {rosTopicName} of type {MessageRegistry.GetRosMessageName<T>()} to get pooled messages from!");
         }
 
+        void InitializeHUD()
+        {
+            if (!Application.isPlaying || (!m_ShowHUD && m_HudPanel == null))
+                return;
+
+            if (m_HudPanel == null)
+            {
+                m_HudPanel = gameObject.AddComponent<HudPanel>();
+            }
+
+            m_HudPanel.isEnabled = m_ShowHUD;
+        }
+
+        void DrawHeaderGUI()
+        {
+            GUIStyle labelStyle = new GUIStyle
+            {
+                alignment = TextAnchor.MiddleLeft,
+                normal = { textColor = Color.white },
+                fontStyle = FontStyle.Bold,
+                fixedWidth = 250
+            };
+
+            GUIStyle contentStyle = new GUIStyle
+            {
+                alignment = TextAnchor.MiddleLeft,
+                padding = new RectOffset(10, 0, 0, 5),
+                normal = { textColor = Color.white },
+                fixedWidth = 300
+            };
+
+
+            // ROS IP Setup
+            GUILayout.BeginHorizontal();
+            DrawConnectionArrows(
+                true,
+                0,
+                0,
+                Time.realtimeSinceStartup - LastMessageReceivedRealtime,
+                Time.realtimeSinceStartup - LastMessageSentRealtime,
+                HasConnectionThread,
+                HasConnectionThread,
+                HasConnectionError
+            );
+
+#if ROS2
+            string protocolName = "ROS2";
+#else
+            string protocolName = "ROS";
+#endif
+
+            GUILayout.Space(30);
+            GUILayout.Label($"{protocolName} IP: ", labelStyle, GUILayout.Width(100));
+
+            if (!HasConnectionThread)
+            {
+                // if you've never run a build on this machine before, initialize the playerpref settings to the ones from the RosConnection
+                if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_IP))
+                    SetIPPref(RosIPAddress);
+                if (!PlayerPrefs.HasKey(PlayerPrefsKey_ROS_TCP_PORT))
+                    SetPortPref(RosPort);
+
+                // NB, here the user is editing the PlayerPrefs values, not the ones in the RosConnection.
+                // (So that the hud remembers what IP you used last time you ran this build.)
+                // The RosConnection receives the edited values when you click Connect.
+                SetIPPref(GUILayout.TextField(RosIPAddressPref));
+                SetPortPref(Convert.ToInt32(GUILayout.TextField(RosPortPref.ToString())));
+
+                GUILayout.EndHorizontal();
+                GUILayout.Label("(Not connected)");
+                if (GUILayout.Button("Connect"))
+                    Connect(RosIPAddressPref, RosPortPref);
+            }
+            else
+            {
+                GUILayout.Label($"{RosIPAddress}:{RosPort}", contentStyle);
+                GUILayout.EndHorizontal();
+            }
+        }
+
+        static GUIStyle s_ConnectionArrowStyle;
+
+        public static void DrawConnectionArrows(bool withBar, float x, float y, float receivedTime, float sentTime, bool isPublisher, bool isSubscriber, bool hasError)
+        {
+            if (s_ConnectionArrowStyle == null)
+            {
+                s_ConnectionArrowStyle = new GUIStyle
+                {
+                    alignment = TextAnchor.MiddleLeft,
+                    normal = { textColor = Color.white },
+                    fontSize = 22,
+                    fontStyle = FontStyle.Bold,
+                    fixedWidth = 250
+                };
+            }
+
+            var baseColor = GUI.color;
+            GUI.color = Color.white;
+            if (withBar)
+                GUI.Label(new Rect(x + 4, y + 5, 25, 15), "I", s_ConnectionArrowStyle);
+            GUI.color = GetConnectionColor(receivedTime, isSubscriber, hasError);
+            GUI.Label(new Rect(x + 8, y + 6, 25, 15), "\u2190", s_ConnectionArrowStyle);
+            GUI.color = GetConnectionColor(sentTime, isPublisher, hasError);
+            GUI.Label(new Rect(x + 8, y + 0, 25, 15), "\u2192", s_ConnectionArrowStyle);
+            GUI.color = baseColor;
+        }
+
+        public static void SetIPPref(string ipAddress)
+        {
+            PlayerPrefs.SetString(PlayerPrefsKey_ROS_IP, ipAddress);
+        }
+
+        public static void SetPortPref(int port)
+        {
+            PlayerPrefs.SetInt(PlayerPrefsKey_ROS_TCP_PORT, port);
+        }
+
+        public static Color GetConnectionColor(float elapsedTime, bool hasConnection, bool hasError)
+        {
+            var bright = new Color(1, 1, 0.5f);
+            var mid = new Color(0, 1, 1);
+            var dark = new Color(0, 0.5f, 1);
+            const float brightDuration = 0.03f;
+            const float fadeToDarkDuration = 1.0f;
+
+            if (!hasConnection)
+                return Color.gray;
+            if (hasError)
+                return Color.red;
+
+            if (elapsedTime <= brightDuration)
+                return bright;
+            return Color.Lerp(mid, dark, elapsedTime / fadeToDarkDuration);
+        }
+
         public static bool IPFormatIsCorrect(string ipAddress)
         {
             if (ipAddress == null || ipAddress == "")

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/ROSPublisher.cs

@@ -1,290 +0,0 @@
-using System;
-using System.Collections.Generic;
-using System.IO;
-using System.Threading;
-using Unity.Robotics.ROSTCPConnector.MessageGeneration;
-using UnityEngine;
-
-namespace Unity.Robotics.ROSTCPConnector
-{
-    public abstract class ROSPublisherBase : OutgoingMessageSender
-    {
-        public abstract string RosMessageName { get; }
-
-        public string TopicName { get; }
-
-        public int QueueSize { get; }
-
-        public bool Latch { get; }
-
-        public bool PublisherRegistered { get; set; }
-
-        protected ROSPublisherBase(string topicName, int queueSize, bool latch)
-        {
-            if (queueSize < 1)
-            {
-                throw new Exception("Queue size must be greater than or equal to 1.");
-            }
-
-            TopicName = topicName;
-            QueueSize = queueSize;
-            Latch = latch;
-            PublisherRegistered = false;
-        }
-
-        public bool EquivalentTo(string topicName, string messageName, int? queueSize, bool? latch)
-        {
-            return TopicName == topicName && RosMessageName == messageName &&
-                   (!queueSize.HasValue || QueueSize == queueSize) &&
-                   (!latch.HasValue || Latch == latch);
-        }
-
-        public abstract void OnConnectionEstablished(MessageSerializer m_MessageSerializer, Stream stream);
-    }
-
-    public class ROSPublisher<T> : ROSPublisherBase where T : Message
-    {
-        public override string RosMessageName { get; }
-
-        //Messages waiting to be sent queue.
-        private LinkedList<T> outgoingMessages = new LinkedList<T>();
-
-        //Keeps track of how many outgoing messages were removed due to queue overflow.
-        //If a message is published but the queue is full, the counter is incremented, the
-        //first message in the queue is recycled and the message to publish is put on the end of the queue.
-        //If this is non 0, a SendTo call will decrement the counter instead of sending data.
-        private int queueOverflowUnsentCounter = 0;
-
-        //Latching - This message will be set if latching is enabled, and on reconnection, it will be sent again.
-        private T lastMessageSent = null;
-
-        //Whether you want to pool messages for reuse (Used to reduce GC calls).
-        private volatile bool _messagePoolEnabled;
-
-        //Optional, used if you want to pool messages and reuse them when they are no longer in use.
-        private Queue<T> inactiveMessagePool = new Queue<T>();
-
-        public ROSPublisher(string topicName, int queueSize, bool latch) : base(topicName, queueSize, latch)
-        {
-            this.RosMessageName = MessageRegistry.GetRosMessageName<T>();
-        }
-
-        internal void PublishInternal(T message)
-        {
-            lock (outgoingMessages)
-            {
-                if (outgoingMessages.Count >= QueueSize)
-                {
-                    //Remove outgoing messages that don't fit in the queue.
-                    //Recycle the message if applicable
-                    RecycleMessageIfApplicable(outgoingMessages.First.Value);
-                    //Update the overflow counter.
-                    queueOverflowUnsentCounter++;
-                    outgoingMessages.RemoveFirst();
-                }
-
-                //Add a new valid message to the end.
-                outgoingMessages.AddLast(message);
-            }
-        }
-
-        public override void OnConnectionEstablished(MessageSerializer m_MessageSerializer, Stream stream)
-        {
-            //Register the publisher with the ROS Endpoint.
-            RegisterPublisherIfApplicable(m_MessageSerializer, stream);
-        }
-
-        private SendToState RemoveMessageToSend(out T messageToSend)
-        {
-            SendToState result = SendToState.NoMessageToSendError;
-            messageToSend = null;
-            lock (outgoingMessages)
-            {
-                if (queueOverflowUnsentCounter > 0)
-                {
-                    //This means that we can't send message to ROS as fast as we're generating them.
-                    //This could potentially be bad as it means that we are dropping messages!
-                    queueOverflowUnsentCounter--;
-                    messageToSend = null;
-                    result = SendToState.QueueFullWarning;
-                }
-                else if (outgoingMessages.Count > 0)
-                {
-                    //Retrieve the next message and populate messageToSend.
-                    messageToSend = outgoingMessages.First.Value;
-                    outgoingMessages.RemoveFirst();
-                    result = SendToState.Normal;
-                }
-            }
-
-            return result;
-        }
-
-        public bool PeekNextMessageToSend(out T messageToSend)
-        {
-            bool result = false;
-            messageToSend = null;
-            lock (outgoingMessages)
-            {
-                if (outgoingMessages.Count > 0)
-                {
-                    messageToSend = outgoingMessages.First.Value;
-                    result = true;
-                }
-            }
-
-
-            return result;
-        }
-
-        private void SendMessageWithStream(MessageSerializer m_MessageSerializer, Stream stream, Message message)
-        {
-            RegisterPublisherIfApplicable(m_MessageSerializer, stream);
-
-            //Clear the serializer
-            m_MessageSerializer.Clear();
-            //Prepare the data to send.
-            m_MessageSerializer.Write(TopicName);
-            m_MessageSerializer.SerializeMessageWithLength(message);
-            //Send via the stream.
-            m_MessageSerializer.SendTo(stream);
-        }
-
-        private void RegisterPublisherIfApplicable(MessageSerializer m_MessageSerializer, Stream stream)
-        {
-            if (PublisherRegistered)
-            {
-                return;
-            }
-
-            //Register the publisher before sending anything.
-            SysCommandPublisherRegistration publisherRegistration =
-                new SysCommandPublisherRegistration(this);
-            publisherRegistration.SendTo(stream, m_MessageSerializer);
-            PublisherRegistered = true;
-
-            if (Latch && lastMessageSent != null)
-            {
-                //This topic is latching, so to mimic that functionality,
-                //here the last sent message is sent again with the new connection.
-                SendMessageWithStream(m_MessageSerializer, stream, lastMessageSent);
-            }
-        }
-
-        public override SendToState SendInternal(MessageSerializer m_MessageSerializer, Stream stream)
-        {
-            RegisterPublisherIfApplicable(m_MessageSerializer, stream);
-            SendToState sendToState = RemoveMessageToSend(out T toSend);
-            if (sendToState == SendToState.Normal)
-            {
-                SendMessageWithStream(m_MessageSerializer, stream, toSend);
-
-                //Recycle the message (if applicable).
-                if (Latch)
-                {
-                    if (lastMessageSent != null && lastMessageSent != toSend)
-                    {
-                        RecycleMessageIfApplicable(lastMessageSent);
-                    }
-
-                    lastMessageSent = toSend;
-                }
-                else
-                {
-                    RecycleMessageIfApplicable(toSend);
-                }
-            }
-
-            return sendToState;
-        }
-
-        public override void ClearAllQueuedData()
-        {
-            List<T> toRecycle;
-            lock (outgoingMessages)
-            {
-                toRecycle = new List<T>(outgoingMessages);
-                outgoingMessages.Clear();
-                queueOverflowUnsentCounter = 0;
-            }
-
-            foreach (T messageToRecycle in toRecycle)
-            {
-                RecycleMessageIfApplicable(messageToRecycle);
-            }
-        }
-
-        private void RecycleMessageIfApplicable(T toRecycle)
-        {
-            if (!MessagePoolEnabled)
-            {
-                return;
-            }
-
-            //Add the message back to the pool.
-            AddMessageToPool(toRecycle);
-        }
-
-        #region Message Pooling
-
-        public bool MessagePoolEnabled
-        {
-            get => _messagePoolEnabled;
-            set
-            {
-                if (_messagePoolEnabled != value)
-                {
-                    _messagePoolEnabled = value;
-                    if (!_messagePoolEnabled)
-                    {
-                        lock (inactiveMessagePool)
-                        {
-                            inactiveMessagePool.Clear();
-                        }
-                    }
-                }
-            }
-        }
-
-        public void AddMessageToPool(T messageToRecycle)
-        {
-            Debug.Assert(MessagePoolEnabled,
-                "Adding a message to a message pool that is not enabled, please set MessagePoolEnabled to true.");
-
-            lock (inactiveMessagePool)
-            {
-                if (MessagePoolEnabled && inactiveMessagePool.Count < (QueueSize + 5))
-                {
-                    //Make sure we're only pooling a reasonable amount.
-                    //We shouldn't need any more than the queue size plus a little.
-                    inactiveMessagePool.Enqueue(messageToRecycle);
-                }
-            }
-        }
-
-        /**
-         * @return a message of type T full of garbage data, be sure to update it accordingly.
-         */
-        public T GetMessageFromPool()
-        {
-            T result = null;
-            MessagePoolEnabled = true;
-            lock (inactiveMessagePool)
-            {
-                if (inactiveMessagePool.Count > 0)
-                {
-                    result = inactiveMessagePool.Dequeue();
-                }
-            }
-
-            if (result == null)
-            {
-                result = Activator.CreateInstance<T>();
-            }
-
-            return result;
-        }
-
-        #endregion
-    }
-}

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs

@@ -0,0 +1,262 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Net.Sockets;
+using System.Threading.Tasks;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector
+{
+    public class RosTopicState
+    {
+        string m_Topic;
+        public string Topic => m_Topic;
+
+        MessageSubtopic m_Subtopic;
+        public MessageSubtopic Subtopic => m_Subtopic;
+
+        string m_RosMessageName;
+        public string RosMessageName => m_RosMessageName;
+
+        TopicMessageSender m_MessageSender;
+        public TopicMessageSender MessageSender;
+        public bool IsPublisher { get; private set; }
+        public bool IsPublisherLatched { get; private set; }
+        public bool SentPublisherRegistration { get; private set; }
+
+        bool m_IsRosService;
+        public bool IsRosService => m_IsRosService;
+
+        ROSConnection m_Connection;
+        public ROSConnection Connection => m_Connection;
+        ROSConnection.InternalAPI m_ConnectionInternal;
+        Func<MessageDeserializer, Message> m_Deserializer;
+
+        Func<Message, Message> m_ServiceImplementation;
+        private Func<Message, Task<Message>> m_ServiceImplementationAsync;
+        RosTopicState m_ServiceResponseTopic;
+        public RosTopicState ServiceResponseTopic => m_ServiceResponseTopic;
+
+        public bool IsUnityService => m_ServiceImplementation != null || m_ServiceImplementationAsync != null;
+
+        List<Action<Message>> m_SubscriberCallbacks = new List<Action<Message>>();
+        public bool HasSubscriberCallback => m_SubscriberCallbacks.Count > 0;
+        public bool SentSubscriberRegistration { get; private set; }
+
+        float m_LastMessageReceivedRealtime;
+        float m_LastMessageSentRealtime;
+        public float LastMessageReceivedRealtime => m_LastMessageReceivedRealtime;
+        public float LastMessageSentRealtime => m_LastMessageSentRealtime;
+
+        internal RosTopicState(string topic, string rosMessageName, ROSConnection connection, ROSConnection.InternalAPI connectionInternal, MessageSubtopic subtopic = MessageSubtopic.Default)
+        {
+            m_Topic = topic;
+            m_Subtopic = subtopic;
+            m_RosMessageName = rosMessageName;
+            m_Connection = connection;
+            m_ConnectionInternal = connectionInternal;
+        }
+
+        internal void ChangeRosMessageName(string rosMessageName)
+        {
+            if (m_RosMessageName != null)
+                Debug.LogWarning($"Inconsistent declaration of topic '{Topic}': was '{m_RosMessageName}', switching to '{rosMessageName}'.");
+            m_RosMessageName = rosMessageName;
+            // force deserializer to be refreshed
+            m_Deserializer = null;
+        }
+
+        internal void OnMessageReceived(byte[] data)
+        {
+            m_LastMessageReceivedRealtime = Time.realtimeSinceStartup;
+            if (m_IsRosService && m_ServiceResponseTopic != null)
+            {
+                //  For a service, incoming messages are a different type from outgoing messages.
+                //  We process them using a separate RosTopicState.
+                m_ServiceResponseTopic.OnMessageReceived(data);
+                return;
+            }
+
+            // don't bother deserializing this message if nobody cares
+            if (m_SubscriberCallbacks.Count == 0)
+            {
+                return;
+            }
+
+            Message message = Deserialize(data);
+
+            m_SubscriberCallbacks.ForEach(item => item(message));
+        }
+
+        void OnMessageSent(Message message)
+        {
+            m_LastMessageSentRealtime = Time.realtimeSinceStartup;
+            if (m_RosMessageName == null)
+            {
+                ChangeRosMessageName(message.RosMessageName);
+            }
+
+            m_SubscriberCallbacks.ForEach(item => item(message));
+        }
+
+        internal async void HandleUnityServiceRequest(byte[] data, int serviceId)
+        {
+            if (!IsUnityService)
+            {
+                Debug.LogError($"Unity service '{m_Topic}' has not been implemented!");
+                return;
+            }
+
+            OnMessageReceived(data);
+
+            // deserialize the request message
+            Message requestMessage = Deserialize(data);
+
+            // run the actual service
+            Message response;
+
+            if (m_ServiceImplementationAsync != null)
+            {
+                response = await m_ServiceImplementationAsync(requestMessage);
+            }
+            else
+            {
+                response = m_ServiceImplementation(requestMessage);
+            }
+
+            m_ServiceResponseTopic.OnMessageSent(response);
+
+            // send the response message back
+            m_ConnectionInternal.SendUnityServiceResponse(serviceId);
+            m_MessageSender.Queue(response);
+            m_ConnectionInternal.AddSenderToQueue(m_MessageSender);
+        }
+
+        Message Deserialize(byte[] data)
+        {
+            if (m_Deserializer == null)
+                m_Deserializer = MessageRegistry.GetDeserializeFunction(m_RosMessageName, m_Subtopic);
+
+            m_ConnectionInternal.Deserializer.InitWithBuffer(data);
+            return m_Deserializer(m_ConnectionInternal.Deserializer);
+        }
+
+        public void AddSubscriber(Action<Message> callback)
+        {
+            m_SubscriberCallbacks.Add(callback);
+
+            RegisterSubscriber();
+        }
+
+        void RegisterSubscriber(NetworkStream stream = null)
+        {
+            if (m_Connection.HasConnectionThread && !SentSubscriberRegistration)
+            {
+                m_ConnectionInternal.SendSubscriberRegistration(m_Topic, m_RosMessageName, stream);
+                SentSubscriberRegistration = true;
+            }
+        }
+
+        public void UnsubscribeAll()
+        {
+            m_SubscriberCallbacks.Clear();
+            m_ConnectionInternal.SendSubscriberUnregistration(m_Topic);
+            SentSubscriberRegistration = false;
+        }
+
+        public void ImplementService(Func<Message, Message> implementation, int queueSize)
+        {
+            m_ServiceImplementation = implementation;
+            m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName);
+            m_ServiceResponseTopic = new RosTopicState(m_Topic, m_RosMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response);
+            CreateMessageSender(queueSize);
+        }
+
+        public void ImplementService(Func<Message, Task<Message>> implementation, int queueSize)
+        {
+            m_ServiceImplementationAsync = implementation;
+            m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName);
+            m_ServiceResponseTopic = new RosTopicState(m_Topic, m_RosMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response);
+            CreateMessageSender(queueSize);
+        }
+
+        public void RegisterPublisher(int queueSize, bool latch)
+        {
+            if (IsPublisher)
+            {
+                Debug.LogWarning($"Publisher for topic {m_Topic} registered twice!");
+                return;
+            }
+            IsPublisher = true;
+            IsPublisherLatched = latch;
+            m_ConnectionInternal.SendPublisherRegistration(m_Topic, m_RosMessageName, queueSize, latch);
+            CreateMessageSender(queueSize);
+        }
+
+        public void Publish(Message message)
+        {
+            m_LastMessageSentRealtime = Time.realtimeSinceStartup;
+            OnMessageSent(message);
+            m_MessageSender.Queue(message);
+            m_ConnectionInternal.AddSenderToQueue(m_MessageSender);
+        }
+
+        public Message GetMessageFromPool() => m_MessageSender.GetMessageFromPool();
+
+        void CreateMessageSender(int queueSize)
+        {
+            m_MessageSender = new TopicMessageSender(Topic, m_RosMessageName, queueSize);
+        }
+
+        public void RegisterRosService(string responseMessageName, int queueSize)
+        {
+            m_IsRosService = true;
+            m_ConnectionInternal.SendRosServiceRegistration(m_Topic, m_RosMessageName);
+            m_ServiceResponseTopic = new RosTopicState(m_Topic, responseMessageName, m_Connection, m_ConnectionInternal, MessageSubtopic.Response);
+            CreateMessageSender(queueSize);
+        }
+
+        internal void SendServiceRequest(Message requestMessage, int serviceId)
+        {
+            m_ConnectionInternal.SendServiceRequest(serviceId);
+            m_MessageSender.Queue(requestMessage);
+            m_ConnectionInternal.AddSenderToQueue(m_MessageSender);
+        }
+
+        internal void OnConnectionEstablished(NetworkStream stream)
+        {
+            if (m_SubscriberCallbacks.Count > 0 && !SentSubscriberRegistration)
+            {
+                m_ConnectionInternal.SendSubscriberRegistration(m_Topic, m_RosMessageName, stream);
+                SentSubscriberRegistration = true;
+            }
+
+            if (IsUnityService)
+            {
+                m_ConnectionInternal.SendUnityServiceRegistration(m_Topic, m_RosMessageName, stream);
+            }
+
+            if (IsPublisher)
+            {
+                //Register the publisher before sending anything.
+                m_ConnectionInternal.SendPublisherRegistration(m_Topic, m_RosMessageName, m_MessageSender.QueueSize, IsPublisherLatched, stream);
+                if (IsPublisherLatched)
+                {
+                    m_MessageSender.PrepareLatchMessage();
+                    m_ConnectionInternal.AddSenderToQueue(m_MessageSender);
+                }
+            }
+
+            if (m_IsRosService)
+            {
+                m_ConnectionInternal.SendRosServiceRegistration(m_Topic, m_RosMessageName, stream);
+            }
+        }
+
+        internal void OnConnectionLost()
+        {
+            SentSubscriberRegistration = false;
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/RosTopicState.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 478c335af85accc499b75e7883e649fc
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/SysCommand.cs

@@ -7,7 +7,6 @@ namespace Unity.Robotics.ROSTCPConnector
 {
     public abstract class SysCommand
     {
-
         public const string k_SysCommand_Log = "__log";
         public const string k_SysCommand_Warning = "__warn";
         public const string k_SysCommand_Error = "__error";
@@ -18,6 +17,10 @@ namespace Unity.Robotics.ROSTCPConnector
         public const string k_SysCommand_RosService = "__ros_service";
         public const string k_SysCommand_UnityService = "__unity_service";
         public const string k_SysCommand_TopicList = "__topic_list";
+        public const string k_SysCommand_RemoveSubscriber = "__remove_subscriber";
+        public const string k_SysCommand_RemovePublisher = "__remove_publisher";
+        public const string k_SysCommand_RemoveRosService = "__remove_ros_service";
+        public const string k_SysCommand_RemoveUnityService = "__remove_unity_service";
 
         public abstract string Command
         {
@@ -53,6 +56,11 @@ namespace Unity.Robotics.ROSTCPConnector
         }
     }
 
+    public struct SysCommand_Topic
+    {
+        public string topic;
+    }
+
     public struct SysCommand_TopicAndType
     {
         public string topic;
@@ -79,27 +87,11 @@ namespace Unity.Robotics.ROSTCPConnector
         public string[] types;
     }
 
-    public class SysCommandPublisherRegistration : SysCommand
+    public struct SysCommand_PublisherRegistration
     {
-        [SerializeField] public string topic;
-        [SerializeField] public string message_name;
-        [SerializeField] public int queue_size;
-        [SerializeField] public bool latch;
-
-        public SysCommandPublisherRegistration(ROSPublisherBase rosPublisher) : this(
-            rosPublisher.TopicName, rosPublisher.RosMessageName, rosPublisher.QueueSize, rosPublisher.Latch)
-        {
-        }
-
-        public SysCommandPublisherRegistration(string topic, string messageName, int queueSize, bool latch)
-        {
-            this.topic = topic;
-            message_name = messageName;
-            queue_size = queueSize;
-            this.latch = latch;
-        }
-
-        public override string Command => k_SysCommand_Publish;
+        public string topic;
+        public string message_name;
+        public int queue_size;
+        public bool latch;
     }
-
 }

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs

@@ -0,0 +1,23 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector
+{
+    public class TFAttachment : MonoBehaviour
+    {
+        [SerializeField]
+        string m_FrameID;
+        public string FrameID { get => m_FrameID; set => m_FrameID = value; }
+        [SerializeField]
+        string m_TFTopic = "/tf";
+        public string TFTopic { get => m_TFTopic; set => m_TFTopic = value; }
+
+        void Start()
+        {
+            transform.parent = TFSystem.GetOrCreateInstance().GetTransformObject(m_FrameID, m_TFTopic).transform;
+            transform.localPosition = Vector3.zero;
+            transform.localRotation = Quaternion.identity;
+        }
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFAttachment.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: f9ca0c4d88f90d140ab1d4dd9e4e8e1c
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs

@@ -0,0 +1,187 @@
+using System.Collections.Generic;
+using UnityEngine;
+
+// Represents a transform - position and rotation.
+//(Like the Unity Transform class, but without the GameObject baggage that comes with it.)
+public struct TFFrame
+{
+    public Vector3 translation;
+    public Quaternion rotation;
+    public static TFFrame identity = new TFFrame(Vector3.zero, Quaternion.identity);
+
+    public TFFrame(Vector3 translation, Quaternion rotation)
+    {
+        this.translation = translation;
+        this.rotation = rotation;
+    }
+
+    public Vector3 TransformPoint(Vector3 point)
+    {
+        return translation + rotation * point;
+    }
+
+    public Vector3 InverseTransformPoint(Vector3 point)
+    {
+        return Quaternion.Inverse(rotation) * (point - translation);
+    }
+
+    public TFFrame Compose(TFFrame child)
+    {
+        return new TFFrame(TransformPoint(child.translation), rotation * child.rotation);
+    }
+
+    public static TFFrame Lerp(TFFrame a, TFFrame b, float lerp)
+    {
+        return new TFFrame
+        {
+            translation = Vector3.Lerp(a.translation, b.translation, lerp),
+            rotation = Quaternion.Lerp(a.rotation, b.rotation, lerp)
+        };
+    }
+}
+
+// Represents a transform frame changing over time.
+public class TFStream
+{
+    public string Name { get; private set; }
+    public string TFTopic { get; private set; }
+    public TFStream Parent { get; private set; }
+    public IEnumerable<TFStream> Children => m_Children;
+
+    // oldest first
+    List<long> m_Timestamps = new List<long>();
+    // same order as m_Timestamps
+    List<TFFrame> m_Frames = new List<TFFrame>();
+    List<TFStream> m_Children = new List<TFStream>();
+
+    // a gameobject at the last known position of this tfstream
+    GameObject m_GameObject;
+    public GameObject GameObject => m_GameObject;
+
+    public TFStream(TFStream parent, string name, string tfTopic)
+    {
+        Name = name;
+        TFTopic = tfTopic;
+        m_GameObject = new GameObject(name);
+        SetParent(parent);
+    }
+
+    public void SetParent(TFStream newParent)
+    {
+        if (Parent == newParent)
+            return;
+
+        if (Parent != null)
+        {
+            Parent.m_Children.Remove(this);
+        }
+
+        if (newParent != null)
+        {
+            m_GameObject.transform.parent = newParent.m_GameObject.transform;
+            newParent.m_Children.Add(this);
+        }
+        else
+        {
+            m_GameObject.transform.parent = null;
+        }
+        Parent = newParent;
+    }
+
+    public void Add(long timestamp, Vector3 translation, Quaternion rotation)
+    {
+        TFFrame newEntry = new TFFrame(translation, rotation);
+        // most likely case: we're just adding a newer transform to the end of the list
+        if (m_Timestamps.Count == 0 || m_Timestamps[m_Timestamps.Count - 1] < timestamp)
+        {
+            m_Timestamps.Add(timestamp);
+            m_Frames.Add(newEntry);
+            m_GameObject.transform.localPosition = translation;
+            m_GameObject.transform.localRotation = rotation;
+        }
+        else
+        {
+            int index = m_Timestamps.BinarySearch(timestamp);
+            if (index < 0)
+            {
+                // no preexisting entry, but ~index gives us the position to insert the new entry
+                m_Timestamps.Insert(~index, timestamp);
+                m_Frames.Insert(~index, newEntry);
+            }
+            else
+            {
+                // we found an existing entry at the same timestamp!? Just replace the old one, I guess.
+                m_Frames[index] = newEntry;
+            }
+        }
+
+        // for now, just a lazy way to keep the buffer from growing infinitely: every 50 updates, discard the oldest 50
+        if (m_Timestamps.Count > 100)
+        {
+            m_Timestamps.RemoveRange(0, 50);
+            m_Frames.RemoveRange(0, 50);
+        }
+    }
+
+    public TFFrame GetLocalTF(long time = 0)
+    {
+        // this stream has no data at all, so just report identity.
+        if (m_Frames.Count == 0)
+            return TFFrame.identity;
+
+        // if time is 0, just get the newest position
+        if (time == 0)
+            return m_Frames[m_Frames.Count - 1];
+
+        int index = m_Timestamps.BinarySearch(time);
+        if (index >= 0)
+        {
+            // no problem, we have an entry at this time
+            return m_Frames[index];
+        }
+
+        index = ~index;
+        if (index == 0)
+        {
+            // older than our first entry: just use the first one
+            return m_Frames[0];
+        }
+        else if (index == m_Frames.Count)
+        {
+            // newer than our last entry: just use the last one
+            return m_Frames[m_Frames.Count - 1];
+        }
+        else
+        {
+            // between two entries: interpolate
+            float lerpValue = (time - m_Timestamps[index - 1]) / (float)(m_Timestamps[index] - m_Timestamps[index - 1]);
+            return TFFrame.Lerp(m_Frames[index - 1], m_Frames[index], lerpValue);
+        }
+    }
+
+    public TFFrame GetWorldTF(long time = 0)
+    {
+        TFFrame parent;
+        if (Parent != null)
+            parent = Parent.GetWorldTF(time);
+        else
+            parent = TFFrame.identity;
+
+        return parent.Compose(GetLocalTF(time));
+    }
+
+    // Can we safely stop polling for updates to a transform at this time?
+    public bool IsTimeStable(long time)
+    {
+        if (time == 0) // time 0 ("use the newest data") is never stable
+            return false;
+
+        if (m_Timestamps.Count == 0 || m_Timestamps[0] > time || m_Timestamps[m_Timestamps.Count - 1] < time)
+            return false;
+
+        if (Parent != null && !Parent.IsTimeStable(time))
+            return false;
+
+        return true;
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFStream.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 2c8edcde9e9616e4e8d671e1c7a642bb
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs

@@ -0,0 +1,194 @@
+using System;
+using System.Collections.Generic;
+using RosMessageTypes.BuiltinInterfaces;
+using RosMessageTypes.Std;
+using RosMessageTypes.Tf2;
+using Unity.Robotics.ROSTCPConnector;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using Unity.Robotics.ROSTCPConnector.ROSGeometry;
+using UnityEngine;
+
+public class TFSystem
+{
+    public static TFSystem instance { get; private set; }
+    Dictionary<string, TFTopicState> m_TFTopics = new Dictionary<string, TFTopicState>();
+
+    class TFTopicState
+    {
+        string m_TFTopic;
+        Dictionary<string, TFStream> m_TransformTable = new Dictionary<string, TFStream>();
+        List<Action<TFStream>> m_Listeners = new List<Action<TFStream>>();
+
+        public TFTopicState(string tfTopic = "/tf")
+        {
+            m_TFTopic = tfTopic;
+            ROSConnection.GetOrCreateInstance().Subscribe<TFMessageMsg>(tfTopic, ReceiveTF);
+        }
+
+        public TFStream GetOrCreateTFStream(string frame_id)
+        {
+            TFStream tf;
+            while (frame_id.EndsWith("/"))
+                frame_id = frame_id.Substring(0, frame_id.Length - 1);
+
+            var slash = frame_id.LastIndexOf('/');
+            var singleName = slash == -1 ? frame_id : frame_id.Substring(slash + 1);
+            if (!m_TransformTable.TryGetValue(singleName, out tf) || tf == null)
+            {
+                if (slash <= 0)
+                {
+                    // there's no slash, or only an initial slash - just create a new root object
+                    // (set the parent later if and when we learn more)
+                    tf = new TFStream(null, singleName, m_TFTopic);
+                }
+                else
+                {
+                    var parent = GetOrCreateTFStream(frame_id.Substring(0, slash));
+                    tf = new TFStream(parent, singleName, m_TFTopic);
+                }
+
+                m_TransformTable[singleName] = tf;
+                NotifyChanged(tf);
+            }
+            else if (slash > 0 && tf.Parent == null)
+            {
+                tf.SetParent(GetOrCreateTFStream(frame_id.Substring(0, slash)));
+            }
+
+            return tf;
+        }
+
+        public void ReceiveTF(TFMessageMsg message)
+        {
+            foreach (var tf_message in message.transforms)
+            {
+                var frame_id = tf_message.header.frame_id + "/" + tf_message.child_frame_id;
+                var tf = GetOrCreateTFStream(frame_id);
+                tf.Add(
+                    tf_message.header.stamp.ToLongTime(),
+                    tf_message.transform.translation.From<FLU>(),
+                    tf_message.transform.rotation.From<FLU>()
+                );
+                NotifyChanged(tf);
+            }
+        }
+
+        public IEnumerable<string> GetTransformNames()
+        {
+            return m_TransformTable.Keys;
+        }
+
+        public IEnumerable<TFStream> GetTransforms()
+        {
+            return m_TransformTable.Values;
+        }
+
+        public TFStream GetTransformStream(string frame_id)
+        {
+            TFStream result = null;
+            m_TransformTable.TryGetValue(frame_id, out result);
+            return result;
+        }
+
+        public void AddListener(Action<TFStream> callback)
+        {
+            m_Listeners.Add(callback);
+        }
+
+        public void NotifyChanged(TFStream stream)
+        {
+            foreach (Action<TFStream> callback in m_Listeners)
+            {
+                callback(stream);
+            }
+        }
+
+        public void NotifyAllChanged()
+        {
+            foreach (var stream in m_TransformTable.Values)
+                NotifyChanged(stream);
+        }
+    }
+
+    private TFSystem()
+    {
+
+    }
+
+    public static TFSystem GetOrCreateInstance()
+    {
+        if (instance != null)
+            return instance;
+
+        ROSConnection ros = ROSConnection.GetOrCreateInstance();
+        instance = new TFSystem();
+        foreach (string s in ros.TFTopics)
+        {
+            instance.GetOrCreateTFTopic(s);
+        }
+        return instance;
+    }
+
+    public IEnumerable<string> GetTransformNames(string tfTopic = "/tf")
+    {
+        return GetOrCreateTFTopic(tfTopic).GetTransformNames();
+    }
+
+    public IEnumerable<TFStream> GetTransforms(string tfTopic = "/tf")
+    {
+        return GetOrCreateTFTopic(tfTopic).GetTransforms();
+    }
+
+    public void AddListener(Action<TFStream> callback, bool notifyAllStreamsNow = true, string tfTopic = "/tf")
+    {
+        TFTopicState state = GetOrCreateTFTopic(tfTopic);
+        state.AddListener(callback);
+        if (notifyAllStreamsNow)
+            state.NotifyAllChanged();
+    }
+
+    public void NotifyAllChanged(TFStream stream)
+    {
+        GetOrCreateTFTopic(stream.TFTopic).NotifyAllChanged();
+    }
+
+    public TFFrame GetTransform(HeaderMsg header, string tfTopic = "/tf")
+    {
+        return GetTransform(header.frame_id, header.stamp.ToLongTime(), tfTopic);
+    }
+
+    public TFFrame GetTransform(string frame_id, long time, string tfTopic = "/tf")
+    {
+        var stream = GetTransformStream(frame_id, tfTopic);
+        if (stream != null)
+            return stream.GetWorldTF(time);
+        return TFFrame.identity;
+    }
+
+    public TFFrame GetTransform(string frame_id, TimeMsg time, string tfTopic = "/tf")
+    {
+        return GetTransform(frame_id, time.ToLongTime(), tfTopic);
+    }
+
+    public TFStream GetTransformStream(string frame_id, string tfTopic = "/tf")
+    {
+        return GetOrCreateTFTopic(tfTopic).GetTransformStream(frame_id);
+    }
+
+    public GameObject GetTransformObject(string frame_id, string tfTopic = "/tf")
+    {
+        TFStream stream = GetOrCreateTFTopic(tfTopic).GetOrCreateTFStream(frame_id);
+        return stream.GameObject;
+    }
+
+    TFTopicState GetOrCreateTFTopic(string tfTopic)
+    {
+        TFTopicState tfTopicState;
+        if (!m_TFTopics.TryGetValue(tfTopic, out tfTopicState))
+        {
+            tfTopicState = new TFTopicState(tfTopic);
+            m_TFTopics[tfTopic] = tfTopicState;
+        }
+        return tfTopicState;
+    }
+}

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TFSystem.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 5fcaaf7eeccf1674c9b78fa6cfba67c8
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/Runtime/TcpConnector/TopicMessageSender.cs

@@ -0,0 +1,238 @@
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Linq;
+using System.Threading;
+using Unity.Robotics.ROSTCPConnector.MessageGeneration;
+using UnityEngine;
+
+namespace Unity.Robotics.ROSTCPConnector
+{
+    public class TopicMessageSender : OutgoingMessageSender
+    {
+        public string RosMessageName { get; private set; }
+
+        public string TopicName { get; private set; }
+
+        public int QueueSize { get; private set; }
+
+        //Messages waiting to be sent queue.
+        LinkedList<Message> m_OutgoingMessages = new LinkedList<Message>();
+
+        //Keeps track of how many outgoing messages were removed due to queue overflow.
+        //If a message is published but the queue is full, the counter is incremented, the
+        //first message in the queue is recycled and the message to publish is put on the end of the queue.
+        //If this is non 0, a SendTo call will decrement the counter instead of sending data.
+        int m_QueueOverflowUnsentCounter = 0;
+
+        //Latching - This message will be set if latching is enabled, and on reconnection, it will be sent again.
+        Message m_LastMessageSent = null;
+
+        //Optional, used if you want to pool messages and reuse them when they are no longer in use.
+        Queue<Message> m_InactiveMessagePool = new Queue<Message>();
+
+        public TopicMessageSender(string topicName, string rosMessageName, int queueSize)
+        {
+            if (queueSize < 1)
+            {
+                throw new Exception("Queue size must be greater than or equal to 1.");
+            }
+
+            TopicName = topicName;
+            RosMessageName = rosMessageName;
+            QueueSize = queueSize;
+        }
+
+        internal void Queue(Message message)
+        {
+            lock (m_OutgoingMessages)
+            {
+                if (m_OutgoingMessages.Count >= QueueSize)
+                {
+                    //Remove outgoing messages that don't fit in the queue.
+                    //Recycle the message if applicable
+                    TryRecycleMessage(m_OutgoingMessages.First.Value);
+                    //Update the overflow counter.
+                    m_QueueOverflowUnsentCounter++;
+                    m_OutgoingMessages.RemoveFirst();
+                }
+
+                //Add a new valid message to the end.
+                m_OutgoingMessages.AddLast(message);
+            }
+        }
+
+        SendToState GetMessageToSend(out Message messageToSend)
+        {
+            SendToState result = SendToState.NoMessageToSendError;
+            messageToSend = null;
+            lock (m_OutgoingMessages)
+            {
+                if (m_QueueOverflowUnsentCounter > 0)
+                {
+                    //This means that we can't send message to ROS as fast as we're generating them.
+                    //This could potentially be bad as it means that we are dropping messages!
+                    m_QueueOverflowUnsentCounter--;
+                    messageToSend = null;
+                    result = SendToState.QueueFullWarning;
+                }
+                else if (m_OutgoingMessages.Count > 0)
+                {
+                    //Retrieve the next message and populate messageToSend.
+                    messageToSend = m_OutgoingMessages.First.Value;
+                    m_OutgoingMessages.RemoveFirst();
+                    result = SendToState.Normal;
+                }
+            }
+
+            return result;
+        }
+
+        public bool PeekNextMessageToSend(out Message messageToSend)
+        {
+            bool result = false;
+            messageToSend = null;
+            lock (m_OutgoingMessages)
+            {
+                if (m_OutgoingMessages.Count > 0)
+                {
+                    messageToSend = m_OutgoingMessages.First.Value;
+                    result = true;
+                }
+            }
+
+
+            return result;
+        }
+
+        void SendMessageWithStream(MessageSerializer messageSerializer, Stream stream, Message message)
+        {
+            //Clear the serializer
+            messageSerializer.Clear();
+            //Prepare the data to send.
+            messageSerializer.Write(TopicName);
+            messageSerializer.SerializeMessageWithLength(message);
+            //Send via the stream.
+            messageSerializer.SendTo(stream);
+        }
+
+        public void PrepareLatchMessage()
+        {
+            if (m_LastMessageSent != null && !m_OutgoingMessages.Any())
+            {
+                //This topic is latching, so to mimic that functionality,
+                // the last sent message is sent again with the new connection.
+                m_OutgoingMessages.AddFirst(m_LastMessageSent);
+            }
+        }
+
+        public override SendToState SendInternal(MessageSerializer messageSerializer, Stream stream)
+        {
+            SendToState sendToState = GetMessageToSend(out Message toSend);
+            if (sendToState == SendToState.Normal)
+            {
+                SendMessageWithStream(messageSerializer, stream, toSend);
+
+                //Recycle the message (if applicable).
+                if (m_LastMessageSent != null && m_LastMessageSent != toSend)
+                {
+                    TryRecycleMessage(m_LastMessageSent);
+                }
+
+                m_LastMessageSent = toSend;
+            }
+
+            return sendToState;
+        }
+
+        public override void ClearAllQueuedData()
+        {
+            List<Message> toRecycle;
+            lock (m_OutgoingMessages)
+            {
+                toRecycle = new List<Message>(m_OutgoingMessages);
+                m_OutgoingMessages.Clear();
+                m_QueueOverflowUnsentCounter = 0;
+            }
+
+            foreach (Message messageToRecycle in toRecycle)
+            {
+                TryRecycleMessage(messageToRecycle);
+            }
+        }
+
+        void TryRecycleMessage(Message toRecycle)
+        {
+            if (!MessagePoolEnabled)
+            {
+                return;
+            }
+
+            //Add the message back to the pool.
+            AddMessageToPool(toRecycle);
+        }
+
+        #region Message Pooling
+
+        //Whether you want to pool messages for reuse (Used to reduce GC calls).
+        volatile bool m_MessagePoolEnabled;
+
+        public bool MessagePoolEnabled => m_MessagePoolEnabled;
+
+        public void SetMessagePoolEnabled(bool enabled)
+        {
+            if (m_MessagePoolEnabled == enabled)
+                return;
+
+            m_MessagePoolEnabled = enabled;
+            if (!m_MessagePoolEnabled)
+            {
+                lock (m_InactiveMessagePool)
+                {
+                    m_InactiveMessagePool.Clear();
+                }
+            }
+        }
+
+        public void AddMessageToPool(Message messageToRecycle)
+        {
+            Debug.Assert(MessagePoolEnabled,
+                "Adding a message to a message pool that is not enabled, please set MessagePoolEnabled to true.");
+
+            lock (m_InactiveMessagePool)
+            {
+                if (MessagePoolEnabled && m_InactiveMessagePool.Count < (QueueSize + 5))
+                {
+                    //Make sure we're only pooling a reasonable amount.
+                    //We shouldn't need any more than the queue size plus a little.
+                    m_InactiveMessagePool.Enqueue(messageToRecycle);
+                }
+            }
+        }
+
+        /**
+         * @return a message of type T full of garbage data, be sure to update it accordingly.
+         */
+        public Message GetMessageFromPool()
+        {
+            Message result = null;
+            SetMessagePoolEnabled(true);
+            lock (m_InactiveMessagePool)
+            {
+                if (m_InactiveMessagePool.Count > 0)
+                {
+                    result = m_InactiveMessagePool.Dequeue();
+                }
+            }
+
+            if (result == null)
+            {
+                result = Activator.CreateInstance<Message>();
+            }
+
+            return result;
+        }
+
+        #endregion
+    }
+}

com.unity.robotics.ros-tcp-connector/Tools.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: bea5d2e440e289646985b5afccde8e04
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

com.unity.robotics.ros-tcp-connector/package.json

@@ -1,6 +1,6 @@
 {
   "name": "com.unity.robotics.ros-tcp-connector",
-  "version": "0.5.0-preview",
+  "version": "0.6.0-preview",
   "displayName": "ROS TCP Connector",
   "description": "Bridge components and message generation allowing Unity to communicate with ROS and ROS2 services",
   "unity": "2020.2",

com.unity.robotics.visualizations/DefaultVisualizationSuite.prefab.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 7e8ecfd17cd58c247916864b74797f3f
+PrefabImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: