unity2moveit2/unity-project/Assets/Scripts/Communication/ROSHandshakeProtocol.cs

using System;
using System.Text;
using UnityEngine;

namespace UnityMoveIt2.Communication
{
    /// <summary>
    /// ROS-TCP-Connector握手协议实现 / ROS-TCP-Connector Handshake Protocol Implementation
    /// 实现与ros-tcp-endpoint的标准握手流程 / Implements standard handshake with ros-tcp-endpoint
    /// </summary>
    public static class ROSHandshakeProtocol
    {
        // ROS-TCP-Connector协议常量 / Protocol Constants
        private const string HANDSHAKE_HEADER = "ROS_TCP";
        private const int PROTOCOL_VERSION = 1;

        /// <summary>
        /// 创建握手消息 / Create Handshake Message
        ///
        /// ROS-TCP-Connector握手格式:
        /// Format: "ROS_TCP<version><connection_id>"
        /// - 固定头: "ROS_TCP" (7字节)
        /// - 协议版本: 整数 (4字节)
        /// - 连接ID: 字符串
        /// </summary>
        public static byte[] CreateHandshakeMessage()
        {
            try
            {
                // 生成唯一连接ID / Generate unique connection ID
                string connectionId = GenerateConnectionId();

                // 构建握手字符串 / Build handshake string
                string handshakeString = $"{HANDSHAKE_HEADER}{PROTOCOL_VERSION}{connectionId}";

                // 转换为字节数组 / Convert to byte array
                byte[] handshakeBytes = Encoding.UTF8.GetBytes(handshakeString);

                Debug.Log($"[ROSHandshake] 创建握手消息 / Creating handshake message");
                Debug.Log($"[ROSHandshake] 协议版本 / Protocol version: {PROTOCOL_VERSION}");
                Debug.Log($"[ROSHandshake] 连接ID / Connection ID: {connectionId}");
                Debug.Log($"[ROSHandshake] 消息长度 / Message length: {handshakeBytes.Length} bytes");

                return handshakeBytes;
            }
            catch (Exception ex)
            {
                Debug.LogError($"[ROSHandshake] 创建握手消息失败 / Failed to create handshake: {ex.Message}");
                return null;
            }
        }

        /// <summary>
        /// 创建简化的握手消息（兼容模式）/ Create Simplified Handshake (Compatibility Mode)
        /// 某些ROS端点可能只需要简单的标识字符串
        /// </summary>
        public static byte[] CreateSimpleHandshake()
        {
            try
            {
                // 最简单的握手：只发送标识
                string simpleHandshake = "UNITY_ROS_CLIENT";
                byte[] handshakeBytes = Encoding.UTF8.GetBytes(simpleHandshake);

                Debug.Log($"[ROSHandshake] 创建简化握手 / Creating simple handshake");
                Debug.Log($"[ROSHandshake] 内容 / Content: {simpleHandshake}");
                Debug.Log($"[ROSHandshake] 长度 / Length: {handshakeBytes.Length} bytes");

                return handshakeBytes;
            }
            catch (Exception ex)
            {
                Debug.LogError($"[ROSHandshake] 创建简化握手失败 / Failed to create simple handshake: {ex.Message}");
                return null;
            }
        }

        /// <summary>
        /// 创建JSON格式的握手消息 / Create JSON Format Handshake
        /// 用于某些需要JSON格式的ROS端点
        /// </summary>
        public static byte[] CreateJsonHandshake()
        {
            try
            {
                // JSON格式握手
                var handshakeJson = new HandshakeData
                {
                    op = "handshake",
                    client = "Unity",
                    version = PROTOCOL_VERSION.ToString(),
                    connection_id = GenerateConnectionId(),
                    timestamp = GetTimestamp()
                };

                string jsonString = JsonUtility.ToJson(handshakeJson);
                byte[] jsonBytes = Encoding.UTF8.GetBytes(jsonString);

                Debug.Log($"[ROSHandshake] 创建JSON握手 / Creating JSON handshake");
                Debug.Log($"[ROSHandshake] JSON: {jsonString}");
                Debug.Log($"[ROSHandshake] 长度 / Length: {jsonBytes.Length} bytes");

                return jsonBytes;
            }
            catch (Exception ex)
            {
                Debug.LogError($"[ROSHandshake] 创建JSON握手失败 / Failed to create JSON handshake: {ex.Message}");
                return null;
            }
        }

