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

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에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다