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

..

Browse Source
This commit is contained in:
backuppc
2025-10-27 12:00:59 +09:00
parent 1d65531b11
commit dbf81bfc60
28 changed files with 301 additions and 5716 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
Cs_HMI/AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj
View File

@@ -94,6 +94,7 @@
<DependentUpon>UnifiedAGVCanvas.cs</DependentUpon>
</Compile>
<Compile Include="Utils\DockingValidator.cs" />
<Compile Include="Utils\DirectionalHelper.cs" />
<Compile Include="Utils\LiftCalculator.cs" />
<Compile Include="Utils\ImageConverterUtil.cs" />
<Compile Include="Utils\AGVDirectionCalculator.cs" />

94
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs
View File

@@ -207,6 +207,9 @@ namespace AGVNavigationCore.Controls
{
DrawPath(g, _currentPath, Color.Purple);
// 경로 내 교차로 강조 표시
HighlightJunctionsInPath(g, _currentPath);
// AGVPathResult의 모터방향 정보가 있다면 향상된 경로 그리기
// 현재는 기본 PathResult를 사용하므로 향후 AGVPathResult로 업그레이드 시 활성화
// TODO: AGVPathfinder 사용시 AGVPathResult로 업그레이드
@@ -282,7 +285,96 @@ namespace AGVNavigationCore.Controls
pathPen.Dispose();
}
/// <summary>
/// 경로에 포함된 교차로(3개 이상의 노드가 연결된 노드)를 파란색으로 강조 표시
/// </summary>
private void HighlightJunctionsInPath(Graphics g, AGVPathResult path)
{
if (path?.Path == null || _nodes == null || _nodes.Count == 0)
return;
const int JUNCTION_CONNECTIONS = 3; // 교차로 판정 기준: 3개 이상의 연결
foreach (var nodeId in path.Path)
{
var node = _nodes.FirstOrDefault(n => n.NodeId == nodeId);
if (node == null) continue;
// 교차로 판정: 3개 이상의 노드가 연결된 경우
if (node.ConnectedNodes != null && node.ConnectedNodes.Count >= JUNCTION_CONNECTIONS)
{
DrawJunctionHighlight(g, node);
}
}
}
/// <summary>
/// 교차로 노드를 파란색 반투명 배경으로 강조 표시
/// </summary>
private void DrawJunctionHighlight(Graphics g, MapNode junctionNode)
{
if (junctionNode == null) return;
const int JUNCTION_HIGHLIGHT_RADIUS = 25; // 강조 표시 반경
// 파란색 반투명 브러시로 배경 원 그리기
using (var highlightBrush = new SolidBrush(Color.FromArgb(80, 70, 130, 200))) // 파란색 (70, 130, 200) 알파 80
using (var highlightPen = new Pen(Color.FromArgb(150, 100, 150, 220), 2)) // 파란 테두리
{
g.FillEllipse(
highlightBrush,
junctionNode.Position.X - JUNCTION_HIGHLIGHT_RADIUS,
junctionNode.Position.Y - JUNCTION_HIGHLIGHT_RADIUS,
JUNCTION_HIGHLIGHT_RADIUS * 2,
JUNCTION_HIGHLIGHT_RADIUS * 2
);
g.DrawEllipse(
highlightPen,
junctionNode.Position.X - JUNCTION_HIGHLIGHT_RADIUS,
junctionNode.Position.Y - JUNCTION_HIGHLIGHT_RADIUS,
JUNCTION_HIGHLIGHT_RADIUS * 2,
JUNCTION_HIGHLIGHT_RADIUS * 2
);
}
// 교차로 라벨 추가
DrawJunctionLabel(g, junctionNode);
}
/// <summary>
/// 교차로 라벨을 표시 (선택사항)
/// </summary>
private void DrawJunctionLabel(Graphics g, MapNode junctionNode)
{
if (junctionNode == null) return;
using (var font = new Font("Arial", 9, FontStyle.Bold))
using (var brush = new SolidBrush(Color.Blue))
{
var text = "교차로";
var textSize = g.MeasureString(text, font);
// 노드 위쪽에 라벨 표시
var labelX = junctionNode.Position.X - textSize.Width / 2;
var labelY = junctionNode.Position.Y - 35;
// 배경 박스 그리기
using (var bgBrush = new SolidBrush(Color.FromArgb(220, 255, 255, 200)))
{
g.FillRectangle(
bgBrush,
labelX - 3,
labelY - 3,
textSize.Width + 6,
textSize.Height + 6
);
}
g.DrawString(text, font, brush, labelX, labelY);
}
}
private void DrawNodesOnly(Graphics g)

