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

using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using System;

namespace UnityMoveIt2.Tests
{
    /// <summary>
    /// 简单ROS连接测试器 / Simple ROS Connection Tester
    /// 可以直接附加到Unity场景中的GameObject进行手动测试
    /// Can be attached to a GameObject in Unity scene for manual testing
    /// </summary>
    public class SimpleROSConnectionTester : MonoBehaviour
    {
        [Header("连接配置 / Connection Configuration")]
        [Tooltip("ROS2服务器IP地址 / ROS2 Server IP Address")]
        public string rosIP = "127.0.0.1";

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

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

        [Header("测试配置 / Test Configuration")]
        [Tooltip("显示详细日志 / Show verbose logs")]
        public bool verboseLogging = true;

        [Tooltip("连接保持时间(秒) / Connection keep-alive duration (seconds)")]
        public float keepAliveDuration = 10f;

        [Header("状态显示 / Status Display")]
        [SerializeField]
        private bool isConnected = false;

        [SerializeField]
        private float connectionTime = 0f;

        [SerializeField]
        private string lastError = "";

        private ROSConnection rosConnection;
        private float connectedSince = 0f;

        #region Unity生命周期 / Unity Lifecycle

        private void Awake()
        {
            Log("=== SimpleROSConnectionTester 初始化 / Initialized ===");
            Log($"目标服务器 / Target Server: {rosIP}:{rosPort}");
        }

        private void Start()
        {
            if (autoConnectOnStart)
            {
                ConnectToROS();
            }
            else
            {
                Log("自动连接已禁用,请手动调用ConnectToROS() / Auto-connect disabled, call ConnectToROS() manually");
            }
        }

        private void Update()
        {
            // 更新连接时间 / Update connection time
            if (isConnected)
            {
                connectionTime = Time.time - connectedSince;
            }
        }

        private void OnDestroy()
        {
            DisconnectFromROS();
        }

        #endregion

        #region 公共方法 / Public Methods

        /// <summary>
        /// 连接到ROS2服务器 / Connect to ROS2 Server
        /// </summary>
        [ContextMenu("连接到ROS / Connect to ROS")]
        public void ConnectToROS()
        {
            Log("======================================");
            Log("开始连接到ROS2 / Starting connection to ROS2");
            Log($"服务器地址 / Server Address: {rosIP}:{rosPort}");
            Log("======================================");

            try
            {
                // 获取或创建ROSConnection实例 / Get or create ROSConnection instance
                if (rosConnection == null)
                {
                    GameObject rosGO = GameObject.Find("ROSConnection");
                    if (rosGO == null)
                    {
                        rosGO = new GameObject("ROSConnection");
                    }

                    rosConnection = rosGO.GetComponent<ROSConnection>();
                    if (rosConnection == null)
                    {
                        rosConnection = rosGO.AddComponent<ROSConnection>();
                    }
                }

                // 配置连接参数 / Configure connection parameters
                rosConnection.RosIPAddress = rosIP;
                rosConnection.RosPort = rosPort;
                rosConnection.ConnectOnStart = false;

                // 执行连接 / Execute connection
                rosConnection.Connect();

                // 等待一小段时间验证连接 / Wait a moment to verify connection
                Invoke(nameof(CheckConnectionStatus), 1f);
            }
            catch (Exception ex)
            {
                LogError($"连接异常 / Connection Exception: {ex.Message}");
                lastError = ex.Message;
                isConnected = false;
            }
        }

        /// <summary>
        /// 断开ROS2连接 / Disconnect from ROS2
        /// </summary>
        [ContextMenu("断开连接 / Disconnect")]
        public void DisconnectFromROS()
        {
            if (rosConnection != null)
            {
                Log("正在断开ROS连接... / Disconnecting from ROS...");
                rosConnection.Disconnect();
                isConnected = false;
                connectionTime = 0f;
                Log("✓ 已断开连接 / ✓ Disconnected");
            }
        }

        /// <summary>
        /// 重新连接 / Reconnect
        /// </summary>
        [ContextMenu("重新连接 / Reconnect")]
        public void Reconnect()
        {
            DisconnectFromROS();
            Invoke(nameof(ConnectToROS), 1f);
        }

        /// <summary>
        /// 发送测试数据 / Send Test Data
        /// </summary>
        [ContextMenu("发送测试数据 / Send Test Data")]
        public void SendTestData()
        {
            if (!isConnected || rosConnection == null)
            {
                LogWarning("未连接,无法发送数据 / Not connected, cannot send data");
                return;
            }

            try
            {
                // 创建简单的测试数据 / Create simple test data
                byte[] testData = System.Text.Encoding.UTF8.GetBytes("Hello from Unity!");

                Log($"发送测试数据: {testData.Length} 字节 / Sending test data: {testData.Length} bytes");
                rosConnection.SendMessage(testData);

                Log("✓ 测试数据已发送 / ✓ Test data sent");
            }
            catch (Exception ex)
            {
                LogError($"发送数据失败 / Send data failed: {ex.Message}");
            }
        }

