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-02-12 09:58:01 +09:00
parent 2b3a9b3d1d
commit d6aed58516
17 changed files with 914 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -240,17 +240,17 @@ namespace AGVNavigationCore.PathFinding.Planning
// 2. Buffer-to-Buffer 예외 처리
// 05~31 구간 체크
var node05 = _mapNodes.FirstOrDefault(n => n.RfidId == 5);
var node31 = _mapNodes.FirstOrDefault(n => n.RfidId == 31);
var node_buff_sta = _mapNodes.Where(t => t.StationType == StationType.Buffer).OrderBy(s => s.RfidId).FirstOrDefault();// (n => n.RfidId == 5);
var node_buff_end = _mapNodes.Where(t => t.StationType == StationType.Buffer).OrderByDescending(s => s.RfidId).FirstOrDefault();//
bool fixpath = false;
Retval = null;
MapNode gatewayNode = null;
if (node05 != null && node31 != null)
if (node_buff_sta != null && node_buff_end != null)
{
// 버퍼 구간 경로 테스트
var rlt = this.FindPathAStar(node05, node31);
var rlt = this.FindPathAStar(node_buff_sta, node_buff_end);
if (rlt.Success)
{
// 버퍼구간내에 시작과 종료가 모두 포함되어있다
@@ -265,53 +265,89 @@ namespace AGVNavigationCore.PathFinding.Planning
if (!fixpath)
{
// 3. 목적지별 Gateway 및 진입 조건 확인
gatewayNode = GetGatewayNode(targetNode);
if (gatewayNode == null)
if (targetNode.StationType == StationType.Limit || targetNode.StationType == StationType.Normal)
{
// 게이트웨이가 없는 경우라면(일반 노드 등), Gateway 로직 없이 기본 경로 탐색
//일반노드라면 방향 무관하게 그냥 이동하게한다.
Retval = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
}
else
{
// Gateway Node 찾음
// 4. Start -> Gateway 경로 계산 (A*)
var pathToGateway = this.FindBasicPath(startNode, gatewayNode, prevNode, prevDir);
if (pathToGateway.Success == false)
return AGVPathResult.CreateFailure($"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.Message}");
//목적지까지 그대로 방향을 계산한다.
var pathToTaget = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
if (pathToTaget.Success == false)
return AGVPathResult.CreateFailure($"Target({targetNode.ID2})까지 경로 실패: {pathToTaget.Message}");
// 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
if (pathToGateway.Path.Count > 1)
//다음이동방향이 이전노드와 동일하다면? 되돌아가야한다는것이다
var predictNext = pathToTaget.Path[1];
if (predictNext.Id == prevNode.Id)
{
var predictNext = pathToGateway.Path[1];
if (predictNext.Id == prevNode.Id)
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
foreach (var item in pathToTaget.DetailedPath)
item.MotorDirection = reverseDir;
}
if (targetNode.DockDirection != DockingDirection.DontCare)
{
Retval = pathToTaget;
}
//현재 진행방향과 목적지의 도킹방향이 일치하는지 확인한다.
else if ((pathToTaget.DetailedPath.Last().MotorDirection == AgvDirection.Backward && targetNode.DockDirection == DockingDirection.Backward) ||
(pathToTaget.DetailedPath.Last().MotorDirection == AgvDirection.Forward && targetNode.DockDirection == DockingDirection.Forward))
{
//일치하는 경우 그대로 사용하낟.
Retval = pathToTaget;
}
else
{
//불일치하는경우라면 Turn 가능노드를 찾아서 이동한 후 턴을 한다.
// 3. 목적지별 Turn 및 진입 조건 확인
gatewayNode = GetTurnNode(targetNode);
// Gateway Node 찾음
// 4. Start -> Gateway 경로 계산 (A*)
var pathToGateway = this.FindBasicPath(startNode, gatewayNode, prevNode, prevDir);
if (pathToGateway.Success == false)
return AGVPathResult.CreateFailure($"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.Message}");
// 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
if (pathToGateway.Path.Count > 1)
{
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
foreach (var item in pathToGateway.DetailedPath)
item.MotorDirection = reverseDir;
//다음이동방향이 이전노드와 동일하다면? 되돌아가야한다는것이다
predictNext = pathToGateway.Path[1];
if (predictNext.Id == prevNode.Id)
{
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
foreach (var item in pathToGateway.DetailedPath)
item.MotorDirection = reverseDir;
}
}
// 마지막 경로는 게이트웨이이므로 제거 (Gateway 진입 후 처리는 GetPathFromGateway에서 담당)
if (pathToGateway.Path.Count > 0 && pathToGateway.Path.Last().Id == gatewayNode.Id)
{
var idx = pathToGateway.Path.Count - 1;
pathToGateway.Path.RemoveAt(idx);
pathToGateway.DetailedPath.RemoveAt(idx);
}
// 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
MapNode GateprevNode = pathToGateway.Path.LastOrDefault() ?? prevNode;
NodeMotorInfo GatePrevDetail = pathToGateway.DetailedPath.LastOrDefault();
var arrivalOrientation = GatePrevDetail?.MotorDirection ?? prevDir;
var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, GateprevNode, arrivalOrientation);
if (!gatewayPathResult.Success)
return AGVPathResult.CreateFailure($"{gatewayPathResult.Message}");
Retval = CombinePaths(pathToGateway, gatewayPathResult);
}
// 마지막 경로는 게이트웨이이므로 제거 (Gateway 진입 후 처리는 GetPathFromGateway에서 담당)
if (pathToGateway.Path.Count > 0 && pathToGateway.Path.Last().Id == gatewayNode.Id)
{
var idx = pathToGateway.Path.Count - 1;
pathToGateway.Path.RemoveAt(idx);
pathToGateway.DetailedPath.RemoveAt(idx);
}
// 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
MapNode GateprevNode = pathToGateway.Path.LastOrDefault() ?? prevNode;
NodeMotorInfo GatePrevDetail = pathToGateway.DetailedPath.LastOrDefault();
var arrivalOrientation = GatePrevDetail?.MotorDirection ?? prevDir;
var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, GateprevNode, arrivalOrientation);
if (!gatewayPathResult.Success)
return AGVPathResult.CreateFailure($"{gatewayPathResult.Message}");
Retval = CombinePaths(pathToGateway, gatewayPathResult);
}
}
@@ -457,13 +493,13 @@ namespace AGVNavigationCore.PathFinding.Planning
var firstnode = Retval.Path.FirstOrDefault();
var firstDet = Retval.DetailedPath.First();
var failmessage = $"[{firstnode.ID2}] 노드의 시작모터 방향({firstDet.MotorDirection})이 올바르지 않습니다";
if (firstnode.StationType == StationType.Charger1 && firstDet.MotorDirection != AgvDirection.Forward)
if (firstnode.StationType == StationType.Charger && firstDet.MotorDirection != AgvDirection.Forward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Loader && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.UnLoader && firstDet.MotorDirection != AgvDirection.Backward)
else if (firstnode.StationType == StationType.Cleaner && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Clearner && firstDet.MotorDirection != AgvDirection.Backward)
else if (firstnode.StationType == StationType.Plating && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Buffer)
{
@@ -621,7 +657,7 @@ namespace AGVNavigationCore.PathFinding.Planning
if (deltaX > 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
if (targetNode.StationType == StationType.Loader || targetNode.StationType == StationType.Charger2)
if (targetNode.StationType == StationType.Loader)
{
deltaX = GTNode.Position.Y - PrevNode.Position.Y;
if (deltaX < 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
@@ -631,14 +667,14 @@ namespace AGVNavigationCore.PathFinding.Planning
switch (targetNode.StationType)
{
case StationType.Loader:
case StationType.Charger2:
case StationType.Charger1:
case StationType.UnLoader:
case StationType.Clearner:
case StationType.Charger:
case StationType.Cleaner:
case StationType.Plating:
case StationType.Buffer:
var rlt1 = new AGVPathResult();
rlt1.Success = true;
//단순 경로를 찾는다
var motdir = targetNode.DockDirection == DockingDirection.Backward ? AgvDirection.Backward : AgvDirection.Forward;
var pathtarget = this.FindBasicPath(GTNode, targetNode, PrevNode, motdir);
@@ -682,15 +718,14 @@ namespace AGVNavigationCore.PathFinding.Planning
}
}
private MapNode GetGatewayNode(MapNode node)
private MapNode GetTurnNode(MapNode node)
{
var rfid = 0;
if (node.StationType == StationType.UnLoader) rfid = 10;
else if (node.StationType == StationType.Charger1) rfid = 9;
else if (node.StationType == StationType.Clearner) rfid = 6;
else if (node.StationType == StationType.Charger2) rfid = 13;
else if (node.StationType == StationType.Loader) rfid = 13;
else if (node.StationType == StationType.Buffer) rfid = 6;
if (node.StationType == StationType.Cleaner) rfid = 3;
else if (node.StationType == StationType.Charger) rfid = 3;
else if (node.StationType == StationType.Plating) rfid = 3;
else if (node.StationType == StationType.Loader) rfid = 3;
else if (node.StationType == StationType.Buffer) rfid = 3;
if (rfid == 0) return null;
return _mapNodes.FirstOrDefault(t => t.RfidId == rfid);