AIRO-1381 Version check (#210)

Author: LaurieCheers-unity

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

@@ -15,6 +15,9 @@ namespace Unity.Robotics.ROSTCPConnector
 {
     public class ROSConnection : MonoBehaviour
     {
+        public const string k_Version = "v0.7.0";
+        public const string k_CompatibleVersionPrefix = "v0.7.";
+
         // Variables required for ROS communication
         [SerializeField]
         [FormerlySerializedAs("hostName")]
@@ -633,6 +636,32 @@ void ReceiveSysCommand(string topic, string json)
         {
             switch (topic)
             {
+                case SysCommand.k_SysCommand_Handshake:
+                    {
+                        var handshakeCommand = JsonUtility.FromJson<SysCommand_Handshake>(json);
+                        if (handshakeCommand.version == null)
+                        {
+                            Debug.LogError($"Corrupted or unreadable ROS-TCP-Endpoint version data! Expected: {k_Version}");
+                        }
+                        else if (!handshakeCommand.version.StartsWith(k_CompatibleVersionPrefix))
+                        {
+                            Debug.LogError($"Incompatible ROS-TCP-Endpoint version: {handshakeCommand.version}. Expected: {k_Version}");
+                        }
+
+                        var handshakeMetadata = JsonUtility.FromJson<SysCommand_Handshake_Metadata>(handshakeCommand.metadata);
+#if ROS2
+                        if (handshakeMetadata.protocol != "ROS2")
+                        {
+                            Debug.LogError($"Incompatible protocol: ROS-TCP-Endpoint is using {handshakeMetadata.protocol}, but Unity is in ROS2 mode. Switch it from the Robotics/Ros Settings menu.");
+                        }
+#else
+                        if (handshakeMetadata.protocol != "ROS1")
+                        {
+                            Debug.LogError($"Incompatible protocol: ROS-TCP-Endpoint is using {handshakeMetadata.protocol}, but Unity is in ROS1 mode. Switch it from the Robotics/Ros Settings menu.");
+                        }
+#endif
+                    }
+                    break;
                 case SysCommand.k_SysCommand_Log:
                     {
                         var logCommand = JsonUtility.FromJson<SysCommand_Log>(json);
@@ -843,12 +872,23 @@ static async Task ConnectionThread(
 
         static async Task ReaderThread(int readerIdx, NetworkStream networkStream, ConcurrentQueue<Tuple<string, byte[]>> queue, int sleepMilliseconds, CancellationToken token)
         {
+            // First message should be the handshake
+            Tuple<string, byte[]> handshakeContent = await ReadMessageContents(networkStream, sleepMilliseconds, token);
+            if (handshakeContent.Item1 == SysCommand.k_SysCommand_Handshake)
+            {
+                ROSConnection.m_HasConnectionError = false;
+                queue.Enqueue(handshakeContent);
+            }
+            else
+            {
+                Debug.LogError($"Invalid ROS-TCP-Endpoint version detected: 0.6.0 or older. Expected: {k_Version}.");
+            }
+
             while (!token.IsCancellationRequested)
             {
                 try
                 {
                     Tuple<string, byte[]> content = await ReadMessageContents(networkStream, sleepMilliseconds, token);
-                    // Debug.Log($"Message {content.Item1} received");
                     ROSConnection.m_HasConnectionError = false;
 
                     if (content.Item1 != "") // ignore keepalive messages

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

@@ -7,6 +7,7 @@ namespace Unity.Robotics.ROSTCPConnector
 {
     public abstract class SysCommand
     {
+        public const string k_SysCommand_Handshake = "__handshake";
         public const string k_SysCommand_Log = "__log";
         public const string k_SysCommand_Warning = "__warn";
         public const string k_SysCommand_Error = "__error";
@@ -67,6 +68,22 @@ public struct SysCommand_TopicAndType
         public string message_name;
     }
 
+    // For backwards compatibility, we encode the handshake in two stages:
+    // Stage 1 - which must NEVER change - is just a version string and a metadata string.
+    public struct SysCommand_Handshake
+    {
+        public string version;
+        public string metadata;
+    }
+
+    // Stage 2 is the json encoded contents of the metadata string.
+    // Because this structure may change with future versions of ROS TCP Connector, we only decode it
+    // after checking the version number is correct.
+    public struct SysCommand_Handshake_Metadata
+    {
+        public string protocol; // "ROS1" or "ROS2"
+    }
+
     public struct SysCommand_Log
     {
         public string text;