unity2moveit2/unity-project/Assets/Scripts/Tests/RealROSConnectionTest.cs

using UnityEngine;
using UnityMoveIt2.Communication;
using Unity.Robotics.ROSTCPConnector;

namespace UnityMoveIt2.Tests
{
    /// <summary>
    /// 真实ROS连接测试组件 / Real ROS Connection Test Component
    /// 附加到GameObject上以测试真实的ROS-TCP通信 / Attach to GameObject to test real ROS-TCP communication
    /// </summary>
    public class RealROSConnectionTest : MonoBehaviour
    {
        [Header("连接配置 / Connection Configuration")]
        [Tooltip("ROS2服务器IP / ROS2 Server IP")]
        public string rosIP = "127.0.0.1";

        [Tooltip("ROS2服务器端口 / ROS2 Server Port")]
        public int rosPort = 10000;

        [Tooltip("启动时自动连接 / Auto connect on start")]
        public bool autoConnect = true;

        [Tooltip("显示详细日志 / Verbose logging")]
        public bool verboseLogging = true;

        [Header("测试配置 / Test Configuration")]
        [Tooltip("连接保持时间(秒) / Keep alive duration (seconds)")]
        public float keepAliveDuration = 30f;

        [Tooltip("心跳间隔(秒) / Heartbeat interval (seconds)")]
        public float heartbeatInterval = 5f;

        [Header("状态显示 / Status Display")]
        [SerializeField] private bool isConnected = false;
        [SerializeField] private float connectionTime = 0f;
        [SerializeField] private int heartbeatCount = 0;
        [SerializeField] private string lastError = "";

        private ROSTCPBridge rosBridge;
        private ROSConnection rosConnection;
        private float connectedSince = 0f;
        private float lastHeartbeat = 0f;

        #region Unity生命周期 / Unity Lifecycle

        private void Awake()
        {
            Log("=== RealROSConnectionTest 初始化 / Initialized ===");
        }

        private void Start()
        {
            // 查找或创建ROSTCPBridge
            rosBridge = FindObjectOfType<ROSTCPBridge>();
            if (rosBridge == null)
            {
                LogWarning("场景中未找到ROSTCPBridge组件 / ROSTCPBridge not found in scene");
                LogWarning("将手动创建ROSConnection / Creating ROSConnection manually");

                ConnectManually();
            }
            else
            {
                Log("找到ROSTCPBridge组件 / Found ROSTCPBridge component");

                // 订阅事件
                rosBridge.OnConnected += OnROSConnected;
                rosBridge.OnDisconnected += OnROSDisconnected;
                rosBridge.OnConnectionError += OnROSConnectionError;

                if (autoConnect)
                {
                    Log("等待ROSTCPBridge连接... / Waiting for ROSTCPBridge to connect...");
                }
            }
        }

        private void Update()
        {
            if (isConnected)
            {
                connectionTime = Time.time - connectedSince;

                // 发送心跳
                if (Time.time - lastHeartbeat >= heartbeatInterval)
                {
                    SendHeartbeat();
                    lastHeartbeat = Time.time;
                }

                // 自动断开
                if (keepAliveDuration > 0 && connectionTime >= keepAliveDuration)
                {
                    Log($"已达到保持时间 {keepAliveDuration} 秒,断开连接 / Reached keep-alive duration, disconnecting");
                    Disconnect();
                }
            }
        }

        private void OnDestroy()
        {
            if (rosBridge != null)
            {
                rosBridge.OnConnected -= OnROSConnected;
                rosBridge.OnDisconnected -= OnROSDisconnected;
                rosBridge.OnConnectionError -= OnROSConnectionError;
            }

            Disconnect();
        }

        #endregion

        #region 连接管理 / Connection Management

        /// <summary>
        /// 手动连接到ROS / Manually connect to ROS
        /// </summary>
        [ContextMenu("手动连接 / Manual Connect")]
        public void ConnectManually()
        {
            try
            {
                Log("======================================");
                Log("开始手动连接到ROS2 / Starting manual connection to ROS2");
                Log($"目标: {rosIP}:{rosPort}");
                Log("======================================");

                rosConnection = ROSConnection.GetOrCreateInstance();

                if (rosConnection == null)
                {
                    LogError("无法创建ROSConnection实例 / Failed to create ROSConnection");
                    return;
                }

                rosConnection.RosIPAddress = rosIP;
                rosConnection.RosPort = rosPort;
                rosConnection.ConnectOnStart = false;

                Log("调用Connect()... / Calling Connect()...");
                rosConnection.Connect();

                // 检查连接状态
                Invoke(nameof(CheckConnectionStatus), 1f);
            }
            catch (System.Exception ex)
            {
                LogError($"连接异常 / Connection exception: {ex.Message}");
                lastError = ex.Message;
            }
        }

