HDRobotics/Assets/Scripts/Network/CommunicationScript.cs

using System;
using System.Collections.Generic;
using System.Threading;
using System.Threading.Tasks;
using Newtonsoft.Json;
using UdpClientLib;
using UnityEngine;

public class CommunicationScript : MonoBehaviour
{

    [Header("로봇 모델 참조")]
    public Transform robotModel;        // 전체 로봇
    public Transform[] robotAxes;       // 각 축별 transform

    public UdpClientManager manager = new UdpClientManager();

    private SingleTcpClient tcpClient;

    private SingleUdpClient udpClient;
    private SingleUdpClient udpClientForHttp;

    private RobotData robotData;

    private readonly object lockObject = new object();
    private bool hasNewData;

    public CancellationTokenSource cancellationTokenSource;

    public bool isMoving;

    public bool isError;

    private Vector3 startMovementPosition;


    void Start()
    {
        hasNewData = false;
        isMoving = false;
        isError = false;
        cancellationTokenSource = new CancellationTokenSource();
        _ = HandleAsyncWork();
    }

    private async Task HandleAsyncWork()
    {
        tcpClient = new SingleTcpClient();
        await tcpClient.ConnectAsync("Tcp-Client", "127.0.0.1", 8888);
        udpClient = manager.AddClient("Udp-client", "127.0.0.1", 50003);
        udpClientForHttp = manager.AddClient("Udp-client2", "127.0.0.1", 50000);
        manager.PrintStatus();
        _ = Task.Run(async () => await CurrrentPositionLoopAsync());
        _ = Task.Run(async () => await MovementLoopAsync());
        //_ = Task.Run(async () => await GetMovementState());
    }
    
    //로봇 포지션 정보 가져오기
    private async Task CurrrentPositionLoopAsync()
    {
        while (!cancellationTokenSource.Token.IsCancellationRequested)
        {
            try
            {
                await udpClientForHttp.SendGetRequestAsync("/project/robot/po_cur");
                string jsonResponse = udpClientForHttp.jsonResponse;

                if (jsonResponse.Contains("\"_type\" : \"Pose\""))
                {
                    var pasrsingJsonResponse = HttpResponseParser.ExtractJsonFromHttpResponse(jsonResponse, "udp");
                    var tempRobotData = JsonConvert.DeserializeObject<RobotData>(pasrsingJsonResponse, new JsonSerializerSettings { CheckAdditionalContent = false });

                    lock (lockObject)
                    {
                        robotData = tempRobotData;
                        hasNewData = true;
                    }
                }

                await Task.Delay(50);
            }
            catch (System.Exception e)
            {
                Debug.Log(e);
                await Task.Delay(1000); // 에러 시 더 긴 대기
            }
        }
    }

    // 포지션 정보를 기반으로 로봇 축 정보 업데이트
    void FixedUpdate()
    {
        lock (lockObject)
        {
            if (hasNewData)
            {
                robotAxes[0].localRotation = Quaternion.AngleAxis(-1 * robotData[0], Vector3.up);
                robotAxes[1].localRotation = Quaternion.AngleAxis(-1 * (robotData[1] - 90), Vector3.forward);
                robotAxes[2].localRotation = Quaternion.AngleAxis(-1 * robotData[2], Vector3.forward);
                robotAxes[3].localRotation = Quaternion.AngleAxis(robotData[3], Vector3.right);
                robotAxes[4].localRotation = Quaternion.AngleAxis(-1 * robotData[4], Vector3.forward);
                robotAxes[5].localRotation = Quaternion.AngleAxis(robotData[5], Vector3.right);
                hasNewData = false;
            }
        }

    }

    //사용자 tcp 드래깅 후 제어기로 이동 명령 하달
    public async Task<bool> StratMovement(Vector3 position)
    {
        startMovementPosition.x = Convert.ToSingle(Math.Round(-1 * position.x * 1000, 2));
        startMovementPosition.y = Convert.ToSingle(Math.Round(-1 * position.z * 1000, 2));
        startMovementPosition.z = Convert.ToSingle(Math.Round(position.y * 1000, 2));
        var jsonResponse = await tcpClient.SendPostRequestAsync("/project/robot/move_to_pose_manual", $"{{\"pose_tg\":{{\"crd\":\"robot\",\"_type\":\"Pose\",\"mechinfo\":1,\"x\":{startMovementPosition.x},\"y\":{startMovementPosition.y},\"z\":{startMovementPosition.z}, \"rx\":{robotData.rx}, \"ry\":{robotData.ry}, \"rz\":{robotData.rz}}}}}");
        return jsonResponse.Contains("200");
    }

    //타겟 포지션 도달까지 이동 명령
    private async Task MovementLoopAsync()
    {
        while (!cancellationTokenSource.Token.IsCancellationRequested)
        {
            if (isMoving)
            {
                await udpClient.SendFilledBytesAsync(new Dictionary<int, byte> { { 2, 0x20 } });
                await Task.Delay(100);
                bool isApproximatelyX = Mathf.Approximately(startMovementPosition.x, Convert.ToSingle(Math.Round(robotData.x, 2)));
                bool isApproximatelyY = Mathf.Approximately(startMovementPosition.y, Convert.ToSingle(Math.Round(robotData.y, 2)));
                bool isApproximatelyZ = Mathf.Approximately(startMovementPosition.z, Convert.ToSingle(Math.Round(robotData.z, 2)));
                if (isApproximatelyX && isApproximatelyY && isApproximatelyZ)
                {
                    isMoving = false;
                }
            }

        }
    }

    //타겟 포지션이 로봇 이동 범위 초과시 에러 처리
    private async Task GetMovementState()
    {
        while (!cancellationTokenSource.Token.IsCancellationRequested)
        {
            try
            {
                var jsonResponse = await tcpClient.SendGetRequestAsync("/project/rgen");
                var pasrsingJsonResponse = HttpResponseParser.ExtractJsonFromHttpResponse(jsonResponse);
                var tempRobotData = JsonConvert.DeserializeObject<RobotStateData>(pasrsingJsonResponse, new JsonSerializerSettings { CheckAdditionalContent = false });

                jsonResponse = await tcpClient.SendGetRequestAsync($"/logManager/search?cat_p=E&id_min={Convert.ToInt32(tempRobotData.eid_last_err) - 3}&id_max={tempRobotData.eid_last_err}");
                pasrsingJsonResponse = HttpResponseParser.ExtractJsonFromHttpResponse(jsonResponse);
                tempRobotData = JsonConvert.DeserializeObject<RobotStateData>(pasrsingJsonResponse, new JsonSerializerSettings { CheckAdditionalContent = false });

                isError = tempRobotData != null && tempRobotData.code != null && (tempRobotData.code.Contains("228") || tempRobotData.code.Contains("6037"));
                Debug.Log(isError);
                await Task.Delay(100);
            }
            catch (System.Exception)
            {
            }
        }
    }

    //이동 중 판단
    public async Task<string> GetMovingState()
    {
        return await tcpClient.SendGetRequestAsync("/project/robot/moving_to_pose_manual");
    }


    void OnDestroy()
    {
        cancellationTokenSource?.Cancel();
        cancellationTokenSource?.Dispose();
    }
}