        /// <summary>
        /// 创建带长度前缀的握手消息 / Create Length-Prefixed Handshake
        /// 标准ROS-TCP协议：4字节长度 + 消息内容
        /// </summary>
        public static byte[] CreateLengthPrefixedHandshake()
        {
            try
            {
                // 创建握手内容
                string handshakeContent = $"{{\"op\":\"handshake\",\"client\":\"Unity\",\"version\":\"{PROTOCOL_VERSION}\"}}";
                byte[] contentBytes = Encoding.UTF8.GetBytes(handshakeContent);

                // 创建长度前缀（4字节，网络字节序）
                int messageLength = contentBytes.Length;
                byte[] lengthPrefix = BitConverter.GetBytes(messageLength);

                // 转换为网络字节序（大端序）
                if (BitConverter.IsLittleEndian)
                {
                    Array.Reverse(lengthPrefix);
                }

                // 组合：长度前缀 + 内容
                byte[] fullMessage = new byte[4 + contentBytes.Length];
                Array.Copy(lengthPrefix, 0, fullMessage, 0, 4);
                Array.Copy(contentBytes, 0, fullMessage, 4, contentBytes.Length);

                Debug.Log($"[ROSHandshake] 创建带长度前缀的握手 / Creating length-prefixed handshake");
                Debug.Log($"[ROSHandshake] 内容长度 / Content length: {messageLength}");
                Debug.Log($"[ROSHandshake] 总长度 / Total length: {fullMessage.Length} bytes");
                Debug.Log($"[ROSHandshake] 内容 / Content: {handshakeContent}");

                return fullMessage;
            }
            catch (Exception ex)
            {
                Debug.LogError($"[ROSHandshake] 创建带长度前缀握手失败 / Failed to create length-prefixed handshake: {ex.Message}");
                return null;
            }
        }

        /// <summary>
        /// 生成唯一连接ID / Generate Unique Connection ID
        /// </summary>
        private static string GenerateConnectionId()
        {
            // 使用Unity实例ID + 时间戳生成唯一ID
            string timestamp = DateTime.Now.ToString("yyyyMMddHHmmss");
            string randomPart = UnityEngine.Random.Range(1000, 9999).ToString();
            return $"Unity_{timestamp}_{randomPart}";
        }

        /// <summary>
        /// 获取时间戳 / Get Timestamp
        /// </summary>
        private static long GetTimestamp()
        {
            return DateTimeOffset.UtcNow.ToUnixTimeSeconds();
        }

        /// <summary>
        /// 验证握手响应 / Validate Handshake Response
        /// </summary>
        public static bool ValidateHandshakeResponse(byte[] response)
        {
            if (response == null || response.Length == 0)
            {
                Debug.LogWarning("[ROSHandshake] 收到空响应 / Received empty response");
                return false;
            }

            try
            {
                string responseString = Encoding.UTF8.GetString(response);
                Debug.Log($"[ROSHandshake] 收到响应 / Received response: {responseString}");

                // 检查响应是否包含成功标识
                bool isValid = responseString.Contains("OK") ||
                              responseString.Contains("success") ||
                              responseString.Contains("accepted") ||
                              responseString.Contains("handshake") ||
                              response.Length > 0; // 任何响应都视为成功

                if (isValid)
                {
                    Debug.Log("[ROSHandshake] ✓ 握手响应有效 / Handshake response valid");
                }
                else
                {
                    Debug.LogWarning("[ROSHandshake] ✗ 握手响应无效 / Handshake response invalid");
                }

                return isValid;
            }
            catch (Exception ex)
            {
                Debug.LogError($"[ROSHandshake] 验证响应失败 / Failed to validate response: {ex.Message}");
                return false;
            }
        }

        /// <summary>
        /// 创建保活消息 / Create Keep-Alive Message
        /// 定期发送以保持连接活跃
        /// </summary>
        public static byte[] CreateKeepAliveMessage()
        {
            try
            {
                string keepAlive = $"{{\"op\":\"keepalive\",\"timestamp\":{GetTimestamp()}}}";
                byte[] keepAliveBytes = Encoding.UTF8.GetBytes(keepAlive);

                Debug.Log($"[ROSHandshake] 创建保活消息 / Creating keep-alive message ({keepAliveBytes.Length} bytes)");

                return keepAliveBytes;
            }
            catch (Exception ex)
            {
                Debug.LogError($"[ROSHandshake] 创建保活消息失败 / Failed to create keep-alive: {ex.Message}");
                return null;
            }
        }

        /// <summary>
        /// 握手数据结构 / Handshake Data Structure
        /// </summary>
        [Serializable]
        private class HandshakeData
        {
            public string op;              // 操作类型 / Operation type
            public string client;          // 客户端标识 / Client identifier
            public string version;         // 协议版本 / Protocol version
            public string connection_id;   // 连接ID / Connection ID
            public long timestamp;         // 时间戳 / Timestamp
        }
    }
}