        /// <summary>
        /// 断开连接 / Disconnect
        /// </summary>
        [ContextMenu("断开连接 / Disconnect")]
        public void Disconnect()
        {
            if (rosConnection != null)
            {
                Log("正在断开连接... / Disconnecting...");
                rosConnection.Disconnect();
            }

            isConnected = false;
            connectionTime = 0f;
            Log("✓ 已断开 / Disconnected");
        }

        private void CheckConnectionStatus()
        {
            if (rosConnection == null)
            {
                LogError("ROSConnection为null / ROSConnection is null");
                return;
            }

            bool hasError = rosConnection.HasConnectionError;

            if (!hasError)
            {
                OnROSConnected();
            }
            else
            {
                OnROSConnectionError("连接检查失败 / Connection check failed");
            }
        }

        #endregion

        #region 事件处理 / Event Handlers

        private void OnROSConnected()
        {
            isConnected = true;
            connectedSince = Time.time;
            connectionTime = 0f;
            lastError = "";
            heartbeatCount = 0;
            lastHeartbeat = Time.time;

            Log("======================================");
            Log("✓✓✓ 真实ROS连接建立成功! / Real ROS Connection Established! ✓✓✓");
            Log("======================================");
            Log($"服务器: {rosIP}:{rosPort}");
            Log($"连接时间: {System.DateTime.Now:HH:mm:ss}");
            Log($"保持时长: {keepAliveDuration}秒 / seconds");
            Log("======================================");
        }

        private void OnROSDisconnected()
        {
            isConnected = false;

            Log("======================================");
            Log("✗ 连接已断开 / Connection Disconnected");
            Log("======================================");
            Log($"连接持续时间: {connectionTime:F1}秒 / seconds");
            Log($"发送心跳次数: {heartbeatCount}");
            Log("======================================");
        }

        private void OnROSConnectionError(string error)
        {
            isConnected = false;
            lastError = error;

            Log("======================================");
            Log("✗✗✗ 连接错误! / Connection Error! ✗✗✗");
            Log("======================================");
            LogError($"错误信息 / Error: {error}");
            Log("======================================");
            LogError("可能的原因 / Possible causes:");
            LogError("1. ROS2服务未启动 / ROS2 service not started");
            LogError("2. IP或端口错误 / Wrong IP or port");
            LogError("3. 防火墙阻止 / Firewall blocking");
            LogError("4. Docker容器未运行 / Docker not running");
            Log("======================================");
            LogError("解决方案 / Solutions:");
            LogError("cd docker && docker-compose up -d");
            LogError("docker-compose ps");
            LogError("docker-compose logs ros2-unity");
            Log("======================================");
        }

        #endregion

        #region 测试功能 / Test Functions

        /// <summary>
        /// 发送心跳测试 / Send heartbeat test
        /// </summary>
        private void SendHeartbeat()
        {
            if (!isConnected || rosConnection == null)
                return;

            try
            {
                heartbeatCount++;

                // 创建简单的测试数据
                byte[] heartbeatData = System.Text.Encoding.UTF8.GetBytes($"HEARTBEAT_{heartbeatCount}_{Time.time}");

                rosConnection.SendMessage(heartbeatData);

                if (verboseLogging)
                {
                    Log($"❤ 心跳 #{heartbeatCount} 已发送 / Heartbeat sent ({heartbeatData.Length} bytes)");
                }
            }
            catch (System.Exception ex)
            {
                LogError($"心跳发送失败 / Heartbeat failed: {ex.Message}");
            }
        }

        /// <summary>
        /// 发送测试消息 / Send test message
        /// </summary>
        [ContextMenu("发送测试消息 / Send Test Message")]
        public void SendTestMessage()
        {
            if (!isConnected || rosConnection == null)
            {
                LogWarning("未连接,无法发送 / Not connected, cannot send");
                return;
            }

            try
            {
                string testMsg = $"TEST_MESSAGE_{System.DateTime.Now:HHmmss}";
                byte[] data = System.Text.Encoding.UTF8.GetBytes(testMsg);

                rosConnection.SendMessage(data);

                Log($"✓ 测试消息已发送 / Test message sent: {testMsg} ({data.Length} bytes)");
            }
            catch (System.Exception ex)
            {
                LogError($"发送失败 / Send failed: {ex.Message}");
            }
        }

