416 lines
13 KiB
C#
416 lines
13 KiB
C#
using Newtonsoft.Json;
|
|
using Newtonsoft.Json.Linq;
|
|
using Palmmedia.ReportGenerator.Core;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Net.Http;
|
|
using System.Net.Sockets;
|
|
using System.Text;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using UdpClientLib;
|
|
using UnityEngine;
|
|
|
|
public interface IProgramModel
|
|
{
|
|
|
|
}
|
|
|
|
public class ProgramModel : IProgramModel
|
|
{
|
|
private string tcpBaseUrl;
|
|
private string udpBaseUrl;
|
|
HttpClient httpClient = new HttpClient();
|
|
private SingleTcpClient tcpClient;
|
|
private SingleUdpClient udpClientForHttp;
|
|
public UdpClientManager manager = new UdpClientManager();
|
|
|
|
private List<JobInfoDTO> allProgramsCache = new List<JobInfoDTO>();
|
|
public RobotProgram CurrentProgram { get; private set; }
|
|
|
|
private RobotData robotData;
|
|
|
|
private readonly object lockObject = new object();
|
|
private bool hasNewData;
|
|
public bool IsUdpLoopRunning = false;
|
|
public bool IsMoving;
|
|
public CancellationTokenSource cancellationTokenSource;
|
|
private Vector3 startMovementPosition;
|
|
|
|
public ProgramModel(string hostip, int tcpPort, int udpPort)
|
|
{
|
|
tcpBaseUrl = $"http://{hostip}:{tcpPort}";
|
|
udpBaseUrl = $"http://{hostip}:{udpPort}";
|
|
udpClientForHttp = manager.AddClient("Udp-client2", hostip, udpPort);
|
|
}
|
|
|
|
public async Task InitializeAsync()
|
|
{
|
|
await LoadAllPrograms();
|
|
hasNewData = false;
|
|
IsUdpLoopRunning = true;
|
|
IsMoving = false;
|
|
return;
|
|
}
|
|
|
|
public bool IsNewDataAvailable()
|
|
{
|
|
lock (lockObject)
|
|
{
|
|
return hasNewData;
|
|
}
|
|
}
|
|
|
|
public RobotData GetLatestRobotData()
|
|
{
|
|
lock (lockObject)
|
|
{
|
|
hasNewData = false; // 데이터를 읽었으므로 플래그를 내림
|
|
return robotData;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 프로그램 생성/불러오기 시스템
|
|
/// </summary>
|
|
|
|
public async Task<bool> CheckProgramExists(string jobProgramName)
|
|
{
|
|
string requestUri = $"{tcpBaseUrl}/file_manager/file_exist?pathname=project/jobs/{jobProgramName}";
|
|
HttpResponseMessage result = await httpClient.GetAsync(requestUri);
|
|
string jsonResponse = await result.Content.ReadAsStringAsync();
|
|
|
|
return jsonResponse.Equals("true");
|
|
}
|
|
|
|
public async Task<bool> CreateNewProgram(string userInputId)
|
|
{
|
|
if (string.IsNullOrEmpty(userInputId)) return false;
|
|
|
|
string newProgramId = $"{userInputId}.job";
|
|
|
|
if (await CheckProgramExists(newProgramId))
|
|
{
|
|
Debug.LogError("파일이 이미 존재합니다.");
|
|
return false;
|
|
}
|
|
else
|
|
Debug.Log($"{newProgramId} 생성");
|
|
|
|
|
|
string robotModelName;
|
|
|
|
try
|
|
{
|
|
robotModelName = await GetRobotModelNameAsync();
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogError($"로봇 모델명을 가져오는 데 실패했습니다: {e.Message}");
|
|
return false;
|
|
}
|
|
|
|
NewJobRequestDTO newJob = new NewJobRequestDTO
|
|
{
|
|
fname = newProgramId,
|
|
model_name = robotModelName,
|
|
n_add_ax = 0
|
|
};
|
|
|
|
string jsonString = JsonConvert.SerializeObject(newJob);
|
|
HttpContent jsonPayload = new StringContent(jsonString, Encoding.UTF8, "application/json");
|
|
|
|
string requestUri = $"{tcpBaseUrl}/project/jobs/create_job";
|
|
try
|
|
{
|
|
HttpResponseMessage result = await httpClient.PostAsync(requestUri, jsonPayload);
|
|
if (result.IsSuccessStatusCode)
|
|
{
|
|
await LoadProgram(userInputId);
|
|
return true;
|
|
}
|
|
else
|
|
return false;
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogError($"프로그램 생성 실패: {userInputId}, {e.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private async Task<string> GetRobotModelNameAsync()
|
|
{
|
|
string requestUri = $"{tcpBaseUrl}/project/rgen";
|
|
|
|
HttpResponseMessage result = await httpClient.GetAsync(requestUri);
|
|
string jsonResponse = await result.Content.ReadAsStringAsync();
|
|
|
|
JObject data = JObject.Parse(jsonResponse);
|
|
|
|
string modelName = (string)data.SelectToken("robot_model");
|
|
|
|
if (string.IsNullOrEmpty(modelName))
|
|
{
|
|
throw new Exception("로봇 상태 API 응답에서 모델명을 찾을 수 없습니다.");
|
|
}
|
|
|
|
return modelName;
|
|
}
|
|
|
|
public async Task<bool> GetRobotMotorStateAsync()
|
|
{
|
|
string requestUri = $"{tcpBaseUrl}/project/rgen";
|
|
|
|
HttpResponseMessage result = await httpClient.GetAsync(requestUri);
|
|
string jsonResponse = await result.Content.ReadAsStringAsync();
|
|
|
|
JObject data = JObject.Parse(jsonResponse);
|
|
|
|
int motorState = (int)data.SelectToken("enable_state");
|
|
if (motorState == 2 || motorState == 256)
|
|
return true;
|
|
else if (motorState == 1)
|
|
return false;
|
|
else
|
|
{
|
|
throw new Exception("로봇 상태 API 응답에서 모터 상태를 찾을 수 없습니다.");
|
|
}
|
|
}
|
|
|
|
public async Task GetTCPAsync(CancellationToken token)
|
|
{
|
|
while (IsUdpLoopRunning)
|
|
{
|
|
try
|
|
{
|
|
//string requestUri = $"{udpBaseUrl}/project/robot/po_cur";
|
|
string requestUri = $"{tcpBaseUrl}/project/robot/po_cur";
|
|
|
|
HttpResponseMessage result = await httpClient.GetAsync(requestUri);
|
|
string jsonResponse = await result.Content.ReadAsStringAsync();
|
|
|
|
var tempRobotData = JsonConvert.DeserializeObject<RobotData>(jsonResponse, 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); // 에러 시 더 긴 대기
|
|
}
|
|
}
|
|
}
|
|
|
|
public async Task<bool> LoadProgram(string programId)
|
|
{
|
|
string requestUri = $"{tcpBaseUrl}/file_manager/files?pathname=project/jobs/{programId}&common";
|
|
|
|
HttpResponseMessage result = await httpClient.GetAsync(requestUri);
|
|
string rawTextContent = await result.Content.ReadAsStringAsync();
|
|
|
|
if (string.IsNullOrEmpty(rawTextContent))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
CurrentProgram = new RobotProgram(programId, rawTextContent);
|
|
|
|
return true;
|
|
}
|
|
|
|
private async Task LoadAllPrograms()
|
|
{
|
|
allProgramsCache.Clear();
|
|
string jsonResponse = null;
|
|
string wrappedJson = null;
|
|
|
|
try
|
|
{
|
|
HttpResponseMessage result = await httpClient.GetAsync($"{tcpBaseUrl}/project/jobs_info");
|
|
jsonResponse = await result.Content.ReadAsStringAsync();
|
|
|
|
wrappedJson = $"{{\"jobs\":{jsonResponse}}}";
|
|
JobListWrapper wrapper = JsonUtility.FromJson<JobListWrapper>(wrappedJson);
|
|
|
|
if (wrapper != null && wrapper.jobs != null)
|
|
{
|
|
allProgramsCache = wrapper.jobs;
|
|
}
|
|
else
|
|
{
|
|
Debug.LogError("서버에서 job 목록을 파싱할 수 없습니다: " + jsonResponse);
|
|
}
|
|
}
|
|
catch (ArgumentException e)
|
|
{
|
|
Debug.LogError($"JSON 파싱 오류: {e.Message}");
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogError($"LoadAllPrograms 알 수 없는 오류: {e.Message}");
|
|
}
|
|
}
|
|
|
|
public List<string> GetAllProgramIds()
|
|
{
|
|
List<string> ids = new List<string>();
|
|
foreach (var jobInfo in allProgramsCache)
|
|
{
|
|
ids.Add(jobInfo.fname);
|
|
}
|
|
return ids;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 로봇 위치 기록 시스템
|
|
/// </summary>
|
|
public async Task<bool> SavePointToProgramAsync(RobotData pointData, int index = -1)
|
|
{
|
|
if (CurrentProgram == null)
|
|
{
|
|
Debug.LogError("저장할 프로그램이 로드되지 않았습니다.");
|
|
return false;
|
|
}
|
|
|
|
// DTO(전송 객체) 생성
|
|
// (서버가 RobotData와 index, 프로그램 ID를 어떻게 받는지에 따라 수정 필요)
|
|
var payload = new
|
|
{
|
|
programId = CurrentProgram.ProgramId,
|
|
indexToUpdate = index, // -1이면 새 포인트, 0 이상이면 해당 인덱스 수정
|
|
pose = pointData
|
|
};
|
|
string jsonPayload = JsonConvert.SerializeObject(payload);
|
|
HttpContent content = new StringContent(jsonPayload, Encoding.UTF8, "application/json");
|
|
|
|
string requestUri = (index == -1)
|
|
? $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/ins_cmd_line"
|
|
: $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/pose_modify";
|
|
|
|
try
|
|
{
|
|
HttpResponseMessage result = await httpClient.PostAsync(requestUri, content);
|
|
if (result.IsSuccessStatusCode)
|
|
{
|
|
// 서버 저장이 성공하면, 메모리(CurrentProgram)에도 반영
|
|
if (index == -1)
|
|
CurrentProgram.AddStep(pointData); // RobotProgram에 새 RobotMoveStep 추가
|
|
else
|
|
CurrentProgram.UpdateStep(index, pointData); // RobotProgram의 해당 스텝 갱신
|
|
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogError($"포인트 저장 실패: {e.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// 서버에 포인트 삭제 요청
|
|
public async Task<bool> DeletePointFromProgramAsync(int index)
|
|
{
|
|
if (CurrentProgram == null) return false;
|
|
|
|
var payload = new { programId = CurrentProgram.ProgramId, indexToDelete = index };
|
|
string jsonPayload = JsonConvert.SerializeObject(payload);
|
|
HttpContent content = new StringContent(jsonPayload, Encoding.UTF8, "application/json");
|
|
|
|
string requestUri = $"{tcpBaseUrl}/project/jobs/{CurrentProgram.ProgramId}/del_cmd_line";
|
|
|
|
try
|
|
{
|
|
HttpResponseMessage result = await httpClient.PostAsync(requestUri, content);
|
|
if (result.IsSuccessStatusCode)
|
|
{
|
|
// 서버 삭제 성공 시, 메모리(CurrentProgram)에서도 삭제
|
|
CurrentProgram.DeleteStep(index);
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogError($"포인트 삭제 실패: {e.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// 실시간 로봇 TCP 이동
|
|
public async Task StreamPoseToRobotUdpAsync(RobotData pose)
|
|
{
|
|
try
|
|
{
|
|
byte[] udpPacket = ConvertPoseToPacket(pose);
|
|
|
|
await udpClientForHttp.SendBytesAsync(udpPacket);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
Debug.LogWarning($"UDP 스트리밍 실패: {e.Message}");
|
|
}
|
|
}
|
|
|
|
private byte[] ConvertPoseToPacket(RobotData pose)
|
|
{
|
|
using (MemoryStream stream = new MemoryStream())
|
|
{
|
|
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();
|
|
}
|
|
}
|
|
|
|
//사용자 tcp 드래깅 후 제어기로 이동 명령 하달
|
|
public async Task<bool> StartMovement(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 udpClientForHttp.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;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void OnDestroy()
|
|
{
|
|
IsUdpLoopRunning = false;
|
|
}
|
|
} |