hansb102318/HDRobotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

<fix> 로봇좌표 TCP전달 수정

Browse Source
This commit is contained in:
SOOBEEN HAN
2025-11-06 15:28:16 +09:00
parent 23e3a34837
commit 033c6aeb8e
12 changed files with 681 additions and 272 deletions
Download Patch File Download Diff File Expand all files Collapse all files

166
Assets/Scripts/Model/ProgramModel.cs
View File

@@ -6,11 +6,13 @@ using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Net.Http;
using System.Net.Mail;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using UdpClientLib;
using UnityEditor.PackageManager;
using UnityEngine;
public interface IProgramModel
@@ -23,7 +25,8 @@ public class ProgramModel : IProgramModel
private string tcpBaseUrl;
private string udpBaseUrl;
HttpClient httpClient = new HttpClient();
private SingleUdpClient udpClientForHttp;
private SingleTcpClient tcpClient;
private SingleUdpClient udpClient;
public UdpClientManager manager = new UdpClientManager();
private List<JobInfoDTO> allProgramsCache = new List<JobInfoDTO>();
@@ -34,14 +37,22 @@ public class ProgramModel : IProgramModel
private readonly object lockObject = new object();
private bool hasNewData;
public bool IsMoving;
public bool isError;
private Vector3 startMovementPosition;
public CancellationTokenSource cancellationTokenSource;
public ProgramModel(string hostip, int tcpPort, int udpPort, RobotController robotController)
{
tcpBaseUrl = $"http://{hostip}:{tcpPort}";
udpBaseUrl = $"http://{hostip}:{udpPort}";
udpClientForHttp = manager.AddClient("Udp-client2", hostip, udpPort);
_ = HandleAsyncWork(hostip, tcpPort, udpPort);
}
public async Task HandleAsyncWork(string hostip, int tcpPort, int udpPort)
{
tcpClient = new SingleTcpClient();
await tcpClient.ConnectAsync("Tcp-Client", hostip, tcpPort);
udpClient = manager.AddClient("Udp-client", hostip, udpPort);
manager.PrintStatus();
}
public async Task InitializeAsync()
@@ -49,7 +60,8 @@ public class ProgramModel : IProgramModel
await LoadAllPrograms();
hasNewData = false;
IsMoving = false;
cancellationTokenSource = new CancellationTokenSource();
isError = false;
return;
}
@@ -118,7 +130,7 @@ public class ProgramModel : IProgramModel
string jsonString = JsonConvert.SerializeObject(newJob);
HttpContent jsonPayload = new StringContent(jsonString, Encoding.UTF8, "application/json");
string requestUri = $"{tcpBaseUrl}/project/jobs/create_job";
try
{
@@ -177,6 +189,20 @@ public class ProgramModel : IProgramModel
}
}
public async Task<int> GetRobotToolNum()
{
string requestUri = $"{tcpBaseUrl}/project/rgen";
HttpResponseMessage result = await httpClient.GetAsync(requestUri);
string jsonResponse = await result.Content.ReadAsStringAsync();
JObject data = JObject.Parse(jsonResponse);
int toolNum = (int)data.SelectToken("tool_no");
return toolNum;
}
public async Task GetTCPAsync(CancellationToken token)
{
while (!token.IsCancellationRequested)
@@ -276,8 +302,7 @@ public class ProgramModel : IProgramModel
return false;
}
// DTO(전송 객체) 생성
// (서버가 RobotData와 index, 프로그램 ID를 어떻게 받는지에 따라 수정 필요)
// 전송 객체 생성
var payload = new
{
programId = CurrentProgram.ProgramId,
@@ -329,7 +354,7 @@ public class ProgramModel : IProgramModel
HttpResponseMessage result = await httpClient.PostAsync(requestUri, content);
if (result.IsSuccessStatusCode)
{
// 서버 삭제 성공 시, 메모리(CurrentProgram)에서도 삭제
// 서버 삭제 성공 시, 메모리에서도 삭제
CurrentProgram.DeleteStep(index);
return true;
}
@@ -342,50 +367,26 @@ public class ProgramModel : IProgramModel
}
}
// 실시간 로봇 TCP 이동
public async Task StreamPoseUdpAsync(RobotData pose)
{
try
{
byte[] udpPacket = ConvertPoseToPacket(pose);
if (udpClientForHttp != null)
{
await udpClientForHttp.SendBytesAsync(udpPacket);
}
else
{
Debug.LogWarning("UDP 클라이언트가 연결되지 않았습니다.");
}
}
catch (Exception e)
{
Debug.LogWarning($"UDP 스트리밍 실패: {e.Message}");
}
}
// 타겟 포지션으로 이동
public async Task<bool> MoveToPoseTcpAsync(RobotData pose)
public async Task<bool> MoveToPoseTcpAsync(Vector3 position)
{
try
{
string jsonString = $"{{\"pose_tg\":{{\"crd\":\"robot\",\"_type\":\"Pose\",\"mechinfo\":1,\"x\":{pose.x},\"y\":{pose.y},\"z\":{pose.z}, \"rx\":{pose.rx}, \"ry\":{pose.ry}, \"rz\":{pose.rz}}}}}";
HttpContent jsonPayload = new StringContent(jsonString, Encoding.UTF8, "application/json");
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));
HttpResponseMessage result = await httpClient.PostAsync("/project/robot/move_to_pose_manual", jsonPayload);
string 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}}}}}");
if (result.IsSuccessStatusCode)
if (jsonResponse.Contains("200"))
{
Debug.Log("TCP POST (Move) 명령 전송 성공");
this.startMovementPosition = new Vector3(pose.x, pose.y, pose.z);
this.IsMoving = true;
IsMoving = true;
return true;
}
else
{
string errorResponse = await result.Content.ReadAsStringAsync();
Debug.LogError($"TCP POST (Move) 실패 ({result.StatusCode}): {errorResponse}");
Debug.LogError($"TCP POST (Move) 실패");
return false;
}
}
@@ -399,74 +400,71 @@ public class ProgramModel : IProgramModel
// TCP POST 이동 명령 체크
public async Task StartMovementCheckLoopAsync(CancellationToken token)
{
while (!token.IsCancellationRequested)
try
{
if (IsMoving)
while (!token.IsCancellationRequested)
{
try
if (IsMoving)
{
await udpClientForHttp.SendFilledBytesAsync(new Dictionary<int, byte> { { 2, 0x20 } });
await udpClient.SendFilledBytesAsync(new Dictionary<int, byte> { { 2, 0x20 } });
await Task.Delay(100);
RobotData currentPose = null;
lock (lockObject)
{
currentPose = this.robotData;
}
lock (lockObject) { currentPose = this.robotData; }
if (currentPose != null)
{
bool isApproximatelyX = Mathf.Approximately(startMovementPosition.x, Convert.ToSingle(Math.Round(currentPose.x, 2)));
bool isApproximatelyY = Mathf.Approximately(startMovementPosition.y, Convert.ToSingle(Math.Round(currentPose.y, 2)));
bool isApproximatelyZ = Mathf.Approximately(startMovementPosition.z, Convert.ToSingle(Math.Round(currentPose.z, 2)));
if (isApproximatelyX && isApproximatelyY && isApproximatelyZ)
{
IsMoving = false; // 도착 완료
Debug.Log("TCP Move: 목표 지점 도착 완료.");
IsMoving = false;
}
}
await Task.Delay(100, token); // 100ms마다 확인
}
catch (TaskCanceledException)
else
{
Debug.Log("Movement Check 루프가 중지되었습니다.");
break;
await Task.Delay(500, token);
}
catch (Exception e)
{
Debug.Log($"Movement Check 루프 오류: {e.Message}");
IsMoving = false; // 오류 발생 시 루프 중단
await Task.Delay(1000, token);
}
}
else
{
// (IsMoving = false일 때는 1초간 대기)
await Task.Delay(1000, token);
}
}
}
private byte[] ConvertPoseToPacket(RobotData pose)
{
using (MemoryStream stream = new MemoryStream())
catch (TaskCanceledException)
{
using (BinaryWriter writer = new BinaryWriter(stream))
{
writer.Write(pose.x);
writer.Write(pose.y);
writer.Write(pose.z);
writer.Write(pose.rx);
writer.Write(pose.ry);
writer.Write(pose.rz);
}
return stream.ToArray();
Debug.Log("MovementCheckLoopAsync Canceled.");
}
catch (Exception e)
{
Debug.LogError($"MovementCheckLoopAsync Error: {e.Message}\n{e.StackTrace}");
}
}
void OnDestroy()
//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()
{
cancellationTokenSource?.Cancel();
cancellationTokenSource?.Dispose();
return await tcpClient.SendGetRequestAsync("/project/robot/moving_to_pose_manual");
}
}