        /// <summary>
        /// 运行完整测试套件 / Run Full Test Suite
        /// </summary>
        [ContextMenu("运行完整测试 / Run Full Test")]
        public void RunFullTest()
        {
            StartCoroutine(FullTestCoroutine());
        }

        #endregion

        #region 私有方法 / Private Methods

        private void CheckConnectionStatus()
        {
            if (rosConnection != null)
            {
                bool hasError = rosConnection.HasConnectionError;

                if (!hasError)
                {
                    isConnected = true;
                    connectedSince = Time.time;
                    connectionTime = 0f;
                    lastError = "";

                    Log("======================================");
                    Log("✓✓✓ 连接成功! / Connection Successful! ✓✓✓");
                    Log("======================================");
                    Log($"服务器: {rosIP}:{rosPort}");
                    Log($"连接时间: {DateTime.Now:HH:mm:ss}");
                    Log("======================================");

                    // 自动断开连接(如果设置了保持时间)
                    // Auto-disconnect after keep-alive duration
                    if (keepAliveDuration > 0)
                    {
                        Log($"将在 {keepAliveDuration} 秒后自动断开 / Will auto-disconnect after {keepAliveDuration} seconds");
                        Invoke(nameof(DisconnectFromROS), keepAliveDuration);
                    }
                }
                else
                {
                    isConnected = false;
                    lastError = "Connection failed";

                    Log("======================================");
                    Log("✗✗✗ 连接失败! / Connection Failed! ✗✗✗");
                    Log("======================================");
                    LogError("可能的原因 / Possible reasons:");
                    LogError("1. ROS2服务未启动 / ROS2 service not started");
                    LogError("2. IP地址或端口错误 / Wrong IP or port");
                    LogError("3. 防火墙阻止连接 / Firewall blocking connection");
                    LogError("4. Docker容器未运行 / Docker container not running");
                    Log("======================================");
                    LogError("解决方案 / Solutions:");
                    LogError("运行: cd docker && docker-compose up -d");
                    LogError("检查: docker-compose ps");
                    LogError("日志: docker-compose logs ros2-unity");
                    Log("======================================");
                }
            }
        }

        private System.Collections.IEnumerator FullTestCoroutine()
        {
            Log("\n\n");
            Log("██████████████████████████████████████████████████");
            Log("█  开始完整测试套件 / Starting Full Test Suite  █");
            Log("██████████████████████████████████████████████████");

            // 测试1: 连接
            Log("\n[测试 1/3] 连接测试 / Connection Test");
            ConnectToROS();
            yield return new UnityEngine.WaitForSeconds(2f);

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

            // 测试2: 保持连接
            Log("\n[测试 2/3] 连接保持测试 / Keep-Alive Test");
            Log("保持连接3秒... / Keeping connection for 3 seconds...");
            yield return new UnityEngine.WaitForSeconds(3f);

            if (isConnected)
            {
                Log("✓ 测试2通过: 连接保持成功 / Test 2 Passed: Connection kept alive");
            }
            else
            {
                LogError("✗ 测试2失败: 连接断开 / Test 2 Failed: Connection dropped");
            }

            // 测试3: 发送数据
            Log("\n[测试 3/3] 数据发送测试 / Data Send Test");
            SendTestData();
            yield return new UnityEngine.WaitForSeconds(1f);
            Log("✓ 测试3完成: 数据已发送 / Test 3 Complete: Data sent");

            // 断开连接
            Log("\n[清理] 断开连接 / Cleanup: Disconnecting");
            DisconnectFromROS();
            yield return new UnityEngine.WaitForSeconds(0.5f);

            Log("\n");
            Log("██████████████████████████████████████████████████");
            Log("█     测试套件完成! / Test Suite Complete!      █");
            Log("██████████████████████████████████████████████████");
            Log($"总测试时间 / Total test time: {Time.time - (connectedSince - 6f):F2}秒 / seconds");
            Log("\n\n");
        }

        #endregion

        #region 日志方法 / Logging Methods

        private void Log(string message)
        {
            if (verboseLogging)
            {
                Debug.Log($"[ROS连接测试器] {message}");
            }
        }

        private void LogWarning(string message)
        {
            Debug.LogWarning($"[ROS连接测试器] {message}");
        }

        private void LogError(string message)
        {
            Debug.LogError($"[ROS连接测试器] {message}");
        }

        #endregion

        #region Inspector辅助 / Inspector Helpers

        private void OnGUI()
        {
            // 在Game视图显示状态 / Display status in Game view
            if (!verboseLogging) return;

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

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

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

            GUI.Box(new Rect(10, 10, 350, 100), statusText, style);

            // 添加按钮 / Add buttons
            if (GUI.Button(new Rect(10, 120, 100, 30), "连接 / Connect"))
            {
                ConnectToROS();
            }

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

            if (GUI.Button(new Rect(230, 120, 130, 30), "完整测试 / Full Test"))
            {
                RunFullTest();
            }
        }

        #endregion
    }
}