From 2590ad91de24282859fd1fc9c7c61eed8c7f8208 Mon Sep 17 00:00:00 2001 From: backuppc Date: Wed, 25 Feb 2026 14:55:36 +0900 Subject: [PATCH] .. --- .../PathFinding/Planning/AGVPathfinder.cs | 296 +++++++++++++++--- AGVLogic/AGVSimulator/Forms/SimulatorForm.cs | 11 +- 2 files changed, 259 insertions(+), 48 deletions(-) diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 54269dc..50742a6 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -552,70 +552,274 @@ namespace AGVNavigationCore.PathFinding.Planning // 시작 태그 검색용 (예: "91F") string startTag = $"{startNode.RfidId}{motDir}"; - + + // 시작과 লক্ষ্য가 다른 존일 때 시작점이 일반 노드(예: 3번)라면, + // (예: 70->3 전진 후 3에서 71 로더로 갈 때) + // 현재 방향 체계상 일반노드에서의 GetMonitorMode() 결과가 최종 zonepath의 Monitor 기준과 + // 반전되어 구해지는 현상이 있으므로, 이 경우에만 monitorMode를 반대로 뒤집어서 찾는다. + if (startZone != targetZone && startNode.StationType == StationType.Normal) + { + monitorMode = monitorMode == MapZoneMonitor.LeftTop ? MapZoneMonitor.RightBtm : MapZoneMonitor.LeftTop; + } + // 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다 var zonepath = GetMapZonePathData(); - - // 목적지가 특정 장비(StationType)인 경우를 우선 검색 - var candidates = zonepath.Where(d => - d.Monitor == monitorMode && - d.NodeEnd == targetNode.StationType && - d.Path.Any(p => p.StartsWith(startNode.RfidId.ToString())) && // 시작 포인트 포함 - d.Path.Any(p => p.StartsWith(targetNode.RfidId.ToString())) // 끝 포인트 포함 - ).ToList(); - // 목적지가 일반 노드이거나 StationType이 매칭되지 않아 결과가 없을 경우, - // StationType 조건 없이 모니터 방향과 노드 포함 여부만으로 경로 검색 - if (!candidates.Any()) + IEnumerable candidates; + // 목적지가 현재 위치와 동일한 경우 처리 + + + // 시작Zone과 목표Zone이 다른 경우에만 하드코딩된 zonepath 검색 + if (startZone != targetZone) { - candidates = zonepath.Where(d => - d.Monitor == monitorMode && + // 목적지가 특정 장비(StationType)인 경우를 우선 검색 + candidates = zonepath.Where(d => + d.Monitor == monitorMode && + d.NodeEnd == targetNode.StationType && d.Path.Any(p => p.StartsWith(startNode.RfidId.ToString())) && // 시작 포인트 포함 d.Path.Any(p => p.StartsWith(targetNode.RfidId.ToString())) // 끝 포인트 포함 ).ToList(); - } - if (candidates.Any()) - { - MapZonePathData bestPath = null; - int bestStartIndex = -1; - int bestEndIndex = -1; - int minPathLength = int.MaxValue; - - foreach (var candidate in candidates) + // 목적지가 일반 노드이거나 StationType이 매칭되지 않아 결과가 없을 경우, + // StationType 조건 없이 모니터 방향과 노드 포함 여부만으로 경로 검색 + if (!candidates.Any()) { - // 시작 태그와 가장 일치하는 인덱스 찾기 (방향까지 고려 "91F") - int startIndex = candidate.Path.FindIndex(p => p == startTag); - if (startIndex == -1) // 방향이 안 맞으면 그냥 RFID로만 찾기 - startIndex = candidate.Path.FindIndex(p => p.StartsWith(startNode.RfidId.ToString())); + candidates = zonepath.Where(d => + d.Monitor == monitorMode && + d.Path.Any(p => p.StartsWith(startNode.RfidId.ToString())) && // 시작 포인트 포함 + d.Path.Any(p => p.StartsWith(targetNode.RfidId.ToString())) // 끝 포인트 포함 + ).ToList(); + } - // 끝 태그 인덱스 (뒤에서부터 찾기) - int endIndex = candidate.Path.FindLastIndex(p => p.StartsWith(targetNode.RfidId.ToString())); + if (candidates.Any()) + { + MapZonePathData bestPath = null; + int bestStartIndex = -1; + int bestEndIndex = -1; + int minPathLength = int.MaxValue; - if (startIndex != -1 && endIndex != -1 && startIndex < endIndex) + foreach (var candidate in candidates) { - int length = endIndex - startIndex; - if (length < minPathLength) + // 시작 태그와 가장 일치하는 인덱스 찾기 (방향까지 고려 "91F") + int startIndex = candidate.Path.FindIndex(p => p == startTag); + if (startIndex == -1) // 방향이 안 맞으면 그냥 RFID로만 찾기 + startIndex = candidate.Path.FindIndex(p => p.StartsWith(startNode.RfidId.ToString())); + + // 끝 태그 인덱스 (뒤에서부터 찾기) + int endIndex = candidate.Path.FindLastIndex(p => p.StartsWith(targetNode.RfidId.ToString())); + + if (startIndex != -1 && endIndex != -1 && startIndex < endIndex) { - minPathLength = length; - bestPath = candidate; - bestStartIndex = startIndex; - bestEndIndex = endIndex; + int length = endIndex - startIndex; + if (length < minPathLength) + { + minPathLength = length; + bestPath = candidate; + bestStartIndex = startIndex; + bestEndIndex = endIndex; + } } } - } - if (bestPath != null) - { - // 추출된 경로 조각 - var slicedPath = bestPath.Path.Skip(bestStartIndex).Take(bestEndIndex - bestStartIndex + 1).ToList(); - var a = ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); - return a; + if (bestPath != null) + { + // 추출된 경로 조각 + var slicedPath = bestPath.Path.Skip(bestStartIndex).Take(bestEndIndex - bestStartIndex + 1).ToList(); + var a = ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); + return a; + } } } - // 하드코딩된 경로가 없으면 기존 A* 로직으로 대체 또는 실패 반환 - return null; + //추가로 처리해준다. + if (startZone == MapZone.Buffer && targetZone == MapZone.Buffer) + { + if (startNode.Id == targetNode.Id) + { + //시작과 목표가 동일하다 + if (monitorMode == MapZoneMonitor.RightBtm) + { + //방향과 모두 일치하므로 더이상 이동할 필요가 없다 + //현재위치를 그대로 반환하자 + var result = new AGVPathResult { Success = true }; + result.Path = new List { startNode }; + result.DetailedPath = new List { new NodeMotorInfo(1, startNode.Id, startNode.RfidId, prevDir, null, MagnetDirection.Straight, false) }; + result.TotalDistance = 0; + return result; + } + else + { + //위치는 현재위치이나 모니터방향이 일치하지 않으므로 턴을 한후 경로를 다시 찾아야한다. + + // 태그 내 숫자로 정확히 매칭하기 위한 람다 (예: 3을 찾을 때 "36"이 매칭되지 않도록) + Func matchNode = (tag, rfid) => + { + string idStr = ""; + foreach (char c in tag) if (char.IsDigit(c)) idStr += c; + return idStr == rfid.ToString(); + }; + + // 1.현재위치에서 턴포인트(3)까지 이동하는 경로를 찾는다. + var path1Candidate = zonepath.FirstOrDefault(d => + { + int sIdx = d.Path.FindIndex(p => p == startTag); + int tIdx = d.Path.FindIndex(p => p == "3T"); + return sIdx != -1 && tIdx != -1 && sIdx <= tIdx; + }); + + if (path1Candidate == null) return AGVPathResult.CreateFailure("턴 포인트(3)로 향하는 하드코딩 경로를 찾을 수 없습니다."); + + int path1StartIdx = path1Candidate.Path.FindIndex(p => p == startTag); // 방향까지 일치하는게 우선 + int path1EndIdx = path1Candidate.Path.FindIndex(p => p == "3T"); + + var slicedPath1 = path1Candidate.Path.Skip(path1StartIdx).Take(path1EndIdx - path1StartIdx + 1).ToList(); + var path1 = ConvertHardcodedPathToResult(slicedPath1, startNode, prevNode, prevDir); + if (path1 == null || !path1.Success) return AGVPathResult.CreateFailure("턴 포인트(3) 경로 변환 실패"); + + var lastPrev = path1.Path.Count > 1 ? path1.Path[path1.Path.Count - 2] : prevNode; + var lastDir = path1.DetailedPath.Last().MotorDirection; + var turnNode = path1.Path.Last(); + + // 2.턴포인트에서 목표까지 이동하는 경로를 찾는다. + var path2Candidate = zonepath.FirstOrDefault(d => + { + if (d.Monitor != monitorMode) return false; + int tIdx = d.Path.FindIndex(p => p == "3T"); + int eIdx = d.Path.FindLastIndex(p => p == $"{targetNode.RfidId}B"); + return tIdx != -1 && eIdx != -1 && tIdx <= eIdx; + }); + + if (path2Candidate == null) return AGVPathResult.CreateFailure("턴 포인트에서 목표로 향하는 하드코딩 경로를 찾을 수 없습니다."); + + 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(); + var path2 = ConvertHardcodedPathToResult(slicedPath2, turnNode, lastPrev, lastDir); + if (path2 == null || !path2.Success) return AGVPathResult.CreateFailure("턴 포인트에서 목표로 향하는 경로 변환 실패"); + + // 3. 1+2 경로를 생성한다. + path1.Path.RemoveAt(path1.Path.Count - 1); + path1.DetailedPath.RemoveAt(path1.DetailedPath.Count - 1); + + // 경로를 반환한다. + return CombinePaths(path1, path2); + } + } + else + { + //시작과 목표노드가 다른경우의 처리 + if (monitorMode == MapZoneMonitor.RightBtm) + { + //모니터가 우측이라면 방향은 맞다 + if (startNode.StationType == StationType.Normal) + { + //일반노드에서 시작했다면 그대로 목표로 이동한다 + //A*로 경로를 생성해서 진행한다.모터는 B로 이동하면된다 + return this.FindBasicPath(startNode, targetNode, prevNode, AgvDirection.Backward); + } + else + { + //버퍼위치에서 다른 버퍼위치로 이동하는 경우인데. + //목표위치가 좌측에 있다면 그대로 이동하면된다. + bool isTargetLeft = targetNode.Position.X < startNode.Position.X; + if (isTargetLeft) + { + return this.FindBasicPath(startNode, targetNode, prevNode, AgvDirection.Backward); + } + else + { + // 목표위치가 우측에 있다면 목표위치보다 한번 더 우측으로 이동해서 좌측으로 다시 진입 + var endBufferNode = _mapNodes.FirstOrDefault(n => n.RfidId == 7); + if (endBufferNode == null) return AGVPathResult.CreateFailure("버퍼 끝 노드(7)를 찾을 수 없습니다."); + + var overPathFull = this.FindBasicPath(startNode, endBufferNode, prevNode, AgvDirection.Forward); + if (overPathFull == null || !overPathFull.Success) + return AGVPathResult.CreateFailure("Overshoot 전체 경로(7번 방향) 탐색 실패"); + + int targetIdx = overPathFull.Path.FindIndex(n => n.Id == targetNode.Id); + if (targetIdx == -1 || targetIdx == overPathFull.Path.Count - 1) + 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 autoOverNode = overPath.Path.Last(); // 오버슈트 된 곳 + var lastDet = overPath.DetailedPath.Last(); + lastDet.MotorDirection = AgvDirection.Backward; //방향을 변경 해준다. + + // 오버슈트 위치에서 다시 Backward로 뒤로 한 칸 이동해 targetNode에 최종 진입 + overPath.Path.Add(targetNode); + overPath.DetailedPath.Add(new NodeMotorInfo(lastDet.seq + 1, targetNode.Id, targetNode.RfidId, AgvDirection.Backward) + { + Speed = SpeedLevel.L, + }); + + return overPath; + } + } + } + else + { + //모니터가 좌측이라면 턴이 필요하다. 버퍼에서 -> 턴위치까지 경로를 계산한다. + //1. 현재위치에서 턴까지 후진으로 이동을한다(현재는 사용자의 코딩 스타일에 맞춰 하드코딩된 zonepath에서 추출 적용) + var path1Candidate = zonepath.FirstOrDefault(d => + { + int sIdx = d.Path.FindIndex(p => p == startTag); + int tIdx = d.Path.FindIndex(p => p == "3T"); + return sIdx != -1 && tIdx != -1 && sIdx <= tIdx; + }); + + if (path1Candidate == null) return AGVPathResult.CreateFailure("턴 포인트(3)로 향하는 하드코딩 경로를 찾을 수 없습니다."); + + int path1StartIdx = path1Candidate.Path.FindIndex(p => p == startTag); + int path1EndIdx = path1Candidate.Path.FindIndex(p => p == "3T"); + + var slicedPath1 = path1Candidate.Path.Skip(path1StartIdx).Take(path1EndIdx - path1StartIdx + 1).ToList(); + var path1 = ConvertHardcodedPathToResult(slicedPath1, startNode, prevNode, prevDir); + if (path1 == null || !path1.Success) return AGVPathResult.CreateFailure("턴 포인트(3) 경로 변환 실패"); + + var lastPrev = path1.Path.Count > 1 ? path1.Path[path1.Path.Count - 2] : prevNode; + var lastDir = path1.DetailedPath.Last().MotorDirection; + var turnNode = path1.Path.Last(); + + //2. zonepath 에서 해당 경로를 찾아서 업데이트 한다. + var path2Candidate = zonepath.FirstOrDefault(d => + { + if (d.Monitor != monitorMode) return false; + int tIdx = d.Path.FindIndex(p => p == "3T"); + int eIdx = d.Path.FindLastIndex(p => p == $"{targetNode.RfidId}B"); + return tIdx != -1 && eIdx != -1 && tIdx <= eIdx; + }); + + if (path2Candidate == null) return AGVPathResult.CreateFailure("턴 포인트에서 목표로 향하는 하드코딩 경로를 찾을 수 없습니다."); + + 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(); + var path2 = ConvertHardcodedPathToResult(slicedPath2, turnNode, lastPrev, lastDir); + if (path2 == null || !path2.Success) return AGVPathResult.CreateFailure("턴 포인트에서 목표로 향하는 경로 변환 실패"); + + // 3. 1+2 경로 통합 + path1.Path.RemoveAt(path1.Path.Count - 1); + path1.DetailedPath.RemoveAt(path1.DetailedPath.Count - 1); + + return CombinePaths(path1, path2); + } + } + } + else + { + //다른경우는 아직 처리하지 않는다 + } + + return AGVPathResult.CreateFailure("경로를 계산할 수 없습니다"); } private MapZoneMonitor GetMonitorMode(MapNode startNode, MapNode prevNode, AgvDirection prevDir) @@ -679,7 +883,7 @@ namespace AGVNavigationCore.PathFinding.Planning var nextTag = pathStrings[i + 1]; string nextRfidStr = ""; foreach (char c in nextTag) if (char.IsDigit(c)) nextRfidStr += c; - + var nextNode = _mapNodes.FirstOrDefault(n => n.RfidId.ToString() == nextRfidStr); if (nextNode != null && node.MagnetDirections.ContainsKey(nextNode.Id)) { diff --git a/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs b/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs index c385149..efb1690 100644 --- a/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs +++ b/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs @@ -1851,8 +1851,15 @@ namespace AGVSimulator.Forms // 첫 번째 AGV의 다음 행동 예측 var agv = _agvList[0]; - var command = agv.Predict(); - this.lbPredict.Text = $"Motor:{command.Motor},Magnet:{command.Magnet},Speed:{command.Speed} : {command.Message}"; + try + { + var command = agv.Predict(); + this.lbPredict.Text = $"Motor:{command.Motor},Magnet:{command.Magnet},Speed:{command.Speed} : {command.Message}"; + }catch ( Exception ex) + { + lbPredict.Text = "예측오류" + ex.Message; + } + }