..
This commit is contained in:
@@ -810,11 +810,11 @@ namespace AGVNavigationCore.Controls
|
||||
/// <summary>
|
||||
/// 동기화 모드 종료
|
||||
/// </summary>
|
||||
public void ExitSyncMode()
|
||||
public void ExitSyncMode(CanvasMode newmode)
|
||||
{
|
||||
if (_canvasMode == CanvasMode.Sync)
|
||||
{
|
||||
_canvasMode = CanvasMode.Edit; // 기본 모드로 복귀 (또는 이전 모드)
|
||||
_canvasMode = newmode; // 기본 모드로 복귀 (또는 이전 모드)
|
||||
UpdateModeUI();
|
||||
Invalidate();
|
||||
}
|
||||
|
||||
@@ -102,6 +102,8 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
/// </summary>
|
||||
public AgvDirection PrevDirection { get; set; }
|
||||
|
||||
public MapNode Gateway { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 기본 생성자
|
||||
/// </summary>
|
||||
|
||||
@@ -169,8 +169,8 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
else if (magdir == "R") magnetDirection = MagnetDirection.Right;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode, magnetDirection);
|
||||
@@ -194,5 +194,556 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
|
||||
|
||||
|
||||
/// <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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user