61
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
View File

@@ -91,6 +91,11 @@ namespace AGVNavigationCore.PathFinding.Core
/// </summary>
public string DirectionChangeNode { get; set; }
/// <summary>
/// 경로계산시 사용했던 최초 이전 포인트 이전의 노드
/// </summary>
public MapNode PrevNode { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
@@ -110,6 +115,7 @@ namespace AGVNavigationCore.PathFinding.Core
RequiredDirectionChange = false;
DirectionChangeNode = string.Empty;
DockingValidation = DockingValidationResult.CreateNotRequired();
PrevNode = null;
}
/// <summary>
@@ -135,29 +141,7 @@ namespace AGVNavigationCore.PathFinding.Core
return result;
}
/// <summary>
/// 성공 결과 생성 (노드별 모터방향 정보 포함)
/// </summary>
/// <param name="path">경로</param>
/// <param name="commands">AGV 명령어 목록</param>
/// <param name="nodeMotorInfos">노드별 모터방향 정보</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<string> path, List<AgvDirection> commands, List<NodeMotorInfo> nodeMotorInfos, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<string>(path),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 실패 결과 생성
@@ -193,37 +177,6 @@ namespace AGVNavigationCore.PathFinding.Core
};
}
/// <summary>
/// 성공 결과 생성 (상세 경로용)
/// </summary>
/// <param name="detailedPath">상세 경로</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <param name="planDescription">계획 설명</param>
/// <param name="directionChange">방향 전환 여부</param>
/// <param name="changeNode">방향 전환 노드</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<NodeMotorInfo> detailedPath, float totalDistance, long calculationTimeMs, int exploredNodes, string planDescription, bool directionChange = false, string changeNode = null)
{
var path = detailedPath?.Select(n => n.NodeId).ToList() ?? new List<string>();
var result = new AGVPathResult
{
Success = true,
Path = path,
DetailedPath = detailedPath ?? new List<NodeMotorInfo>(),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes,
PlanDescription = planDescription ?? string.Empty,
RequiredDirectionChange = directionChange,
DirectionChangeNode = changeNode ?? string.Empty
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 경로 메트릭 계산

1
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
View File

@@ -403,6 +403,7 @@ namespace AGVNavigationCore.PathFinding.Core
// DetailedPath 설정
result.DetailedPath = combinedDetailedPath;
result.PrevNode = previousResult.PrevNode;
return result;
}

133
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -122,9 +122,15 @@ namespace AGVNavigationCore.PathFinding.Planning
//1.목적지까지의 최단거리 경로를 찾는다.
var pathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
pathResult.PrevNode = prevNode;
if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
//정방향/역방향 이동 시 다음 노드 확인
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, currentDirection, _mapNodes);
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, ReverseDirection, _mapNodes);
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
if (targetNode.DockDirection == DockingDirection.DontCare ||
(targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
@@ -135,6 +141,7 @@ namespace AGVNavigationCore.PathFinding.Planning
return pathResult;
}
//2-1 현재위치의 반대방향과 대상의 방향이 맞는 경우에도 그대로 사용가능하다.
//if (targetNode.DockDirection == DockingDirection.DontCare ||
// (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Backward) ||
@@ -147,17 +154,20 @@ namespace AGVNavigationCore.PathFinding.Planning
// return pathResult;
//}
//2-2 정방향/역방향 이동 시 다음 노드 확인
var nextNodeForward = GetNextNodeByDirection(startNode, prevNode, currentDirection, _mapNodes);
var nextNodeBackward = GetNextNodeByDirection(startNode, prevNode, ReverseDirection, _mapNodes);
//뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다.
if (nextNodeBackward.NodeId == pathResult.Path[1])
if (nextNodeBackward.NodeId == pathResult.Path[1] && targetNode.DockDirection == DockingDirection.Backward)
{
MakeDetailData(pathResult, ReverseDirection);
MakeMagnetDirection(pathResult);
return pathResult;
}
if(nextNodeForward.NodeId == pathResult.Path[1] && targetNode.DockDirection == DockingDirection.Forward)
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
return pathResult;
}
//if(nextNodeForward.NodeId == pathResult.Path[1])
//{
@@ -184,12 +194,27 @@ namespace AGVNavigationCore.PathFinding.Planning
//1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
var path1 = _basicPathfinder.FindPath(startNode.NodeId, JunctionInPath.NodeId);
path1.PrevNode = prevNode;
//다음좌표를 보고 정방향인지 역방향인지 체크한다.
if( nextNodeForward.NodeId.Equals( path1.Path[1]))
{
MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else if(nextNodeBackward.NodeId.Equals(path1.Path[1]))
{
MakeDetailData(path1, ReverseDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else return AGVPathResult.CreateFailure("교차로까지 계산된 경로에 현재 위치정보로 추측을 할 수 없습니다", 0, 0);
// path1의 상세 경로 정보 채우기 (모터 방향 설정)
MakeDetailData(path1, currentDirection);
//2.교차로 - 종료위치
var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId);
path2.PrevNode = prevNode;
MakeDetailData(path2, ReverseDirection);
//3.방향전환을 위환 대체 노드찾기
@@ -265,100 +290,6 @@ namespace AGVNavigationCore.PathFinding.Planning
}
}
/// <summary>
/// 현재 노드에서 주어진 방향(Forward/Backward)으로 이동할 때 다음 노드를 반환
/// </summary>
/// <param name="currentNode">현재 노드</param>
/// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
/// <param name="direction">이동 방향 (Forward 또는 Backward)</param>
/// <param name="allNodes">모든 맵 노드</param>
/// <returns>다음 노드 (또는 null)</returns>
private MapNode GetNextNodeByDirection(MapNode currentNode, MapNode prevNode, AgvDirection direction, List<MapNode> allNodes)
{
if (currentNode == null || prevNode == null || allNodes == null)
return null;
// 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
var connectedNodeIds = currentNode.ConnectedNodes;
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
return null;
var candidateNodes = allNodes.Where(n =>
connectedNodeIds.Contains(n.NodeId)
).ToList();
if (candidateNodes.Count == 0)
return null;
// Forward인 경우: 이전→현재 방향으로 계속 직진하는 노드 우선
// Backward인 경우: 이전→현재 방향의 반대로 이동하는 노드 우선
var movementVector = new PointF(
currentNode.Position.X - prevNode.Position.X,
currentNode.Position.Y - prevNode.Position.Y
);
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
if (movementLength < 0.001f)
return candidateNodes[0];
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 각 후보 노드에 대해 점수 계산
MapNode bestNode = null;
float bestScore = float.MinValue;
foreach (var candidate in candidateNodes)
{
var toNextVector = new PointF(
candidate.Position.X - currentNode.Position.X,
candidate.Position.Y - currentNode.Position.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
if (toNextLength < 0.001f)
continue;
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 내적 계산 (유사도: -1 ~ 1)
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float score;
if (direction == AgvDirection.Forward)
{
// Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
score = dotProduct;
}
else // Backward
{
// Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
score = -dotProduct;
}
if (score > bestScore)
{
bestScore = score;
bestNode = candidate;
}
}
return bestNode;
}
/// <summary>
/// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다

114
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs Normal file
View File

@@ -0,0 +1,114 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// AGV 방향 계산 헬퍼 유틸리티
/// 현재 위치에서 주어진 모터 방향으로 이동할 때 다음 노드를 계산
/// </summary>
public static class DirectionalHelper
{
/// <summary>
/// 현재 노드에서 주어진 방향(Forward/Backward)으로 이동할 때 다음 노드를 반환
/// </summary>
/// <param name="currentNode">현재 노드</param>
/// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
/// <param name="direction">이동 방향 (Forward 또는 Backward)</param>
/// <param name="allNodes">모든 맵 노드</param>
/// <returns>다음 노드 (또는 null)</returns>
public static MapNode GetNextNodeByDirection(
MapNode currentNode,
MapNode prevNode,
AgvDirection direction,
List<MapNode> allNodes)
{
if (currentNode == null || prevNode == null || allNodes == null)
return null;
// 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
var connectedNodeIds = currentNode.ConnectedNodes;
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
return null;
var candidateNodes = allNodes.Where(n =>
connectedNodeIds.Contains(n.NodeId)
).ToList();
if (candidateNodes.Count == 0)
return null;
// Forward인 경우: 이전→현재 방향으로 계속 직진하는 노드 우선
// Backward인 경우: 이전→현재 방향의 반대로 이동하는 노드 우선
var movementVector = new PointF(
currentNode.Position.X - prevNode.Position.X,
currentNode.Position.Y - prevNode.Position.Y
);
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
if (movementLength < 0.001f)
return candidateNodes[0];
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 각 후보 노드에 대해 점수 계산
MapNode bestNode = null;
float bestScore = float.MinValue;
foreach (var candidate in candidateNodes)
{
var toNextVector = new PointF(
candidate.Position.X - currentNode.Position.X,
candidate.Position.Y - currentNode.Position.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
if (toNextLength < 0.001f)
continue;
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 내적 계산 (유사도: -1 ~ 1)
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float score;
if (direction == AgvDirection.Forward)
{
// Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
score = dotProduct;
}
else // Backward
{
// Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
score = -dotProduct;
}
if (score > bestScore)
{
bestScore = score;
bestNode = candidate;
}
}
return bestNode;
}
}
}

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

@@ -41,6 +41,59 @@ namespace AGVNavigationCore.Utils
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {targetNodeId} 타입:{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;
var curNode = mapNodes?.FirstOrDefault(n => n.NodeId == curNodeId);
var nextNode = mapNodes?.FirstOrDefault(n => n.NodeId == nextNodeId);
if (curNode != null && nextNode != null)
{
MapNode prevNode = null;
if (i == 0) prevNode = pathResult.PrevNode;
else
{
var prevNodeId = pathResult.DetailedPath[i - 1].NodeId;
prevNode = mapNodes?.FirstOrDefault(n => n.NodeId == prevNodeId);
}
if (prevNode != null)
{
// DirectionalHelper를 사용하여 예상되는 다음 노드 확인
var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
curNode,
prevNode,
pathResult.DetailedPath[i].MotorDirection,
mapNodes
);
if (expectedNextNode != null && !expectedNextNode.NodeId.Equals(nextNode.NodeId))
{
string error =
$"[DockingValidator] ⚠️ 경로 방향 불일치: " +
$"현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " +
$"예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
return DockingValidationResult.CreateInvalid(
targetNodeId,
LastNode.Type,
pathResult.DetailedPath[i].MotorDirection,
pathResult.DetailedPath[i].MotorDirection,
error);
}
}
}
}
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
if (LastNode.DockDirection == DockingDirection.DontCare)
{