From 43e74588662d7a0f561e7a4d013e4e5491e0885f Mon Sep 17 00:00:00 2001 From: backuppc Date: Fri, 27 Feb 2026 16:15:30 +0900 Subject: [PATCH] .. --- .../PathFinding/Planning/AGVPathfinder.cs | 340 +++++++++++++++++- HMI/Project/StateMachine/Step/_Util.cs | 25 +- 2 files changed, 332 insertions(+), 33 deletions(-) diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 0623c61..f02c1f5 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -506,7 +506,150 @@ namespace AGVNavigationCore.PathFinding.Planning //일반노드도 일부 경로를 계산한다. // 일반노드 3 -> + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Loader, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "3B", "71B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Cleaner, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "3T", "3B", "70B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Plating, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "3B", "11B", "72B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Charger, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "3T", "3F", "11F", "7B", "10B", "6B", "73B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Buffer, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" } + }); + + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Loader, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "3T", "3B", "71B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Cleaner, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "3B", "70B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Plating, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "3T", "3B", "11B", "72B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Charger, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "3F", "11F", "7B", "10B", "6B", "73B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Buffer, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "3T", "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" } + }); + // 일반노드 7 -> + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Loader, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "7F", "11F", "3B", "71B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Cleaner, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "7F", "11F", "3T", "3B", "70B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Plating, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "7F", "11B", "72B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Charger, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "7F", "11F", "3T", "3F", "11F", "7B", "10B", "6B", "73B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Buffer, + Monitor = MapZoneMonitor.LeftTop, + Path = new List { "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" } + }); + + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Loader, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "7B", "11B", "3T", "3B", "71B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Cleaner, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "7B", "11B", "3B", "70B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Plating, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "7B", "11B", "3T", "3B", "11B", "72B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Charger, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "7B", "10B", "6B", "73B" } + }); + retval.Add(new MapZonePathData + { + NodeSta = StationType.Normal, + NodeEnd = StationType.Buffer, + Monitor = MapZoneMonitor.RightBtm, + Path = new List { "7B", "11B", "3T", "3B", "11B", "7B", "8B", "9B", "20B", "34B", "33B", "32B", "31B", "35B", "36B" } + }); return retval; } @@ -564,23 +707,71 @@ namespace AGVNavigationCore.PathFinding.Planning // 반전되어 구해지는 현상이 있으므로, 이 경우에만 monitorMode를 반대로 뒤집어서 찾는다. if (startZone != targetZone && startNode.StationType == StationType.Normal) { - monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop; + //monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop; } // 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다 var zonepath = GetMapZonePathData(); IEnumerable candidates; - // 목적지가 현재 위치와 동일한 경우 처리 //시작이 일반노드라면 , 경로가 등록된 주변 노드로 이동해서 이후경로를 계산하자 //일반노드용 기준은 7,3이있다. - //11번은 7로 이동해서 경로를 계산한다. + //11번은 3로 이동해서 경로를 계산한다. //10,6번은 7로 이동해서 경로를 계산한다. - //대상노드까지 CalculateScriptedPath를 재귀로 호출해서 계산을 완료한 후, 대상에서 목적지계산해서 전체 경로를 만들어낸다. + //대상노드까지 CalculateScriptedPath를 재귀로 호출해서 계산을 완료한 후, 대상에서 목적지 계산해서 전체 경로를 만들어낸다. + if (startNode.StationType == StationType.Normal && startNode.RfidId != 3 && startNode.RfidId != 7) + { + var newtargetRFID = startNode.RfidId == 11 ? 3 : 7; + var refNode = _mapNodes.FirstOrDefault(n => n.RfidId == newtargetRFID); + if (refNode != null && refNode.Id != startNode.Id) + { + // 기준 노드(7 or 3)까지의 기본 경로를 먼저 구함 + var pathToRef = this.FindBasicPath(startNode, refNode, prevNode, prevDir); + if (pathToRef != null && pathToRef.Success) + { + var lastDet = pathToRef.DetailedPath.Last(); + var secondLastNode = pathToRef.Path.Count > 1 ? pathToRef.Path[pathToRef.Path.Count - 2] : prevNode; + + // 기준 노드(7 or 3)에서 목적지까지의 경로를 재귀적으로 계산 (이 때 하드코딩된 zonepath가 활용됨) + var pathFromRef = CalculateScriptedPath(refNode, targetNode, secondLastNode, lastDet.MotorDirection); + if (pathFromRef != null && pathFromRef.Success) + { + // 중복 노드 제거를 위해 첫 번째 경로의 마지막 노드 정보를 제거하고 합침 + pathToRef.Path.RemoveAt(pathToRef.Path.Count - 1); + pathToRef.DetailedPath.RemoveAt(pathToRef.DetailedPath.Count - 1); + return CombinePaths(pathToRef, pathFromRef); + } + } + } + } + + //이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다 + //그런경우는 복잡하게 추가 계산할 필요가 없으니까 + var exactMatchList = zonepath.Where(d => + d.NodeSta == startNode.StationType && + d.NodeEnd == targetNode.StationType && + d.Monitor == monitorMode); + + var exactMatch = exactMatchList.FirstOrDefault(d=> + d.Path.First().StartsWith(startNode.RfidId.ToString()) && + d.Path.Last().StartsWith(targetNode.RfidId.ToString())); + if (exactMatch != null) + { + int startIndex = exactMatch.Path.FindIndex(p => p == startTag); + if (startIndex == -1) startIndex = exactMatch.Path.FindIndex(p => p.StartsWith(startNode.RfidId.ToString())); + int endIndex = exactMatch.Path.FindLastIndex(p => p.StartsWith(targetNode.RfidId.ToString())); + + if (startIndex != -1 && endIndex != -1 && startIndex <= endIndex) + { + var slicedPath = exactMatch.Path.Skip(startIndex).Take(endIndex - startIndex + 1).ToList(); + return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); + } + } + // 시작Zone과 목표Zone이 다른 경우에만 하드코딩된 zonepath 검색 if (startZone != targetZone) @@ -704,7 +895,7 @@ namespace AGVNavigationCore.PathFinding.Planning if (path2Candidate == null) return AGVPathResult.CreateFailure("턴 포인트에서 목표로 향하는 하드코딩 경로를 찾을 수 없습니다."); - int path2StartIdx = path2Candidate.Path.FindIndex(p => p == "3T"); + int path2StartIdx = path2Candidate.Path.FindIndex(p => p == "3T"); int path2EndIdx = path2Candidate.Path.FindLastIndex(p => p == $"{targetNode.RfidId}B"); var slicedPath2 = path2Candidate.Path.Skip(path2StartIdx).Take(path2EndIdx - path2StartIdx + 1).ToList(); @@ -747,7 +938,7 @@ namespace AGVNavigationCore.PathFinding.Planning if (endBufferNode == null) return AGVPathResult.CreateFailure("버퍼 끝 노드(7)를 찾을 수 없습니다."); var overPathFull = this.FindBasicPath(startNode, endBufferNode, prevNode, AgvDirection.Forward); - if (overPathFull == null || !overPathFull.Success) + if (overPathFull == null || !overPathFull.Success) return AGVPathResult.CreateFailure("Overshoot 전체 경로(7번 방향) 탐색 실패"); int targetIdx = overPathFull.Path.FindIndex(n => n.Id == targetNode.Id); @@ -755,11 +946,11 @@ namespace AGVNavigationCore.PathFinding.Planning return AGVPathResult.CreateFailure("Overshoot를 위한 여유 공간(다음 노드)이 없습니다."); // 목표 노드 다음 노드(오버슈트 지점)까지만 잘라내어 새 경로 구성 - var overPath = new AGVPathResult - { - Success = true, - Path = overPathFull.Path.Take(targetIdx + 2).ToList(), - DetailedPath = overPathFull.DetailedPath.Take(targetIdx + 2).ToList() + var overPath = new AGVPathResult + { + Success = true, + Path = overPathFull.Path.Take(targetIdx + 2).ToList(), + DetailedPath = overPathFull.DetailedPath.Take(targetIdx + 2).ToList() }; var autoOverNode = overPath.Path.Last(); // 오버슈트 된 곳 @@ -835,10 +1026,130 @@ namespace AGVNavigationCore.PathFinding.Planning return AGVPathResult.CreateFailure("경로를 계산할 수 없습니다"); } + private MapZoneMonitor GetMonitorMode(MapNode startNode, MapNode prevNode, AgvDirection prevDir) { if (prevNode == null) return MapZoneMonitor.RightBtm; + //모니터방향도 상황에 따라 다른경우가 있다. 이것도 하드코딩하다. + //prev -> start 와 모터방향(prevdir) 에 따라서 경우의 수를 입력한다. + //일반노드가아닌 노드와, 일반노드중 7,11을 포함해서 모든 경우를 정의해야한다. + + //3 - junction(turn) + if (startNode.RfidId == 3) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 70) + return MapZoneMonitor.RightBtm; + else if (prevNode.RfidId == 11 || prevNode.RfidId == 71) + return MapZoneMonitor.LeftTop; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 70) + return MapZoneMonitor.LeftTop; + else if (prevNode.RfidId == 11 || prevNode.RfidId == 71) + return MapZoneMonitor.RightBtm; + } + } + + //7 - junction + if (startNode.RfidId == 7) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 10 || prevNode.RfidId == 11) + return MapZoneMonitor.RightBtm; + else if (prevNode.RfidId == 8) + return MapZoneMonitor.LeftTop; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 10 || prevNode.RfidId == 11) + return MapZoneMonitor.LeftTop; + else if (prevNode.RfidId == 8) + return MapZoneMonitor.RightBtm; + } + } + + //70 - cleaner + if (startNode.RfidId == 70) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 3) + return MapZoneMonitor.LeftTop; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 3) + return MapZoneMonitor.RightBtm; + } + } + + //71 - loader + if (startNode.RfidId == 71) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 3) + return MapZoneMonitor.RightBtm; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 3) + return MapZoneMonitor.LeftTop; + } + } + + + //73 - charger + if (startNode.RfidId == 73) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 6) + return MapZoneMonitor.LeftTop; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 6) + return MapZoneMonitor.RightBtm; + } + } + + //72 -- plating + if (startNode.RfidId == 72) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 11) + return MapZoneMonitor.LeftTop; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 11) + return MapZoneMonitor.RightBtm; + } + } + + //31~34,35,36 --buffer + if (startNode.RfidId >= 31 && startNode.RfidId <= 36) + { + if (prevDir == AgvDirection.Forward) + { + if (prevNode.RfidId == 20) + return MapZoneMonitor.LeftTop; + } + else if (prevDir == AgvDirection.Backward) + { + if (prevNode.RfidId == 20) + return MapZoneMonitor.RightBtm; + } + } + + int dx = startNode.Position.X - prevNode.Position.X; int dy = startNode.Position.Y - prevNode.Position.Y; @@ -853,8 +1164,15 @@ namespace AGVNavigationCore.PathFinding.Planning } return isMonitorLeft ? MapZoneMonitor.LeftTop : MapZoneMonitor.RightBtm; + + + + + } + + private AGVPathResult ConvertHardcodedPathToResult(List pathStrings, MapNode startNode, MapNode prevNode, AgvDirection prevDir) { var result = new AGVPathResult { Success = true }; diff --git a/HMI/Project/StateMachine/Step/_Util.cs b/HMI/Project/StateMachine/Step/_Util.cs index 250f032..04c9bde 100644 --- a/HMI/Project/StateMachine/Step/_Util.cs +++ b/HMI/Project/StateMachine/Step/_Util.cs @@ -194,28 +194,6 @@ namespace Project } else VAR.I32[eVarInt32.PathValidationError] = 0; - //현재위치 기준으로 재 계산하여. 더 최적화된 루트가 있다면 처리를 해준다. - /* - //TODO: 이제 경로가 하드코딩되어있으니 자동 갱신은 사용하지 않는다. - if (PUB._virtualAGV.CurrentPath.DetailedPath.Count > 5) - { - var PathResult2 = CalcPath(PUB._virtualAGV.CurrentNode, PUB._virtualAGV.TargetNode); - if (PathResult2 != null && PathResult2.Success) - { - //절반이상 경로가 짧을때에는 재계산을 하게한다 - var halfcnt = (int)(PUB._virtualAGV.CurrentPath.DetailedPath.Count / 2.0); - if (PathResult2.DetailedPath.Count > 2 && PathResult2.DetailedPath.Count < halfcnt) - { - var msg = $"단축경로가 확인되었습니다. 경로를 삭제 합니다"; - PUB.log.AddE(msg); - Console.WriteLine(msg); - PUB._virtualAGV.SetPath(null); - return false; - } - - } - } - */ //predict 를 이용하여 다음 이동을 모두 확인한다. var nextAction = PUB._virtualAGV.Predict(); @@ -331,6 +309,9 @@ namespace Project else { //PREDICT에서 이동을 명령했따 + + + var bunki = arDev.Narumi.eBunki.Strate; if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left; else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;