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

파일정리

Browse Source
This commit is contained in:
ChiKyun Kim
2026-01-29 14:03:17 +09:00
parent 00cc0ef5b7
commit 58ca67150d
440 changed files with 47236 additions and 99165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

749
AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs Normal file
View File

@@ -0,0 +1,749 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 경로 계획기
/// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
/// </summary>
public class AGVPathfinder
{
private readonly List<MapNode> _mapNodes;
private readonly AStarPathfinder _basicPathfinder;
private readonly DirectionalPathfinder _directionPathfinder;
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly DirectionChangePlanner _directionChangePlanner;
public AGVPathfinder(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_basicPathfinder = new AStarPathfinder();
_basicPathfinder.SetMapNodes(_mapNodes);
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
_directionChangePlanner = new DirectionChangePlanner(_mapNodes);
_directionPathfinder = new DirectionalPathfinder();
}
/// <summary>
/// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
/// </summary>
/// <param name="startNode">기준이 되는 노드</param>
/// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
public MapNode FindNearestJunction(MapNode startNode)
{
if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
return null;
// 교차로: 3개 이상의 노드가 연결된 노드
var junctions = _mapNodes.Where(n =>
n.IsActive &&
n.IsNavigationNode() &&
n.ConnectedNodes != null &&
n.DisableCross == false &&
n.ConnectedNodes.Count >= 3 &&
n.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false &&
n.Id != startNode.Id
).ToList();
// docking 포인트가 연결된 노드는 제거한다.
if (junctions.Count == 0)
return null;
// 직선 거리 기반으로 가장 가까운 교차로 찾기
MapNode nearestJunction = null;
float minDistance = float.MaxValue;
foreach (var junction in junctions)
{
float dx = junction.Position.X - startNode.Position.X;
float dy = junction.Position.Y - startNode.Position.Y;
float distance = (float)Math.Sqrt(dx * dx + dy * dy);
if (distance < minDistance)
{
minDistance = distance;
nearestJunction = junction;
}
}
return nearestJunction;
}
/// <summary>
/// 지정한 노드에서 경로상 가장 가까운 교차로를 찾는다.
/// (최단 경로 내에서 3개 이상 연결된 교차로를 찾음)
/// </summary>
/// <param name="startNode">시작 노드</param>
/// <param name="targetNode">목적지 노드</param>
/// <returns>경로상의 가장 가까운 교차로 노드 (또는 null)</returns>
public MapNode FindNearestJunctionOnPath(AGVPathResult pathResult)
{
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
return null;
// 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
var StartNode = pathResult.Path.First();
foreach (var pathNode in pathResult.Path)
{
if (pathNode != null &&
pathNode.IsActive &&
pathNode.IsNavigationNode() &&
pathNode.DisableCross == false &&
pathNode.ConnectedNodes != null &&
pathNode.ConnectedNodes.Count >= 3 &&
pathNode.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false)
{
if (pathNode.Id.Equals(StartNode.Id) == false)
return pathNode;
}
}
return null;
}
public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode)
{
// 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
return _basicPathfinder.FindPathAStar(startNode, targetNode);
}
/// <summary>
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
/// </summary>
/// <summary>
/// 단순 경로 찾기 (복잡한 제약조건/방향전환 로직 없이 A* 결과만 반환)
/// </summary>
public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode _prevNode, AgvDirection prevDirection)
{
// 1. 입력 검증
if (startNode == null || targetNode == null)
return AGVPathResult.CreateFailure("노드 정보 오류", 0, 0);
// 2. A* 경로 탐색
var pathResult = _basicPathfinder.FindPathAStar(startNode, targetNode);
pathResult.PrevNode = _prevNode;
pathResult.PrevDirection = prevDirection;
if (!pathResult.Success)
return AGVPathResult.CreateFailure(pathResult.Message ?? "경로 없음", 0, 0);
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
if (pathResult.Path != null && pathResult.Path.Count > 0)
{
var detailedPath = new List<NodeMotorInfo>();
for (int i = 0; i < pathResult.Path.Count; i++)
{
var node = pathResult.Path[i];
var nextNode = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1] : null;
// 마그넷 방향 계산 (갈림길인 경우)
// 마그넷 방향 계산 (갈림길인 경우)
MagnetDirection magnetDirection = MagnetDirection.Straight;
//갈림길에 있다면 미리 방향을 저장해준다.
if ((node.ConnectedNodes?.Count ?? 0) > 2 && nextNode != null)
{
//다음 노드ID를 확인해서 마그넷 방향 데이터를 찾는다.
if (node.MagnetDirections.ContainsKey(nextNode.Id) == false)
{
return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
}
else
{
var magdir = node.MagnetDirections[nextNode.Id].ToString();
if (magdir == "L") magnetDirection = MagnetDirection.Left;
else if (magdir == "R") magnetDirection = MagnetDirection.Right;
}
}
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode, magnetDirection);
// 속도 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);
if (mapNode != null)
{
nodeInfo.Speed = mapNode.SpeedLimit;
detailedPath.Add(nodeInfo);
}
}
pathResult.DetailedPath = detailedPath;
}
return pathResult;
}
/// <summary>
/// 길목(Gateway) 기반 고급 경로 계산 (기존 SimulatorForm.CalcPath 이관)
/// </summary>
public AGVPathResult CalculatePath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
{
AGVPathResult Retval;
// var o_StartNode = startNode;
// startNode, targetNode는 이미 인자로 받음
if (startNode == null || targetNode == null) return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음");
try
{
// 종료노드라면 이전위치로 이동시켜야한다. (Simulator Logic)
// 만약 시작노드가 끝단(ConnectedMapNodes.Count == 1)이라면,
// AGV가 해당 노드에 '도착'한 상태가 아니라 '작업' 중일 수 있으므로
// 이전 노드(진입점)로 위치를 보정하여 경로를 계산한다.
AGVPathResult LimitPath = null;
if (startNode.ConnectedMapNodes.Count == 1)
{
// 시작점 -> 이전점 경로 (보통 후진이나 전진 1칸)
LimitPath = this.FindPathAStar(startNode, prevNode);
if (LimitPath.Success)
{
for (int i = 0; i < LimitPath.Path.Count; i++)
{
var nodeinfo = LimitPath.Path[i];
var dir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
LimitPath.DetailedPath.Add(new NodeMotorInfo(i + 1, nodeinfo.Id, nodeinfo.RfidId, dir));
}
// 시작 위치 및 방향 변경
// var org_start = startNode; // Unused
startNode = prevNode;
prevNode = LimitPath.Path.First(); // startNode (original)
prevDir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
}
else
{
// 경로 생성 실패 시 보정 없이 진행하거나 에러 처리
// 여기서는 일단 기존 로직대로 진행
}
}
// 2. Buffer-to-Buffer 예외 처리
// 05~31 구간 체크
var node05 = _mapNodes.FirstOrDefault(n => n.RfidId == 5);
var node31 = _mapNodes.FirstOrDefault(n => n.RfidId == 31);
bool fixpath = false;
Retval = null;
MapNode gatewayNode = null;
if (node05 != null && node31 != null)
{
// 버퍼 구간 경로 테스트
var rlt = this.FindPathAStar(node05, node31);
if (rlt.Success)
{
// 버퍼구간내에 시작과 종료가 모두 포함되어있다
if (rlt.Path.Find(n => n.Id == startNode.Id) != null &&
rlt.Path.Find(n => n.Id == targetNode.Id) != null)
{
Retval = CalcPathBufferToBuffer(startNode, targetNode, prevNode, prevDir);
fixpath = true;
}
}
}
if (!fixpath)
{
// 3. 목적지별 Gateway 및 진입 조건 확인
gatewayNode = GetGatewayNode(targetNode);
if (gatewayNode == null)
{
// 게이트웨이가 없는 경우라면(일반 노드 등), 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}");
// 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
if (pathToGateway.Path.Count > 1)
{
var 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);
}
}
//게이트웨이
Retval.Gateway = gatewayNode;
// 경로 오류 검사
if (Retval == null || Retval.Success == false) return Retval ?? AGVPathResult.CreateFailure("경로 계산 결과 없음");
if (LimitPath != null)
{
Retval = CombinePaths(LimitPath, Retval);
}
// 해당 경로와 대상의 도킹포인트 방향 검사
if (targetNode.DockDirection != DockingDirection.DontCare)
{
var lastPath = Retval.DetailedPath.LastOrDefault();
if (lastPath != null)
{
if (targetNode.DockDirection == DockingDirection.Forward && lastPath.MotorDirection != AgvDirection.Forward)
{
return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(FWD) Target:{targetNode.DockDirection}");
}
if (targetNode.DockDirection == DockingDirection.Backward && lastPath.MotorDirection != AgvDirection.Backward)
{
return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(BWD) Target:{targetNode.DockDirection}");
}
}
}
// 경로 최적화: A -> B -> A 패턴 제거
// 6[F][R] → 13[B][L] → 6[F][L] 같은 경우 제거
while (fixpath == false)
{
var updatecount = 0;
for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
{
var n1 = Retval.DetailedPath[i];
var n2 = Retval.DetailedPath[i + 1];
var n3 = Retval.DetailedPath[i + 2];
if (n1.NodeId == n3.NodeId)
{
bool isInverse = false;
// 1. 모터 방향이 반대인가? (F <-> B)
bool isMotorInverse = (n1.MotorDirection != n2.MotorDirection) &&
(n1.MotorDirection == AgvDirection.Forward || n1.MotorDirection == AgvDirection.Backward) &&
(n2.MotorDirection == AgvDirection.Forward || n2.MotorDirection == AgvDirection.Backward);
if (isMotorInverse)
{
// 2. 마그넷 방향이 반대인가? (L <-> R, S <-> S)
bool isMagnetInverse = false;
if (n1.MagnetDirection == MagnetDirection.Straight && n2.MagnetDirection == MagnetDirection.Straight) isMagnetInverse = true;
else if (n1.MagnetDirection == MagnetDirection.Left && n2.MagnetDirection == MagnetDirection.Right) isMagnetInverse = true;
else if (n1.MagnetDirection == MagnetDirection.Right && n2.MagnetDirection == MagnetDirection.Left) isMagnetInverse = true;
if (isMagnetInverse) isInverse = true;
}
if (isInverse)
{
// 제자리 회귀 경로 발견 -> 앞의 두 노드(n1, n2)를 제거하여 n3만 남김
Retval.DetailedPath.RemoveAt(i);
Retval.DetailedPath.RemoveAt(i);
if (Retval.Path.Count > i + 1)
{
Retval.Path.RemoveAt(i);
Retval.Path.RemoveAt(i);
}
i--; // 인덱스 재조정
updatecount += 1;
}
}
}
if (updatecount == 0) break;
}
// 불가능한 회전 경로 검사 (사용자 요청 로직 반영)
for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
{
var n1 = Retval.DetailedPath[i];
var n2 = Retval.DetailedPath[i + 1];
var n3 = Retval.DetailedPath[i + 2];
if (n1.NodeId == n3.NodeId &&
n1.MotorDirection == n3.MotorDirection &&
n1.MotorDirection == n2.MotorDirection) // Fix: 중간 노드 방향도 같을 때만 에러
{
return AGVPathResult.CreateFailure($"불가능한 회전 경로가 포함되어있습니다. {n1.RfidId}->{n2.RfidId}->{n3.RfidId}");
}
}
// 기타 검증 로직 (마지막 노드 도킹, 시작노드 일치 등)
var lastnode = Retval.Path.Last();
if (lastnode.StationType != StationType.Normal)
{
var lastnodePath = Retval.DetailedPath.Last();
if (lastnode.DockDirection == DockingDirection.Forward && lastnodePath.MotorDirection != AgvDirection.Forward)
return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
if (lastnode.DockDirection == DockingDirection.Backward && lastnodePath.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
}
// 첫번째 노드 일치 검사 - 필요시 수행 (startNode가 변경될 수 있어서 o_StartNode 등 필요할 수도 있음)
// 여기서는 생략 혹은 간단히 체크
// 되돌아가는 길 방향 일치 검사
if (Retval.DetailedPath.Count > 1)
{
var FirstDetailPath = Retval.DetailedPath[0];
var NextDetailPath = Retval.DetailedPath[1];
AgvDirection? PredictNextDir = null;
if (NextDetailPath.NodeId == prevNode.Id)
{
if (NextDetailPath.MagnetDirection == MagnetDirection.Straight)
PredictNextDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
}
if (PredictNextDir != null && (FirstDetailPath.MotorDirection != (AgvDirection)PredictNextDir))
{
// return AGVPathResult.CreateFailure($"되돌아가는 길인데 방향이 일치하지않음");
// 경고 수준이나 무시 가능한 경우도 있음
}
}
// 연결성 검사
for (int i = 0; i < Retval.DetailedPath.Count - 1; i++)
{
var cnode = Retval.Path[i];
var nnode = Retval.Path[i + 1];
if (cnode.ConnectedNodes.Contains(nnode.Id) == false && cnode.Id != nnode.Id)
{
return AGVPathResult.CreateFailure($"[{cnode.RfidId}] 노드에 연결되지 않은 [{nnode.RfidId}]노드가 지정됨");
}
}
//각 도킹포인트별로 절대 움직이면 안되는 조건확인
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)
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)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Clearner && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Buffer)
{
//버퍼는 도킹이되어잇느닞 확인하고. 그때 방향을 체크해야한다.
}
return Retval;
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"[계산오류] {ex.Message}");
}
}
private AGVPathResult CalcPathBufferToBuffer(MapNode start, MapNode target, MapNode prev, AgvDirection prevDir)
{
// Monitor Side 판단 및 Buffer 간 이동 로직
int deltaX = 0;
int deltaY = 0;
if (prev == null) return AGVPathResult.CreateFailure("이전 노드 정보가 없습니다");
else
{
deltaX = start.Position.X - prev.Position.X;
deltaY = -(start.Position.Y - prev.Position.Y);
}
if (Math.Abs(deltaY) > Math.Abs(deltaX))
deltaX = deltaY;
bool isMonitorLeft = false;
if (deltaX > 0) // 오른쪽(Forward)으로 이동해 옴
isMonitorLeft = (prevDir == AgvDirection.Backward);
else if (deltaX < 0) // 왼쪽(Reverse)으로 이동해 옴
isMonitorLeft = (prevDir == AgvDirection.Forward);
else
return AGVPathResult.CreateFailure("이전 노드와의 방향을 알 수 없습니다");
if (isMonitorLeft)
{
// Monitor Left -> Gateway 탈출
var GateWayNode = _mapNodes.FirstOrDefault(n => n.RfidId == 6);
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
AGVPathResult escPath = null;
if (start.Position.X > prev.Position.X)
escPath = this.FindBasicPath(start, GateWayNode, prev, prevDir);
else
escPath = this.FindBasicPath(start, GateWayNode, prev, reverseDir);
if (!escPath.Success) return AGVPathResult.CreateFailure("버퍼 탈출 경로 실패");
var lastNode = escPath.Path.Last();
var lastPrev = escPath.Path[escPath.Path.Count - 2];
var lastDir = escPath.DetailedPath.Last().MotorDirection;
var gateToTarget = GetPathFromGateway(GateWayNode, target, lastPrev, lastDir);
escPath.Path.RemoveAt(escPath.Path.Count - 1);
escPath.DetailedPath.RemoveAt(escPath.DetailedPath.Count - 1);
return CombinePaths(escPath, gateToTarget);
}
else
{
// Monitor Right -> 직접 진입 또는 Overshoot
bool isTargetLeft = target.Position.X < start.Position.X;
if (target == start)
{
// 제자리 재정렬 (Same as Simulator logic)
var list = new List<MapNode>();
var retval = AGVPathResult.CreateSuccess(list, new List<AgvDirection>(), 0, 0);
var resversedir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
retval.Path.Add(target);
if (deltaX < 0)
{
var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
if (nextNode != null)
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
retval.Path.Add(nextNode);
var lastDefailt = retval.DetailedPath.Last();
retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Forward)
{
Speed = SpeedLevel.M,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo((retval.DetailedPath.Max(t => t.seq) + 1), target.Id, target.RfidId, AgvDirection.Forward));
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
}
else
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, resversedir));
retval.Path.Add(prev);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Last().seq + 1, prev.Id, prev.RfidId, prevDir)
{
Speed = SpeedLevel.M,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, prevDir));
}
}
else
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
retval.Path.Add(nextNode);
var lastDefailt = retval.DetailedPath.Last();
retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Backward)
{
Speed = SpeedLevel.L,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
}
return retval;
}
else if (isTargetLeft)
{
return this.FindBasicPath(start, target, prev, AgvDirection.Backward);
}
else
{
// Overshoot
var path1 = this.FindBasicPath(start, target, prev, AgvDirection.Forward);
if (path1.Path.Count < 2) return AGVPathResult.CreateFailure("Overshoot 경로 생성 실패");
var last = path1.Path.Last();
var lastD = path1.DetailedPath.Last();
path1.Path.RemoveAt(path1.Path.Count - 1);
path1.DetailedPath.RemoveAt(path1.DetailedPath.Count - 1);
path1.Path.Add(last);
path1.DetailedPath.Add(new NodeMotorInfo(lastD.seq + 1, lastD.NodeId, lastD.RfidId, AgvDirection.Backward)
{
Speed = SpeedLevel.L,
});
return path1;
}
}
}
private AGVPathResult GetPathFromGateway(MapNode GTNode, MapNode targetNode, MapNode PrevNode, AgvDirection PrevDirection)
{
AGVPathResult resultPath = null;
var deltaX = GTNode.Position.X - PrevNode.Position.X;
var isMonitorLeft = false;
if (deltaX > 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
if (targetNode.StationType == StationType.Loader || targetNode.StationType == StationType.Charger2)
{
deltaX = GTNode.Position.Y - PrevNode.Position.Y;
if (deltaX < 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
}
switch (targetNode.StationType)
{
case StationType.Loader:
case StationType.Charger2:
case StationType.Charger1:
case StationType.UnLoader:
case StationType.Clearner:
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);
if ((targetNode.DockDirection == DockingDirection.Backward && isMonitorLeft) ||
(targetNode.DockDirection == DockingDirection.Forward && !isMonitorLeft))
{
var turnPatterns = GetTurnaroundPattern(GTNode, targetNode);
if (turnPatterns == null || !turnPatterns.Any()) return new AGVPathResult { Success = false, Message = $"회차 패턴 없음: Dir {PrevDirection}" };
foreach (var item in turnPatterns)
{
var rfidvalue = ushort.Parse(item.Substring(0, 4));
var node = _mapNodes.FirstOrDefault(t => t.RfidId == rfidvalue);
rlt1.Path.Add(node);
AgvDirection nodedir = item.Substring(4, 1) == "F" ? AgvDirection.Forward : AgvDirection.Backward;
MagnetDirection magnet = MagnetDirection.Straight;
var magchar = item.Substring(5, 1);
if (magchar == "L") magnet = MagnetDirection.Left;
else if (magchar == "R") magnet = MagnetDirection.Right;
rlt1.DetailedPath.Add(new NodeMotorInfo(rlt1.DetailedPath.Count, node.Id, node.RfidId, nodedir, null, magnet)
{
Speed = SpeedLevel.L,
});
}
if (pathtarget.DetailedPath.First().NodeId != rlt1.DetailedPath.Last().NodeId ||
pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection)
{
// Gateway 턴 마지막 주소 불일치 경고 (로깅 등)
}
pathtarget.Path.RemoveAt(0);
pathtarget.DetailedPath.RemoveAt(0);
}
return CombinePaths(rlt1, pathtarget);
default:
return AGVPathResult.CreateFailure($"지원되지 않는 StationType: {targetNode.StationType}");
}
}
private MapNode GetGatewayNode(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 (rfid == 0) return null;
return _mapNodes.FirstOrDefault(t => t.RfidId == rfid);
}
private List<string> GetTurnaroundPattern(MapNode gatewayNode, MapNode targetNode)
{
switch (gatewayNode.RfidId)
{
case 6:
if (targetNode.StationType == StationType.Buffer)
return new List<string> { "0006BL", "0007FS", "0013BL", "0006BL" };
else
return new List<string> { "0006BL", "0007FS", "0013BL", "0006BS" };
case 9: return new List<string> { "0009FL", "0010BS", "0007FL", "0009FS" };
case 10: return new List<string> { "0010BR", "0009FR", "0007BS", "0010BS" };
case 13: return new List<string> { "0013BL", "0006FL", "0007BS", "0013BS" };
default: return null;
}
}
private AGVPathResult CombinePaths(AGVPathResult p1, AGVPathResult p2)
{
var res = new AGVPathResult();
res.Success = true;
var p1last = p1.DetailedPath.LastOrDefault();
var p2fist = p2.DetailedPath.FirstOrDefault();
if (p1last != null && p2fist != null &&
(p1last.NodeId == p2fist.NodeId && p1last.MotorDirection == p2fist.MotorDirection && p1last.MagnetDirection == p2fist.MagnetDirection))
{
p1.Path.RemoveAt(p1.Path.Count - 1);
p1.DetailedPath.RemoveAt(p1.DetailedPath.Count - 1);
}
foreach (var item in p1.Path) res.Path.Add(item);
foreach (var item in p2.Path) res.Path.Add(item);
foreach (var item in p1.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
foreach (var item in p2.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
return res;
}
}
}