Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

..

Browse Source
This commit is contained in:
backuppc
2026-01-07 17:33:17 +09:00
parent 90340f4a7d
commit 9776205364
17 changed files with 186 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

186
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
View File

@@ -46,94 +46,122 @@ namespace AGVNavigationCore.Utils
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {LastNode.Id} 타입:{LastNode.Type} ({(int)LastNode.Type})");
//detail 경로 이동 예측 검증
for (int i = 0; i < pathResult.DetailedPath.Count - 1; i++)
{
var curNodeId = pathResult.DetailedPath[i].NodeId;
var nextNodeId = pathResult.DetailedPath[i + 1].NodeId;
////detail 경로 이동 예측 검증
//for (int i = 0; i < pathResult.DetailedPath.Count - 1; i++)
//{
// var curNodeId = pathResult.DetailedPath[i].NodeId;
// var nextNodeId = pathResult.DetailedPath[i + 1].NodeId;
var curNode = mapNodes?.FirstOrDefault(n => n.Id == curNodeId);
var nextNode = mapNodes?.FirstOrDefault(n => n.Id == nextNodeId);
// var curNode = mapNodes?.FirstOrDefault(n => n.Id == curNodeId);
// var nextNode = mapNodes?.FirstOrDefault(n => n.Id == nextNodeId);
if (curNode != null && nextNode != null)
{
MapNode prevNode = null;
AgvDirection prevDir = AgvDirection.Stop;
if (i == 0)
{
prevNode = pathResult.PrevNode;
prevDir = pathResult.PrevDirection;
}
else
{
var prevNodeId = pathResult.DetailedPath[i - 1].NodeId;
prevNode = mapNodes?.FirstOrDefault(n => n.Id == prevNodeId);
prevDir = pathResult.DetailedPath[i - 1].MotorDirection;
}
// if (curNode != null && nextNode != null)
// {
// MapNode prevNode = null;
// AgvDirection prevDir = AgvDirection.Stop;
// if (i == 0)
// {
// prevNode = pathResult.PrevNode;
// prevDir = pathResult.PrevDirection;
// }
// else
// {
// var prevNodeId = pathResult.DetailedPath[i - 1].NodeId;
// prevNode = mapNodes?.FirstOrDefault(n => n.Id == prevNodeId);
// prevDir = pathResult.DetailedPath[i - 1].MotorDirection;
// }
if (prevNode != null)
{
// DirectionalHelper를 사용하여 예상되는 다음 노드 확인
Console.WriteLine(
$"\n[ValidateDockingDirection] 경로 검증 단계 {i}:");
Console.WriteLine(
$" 이전→현재→다음: {prevNode.Id}({prevNode.RfidId}) → {curNode.Id}({curNode.RfidId}) → {nextNode.Id}({nextNode.RfidId})");
Console.WriteLine(
$" 현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})");
Console.WriteLine(
$" 이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}");
Console.WriteLine(
$" 마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}");
// if (prevNode != null)
// {
// // DirectionalHelper를 사용하여 예상되는 다음 노드 확인
// Console.WriteLine(
// $"\n[ValidateDockingDirection] 경로 검증 단계 {i}:");
// Console.WriteLine(
// $" 이전→현재→다음: {prevNode.Id}({prevNode.RfidId}) → {curNode.Id}({curNode.RfidId}) → {nextNode.Id}({nextNode.RfidId})");
// Console.WriteLine(
// $" 현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})");
// Console.WriteLine(
// $" 이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}");
// Console.WriteLine(
// $" 마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}");
var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
curNode,
prevNode,
prevDir,
pathResult.DetailedPath[i].MotorDirection,
pathResult.DetailedPath[i].MagnetDirection,
mapNodes
);
// var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// pathResult.DetailedPath[i].MagnetDirection,
// mapNodes
// );
Console.WriteLine(
$" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.Id ?? "null"}");
Console.WriteLine(
$" [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.Id}]");
// var expectedNextNodeL = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// PathFinding.Planning.MagnetDirection.Left,
// mapNodes
// );
if (expectedNextNode != null && !expectedNextNode.Id.Equals(nextNode.Id))
{
string error =
$"[DockingValidator] ⚠️ 경로 방향 불일치" +
$"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.Id ?? string.Empty)}] " +
$"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.Id}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
Console.WriteLine(
$"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
Console.WriteLine(
$" 이동 벡터:");
Console.WriteLine(
$" 이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})");
Console.WriteLine(
$" 현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})");
Console.WriteLine(
$" 현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})");
Console.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}");
return DockingValidationResult.CreateInvalid(
LastNode.Id,
LastNode.Type,
pathResult.DetailedPath[i].MotorDirection,
pathResult.DetailedPath[i].MotorDirection,
error);
// var expectedNextNodeR = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// PathFinding.Planning.MagnetDirection.Right,
// mapNodes
// );
// var expectedNextNodeS = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// PathFinding.Planning.MagnetDirection.Straight,
// mapNodes
// );
}
else
{
Console.WriteLine(
$" ✅ 경로 방향 일치!");
}
}
}
}
// Console.WriteLine(
// $" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.Id ?? "null"}");
// Console.WriteLine(
// $" [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.Id}]");
// if (expectedNextNode != null && !expectedNextNode.Id.Equals(nextNode.Id))
// {
// string error =
// $"[DockingValidator] ⚠️ 경로 방향 불일치" +
// $"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.Id ?? string.Empty)}] " +
// $"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.Id}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
// Console.WriteLine(
// $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
// Console.WriteLine(
// $" 이동 벡터:");
// Console.WriteLine(
// $" 이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})");
// Console.WriteLine(
// $" 현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})");
// Console.WriteLine(
// $" 현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})");
// Console.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}");
// return DockingValidationResult.CreateInvalid(
// LastNode.Id,
// LastNode.Type,
// pathResult.DetailedPath[i].MotorDirection,
// pathResult.DetailedPath[i].MotorDirection,
// error);
// }
// else
// {
// Console.WriteLine(
// $" ✅ 경로 방향 일치!");
// }
// }
// }
//}
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)