unity2moveit2/unity-project/Packages/com.unity.robotics.ros-tcp-connector/Runtime/ROSConnection.cs

using System;
using System.Net.Sockets;
using System.Threading;
using UnityEngine;

namespace Unity.Robotics.ROSTCPConnector
{
    /// <summary>
    /// 简化版ROS连接类 / Simplified ROS Connection Class
    /// 提供与ROS2的TCP通信功能 / Provides TCP communication with ROS2
    /// </summary>
    public class ROSConnection : MonoBehaviour
    {
        private static ROSConnection s_Instance;

        [Header("Connection Settings")]
        public string RosIPAddress = "127.0.0.1";
        public int RosPort = 10000;
        public bool ConnectOnStart = true;

        private TcpClient tcpClient;
        private NetworkStream stream;
        private Thread receiveThread;
        private bool isConnected = false;
        private bool shouldStop = false;

        public static ROSConnection GetOrCreateInstance()
        {
            if (s_Instance == null)
            {
                GameObject go = new GameObject("ROSConnection");
                s_Instance = go.AddComponent<ROSConnection>();
                DontDestroyOnLoad(go);
            }
            return s_Instance;
        }

        public static ROSConnection instance => GetOrCreateInstance();

        private void Awake()
        {
            if (s_Instance != null && s_Instance != this)
            {
                Destroy(gameObject);
                return;
            }
            s_Instance = this;
            DontDestroyOnLoad(gameObject);
        }

        private void Start()
        {
            if (ConnectOnStart)
            {
                Connect();
            }
        }

        public void Connect()
        {
            try
            {
                tcpClient = new TcpClient();
                tcpClient.Connect(RosIPAddress, RosPort);
                stream = tcpClient.GetStream();
                isConnected = true;

                Debug.Log($"[ROSConnection] Connected to ROS at {RosIPAddress}:{RosPort}");

                // Start receive thread
                receiveThread = new Thread(ReceiveData);
                receiveThread.IsBackground = true;
                receiveThread.Start();
            }
            catch (Exception e)
            {
                Debug.LogError($"[ROSConnection] Connection failed: {e.Message}");
                isConnected = false;
            }
        }

        public void Disconnect()
        {
            shouldStop = true;
            isConnected = false;

            if (stream != null)
            {
                stream.Close();
                stream = null;
            }

            if (tcpClient != null)
            {
                tcpClient.Close();
                tcpClient = null;
            }

            if (receiveThread != null && receiveThread.IsAlive)
            {
                receiveThread.Join(1000);
            }

            Debug.Log("[ROSConnection] Disconnected from ROS");
        }

        private void ReceiveData()
        {
            byte[] buffer = new byte[4096];
            while (!shouldStop && isConnected)
            {
                try
                {
                    if (stream != null && stream.DataAvailable)
                    {
                        int bytesRead = stream.Read(buffer, 0, buffer.Length);
                        if (bytesRead > 0)
                        {
                            // Process received data
                            // In a full implementation, this would deserialize ROS messages
                        }
                    }
                    Thread.Sleep(10);
                }
                catch (Exception e)
                {
                    Debug.LogError($"[ROSConnection] Receive error: {e.Message}");
                    break;
                }
            }
        }

        public void SendMessage(byte[] data)
        {
            if (!isConnected || stream == null)
            {
                Debug.LogWarning("[ROSConnection] Cannot send: not connected");
                return;
            }

            try
            {
                stream.Write(data, 0, data.Length);
            }
            catch (Exception e)
            {
                Debug.LogError($"[ROSConnection] Send error: {e.Message}");
            }
        }

        public bool HasConnectionError => !isConnected;

        private void OnDestroy()
        {
            Disconnect();
        }

        private void OnApplicationQuit()
        {
            Disconnect();
        }
    }
}