        /// <summary>
        /// 运行完整连接测试 / Run full connection test
        /// </summary>
        [ContextMenu("运行完整测试 / Run Full Test")]
        public void RunFullTest()
        {
            StartCoroutine(FullTestCoroutine());
        }

        private System.Collections.IEnumerator FullTestCoroutine()
        {
            Log("\n\n");
            Log("██████████████████████████████████████████████");
            Log("█   真实ROS连接完整测试 / Real ROS Full Test  █");
            Log("██████████████████████████████████████████████");

            // 测试1: 连接
            Log("\n[测试 1/4] 建立连接 / Establishing Connection");
            ConnectManually();
            yield return new WaitForSeconds(2f);

            if (!isConnected)
            {
                LogError("✗ 测试失败: 无法连接 / Test Failed: Cannot connect");
                yield break;
            }
            Log("✓ 测试1通过: 连接成功 / Test 1 Passed");

            // 测试2: 保持连接
            Log("\n[测试 2/4] 连接保持 / Keep Alive");
            yield return new WaitForSeconds(5f);

            if (isConnected)
            {
                Log($"✓ 测试2通过: 连接保持 {connectionTime:F1}秒 / Test 2 Passed");
            }
            else
            {
                LogError("✗ 测试2失败: 连接断开 / Test 2 Failed");
                yield break;
            }

            // 测试3: 发送数据
            Log("\n[测试 3/4] 数据发送 / Data Send");
            for (int i = 0; i < 3; i++)
            {
                SendTestMessage();
                yield return new WaitForSeconds(1f);
            }
            Log("✓ 测试3完成: 发送3条消息 / Test 3 Complete: 3 messages sent");

            // 测试4: 断开连接
            Log("\n[测试 4/4] 断开连接 / Disconnection");
            Disconnect();
            yield return new WaitForSeconds(1f);
            Log("✓ 测试4完成: 已断开 / Test 4 Complete");

            Log("\n");
            Log("██████████████████████████████████████████████");
            Log("█      测试完成! / Test Complete!           █");
            Log("██████████████████████████████████████████████");
            Log($"总连接时间 / Total connection time: {connectionTime:F1}秒");
            Log($"发送心跳 / Heartbeats sent: {heartbeatCount}");
            Log("\n\n");
        }

        #endregion

        #region 日志 / Logging

        private void Log(string message)
        {
            if (verboseLogging)
            {
                Debug.Log($"[RealROSTest] {message}");
            }
        }

        private void LogWarning(string message)
        {
            Debug.LogWarning($"[RealROSTest] {message}");
        }

        private void LogError(string message)
        {
            Debug.LogError($"[RealROSTest] {message}");
        }

        #endregion

        #region GUI显示 / GUI Display

        private void OnGUI()
        {
            if (!verboseLogging) return;

            GUIStyle boxStyle = new GUIStyle(GUI.skin.box);
            boxStyle.alignment = TextAnchor.UpperLeft;
            boxStyle.fontSize = 14;
            boxStyle.normal.textColor = isConnected ? Color.green : Color.red;

            string statusText = "真实ROS连接状态 / Real ROS Connection Status\n";
            statusText += "==========================================\n";
            statusText += $"服务器 / Server: {rosIP}:{rosPort}\n";
            statusText += $"状态 / Status: {(isConnected ? "✓ 已连接 / Connected" : "✗ 未连接 / Disconnected")}\n";
            statusText += $"连接时长 / Duration: {connectionTime:F1}s\n";
            statusText += $"心跳次数 / Heartbeats: {heartbeatCount}\n";

            if (!string.IsNullOrEmpty(lastError))
            {
                statusText += $"错误 / Error: {lastError}\n";
            }

            GUI.Box(new Rect(10, 10, 450, 130), statusText, boxStyle);

            // 按钮
            if (GUI.Button(new Rect(10, 150, 100, 30), "连接 / Connect"))
            {
                ConnectManually();
            }

            if (GUI.Button(new Rect(120, 150, 100, 30), "断开 / Disconnect"))
            {
                Disconnect();
            }

            if (GUI.Button(new Rect(230, 150, 110, 30), "发送测试 / Send Test"))
            {
                SendTestMessage();
            }

            if (GUI.Button(new Rect(350, 150, 110, 30), "完整测试 / Full Test"))
            {
                RunFullTest();
            }
        }

        #endregion
    }
}