diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs b/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs index ef63a38..bc4418c 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs @@ -165,7 +165,7 @@ namespace AGVNavigationCore.PathFinding.Core Success = false, Message = errorMessage, CalculationTimeMs = calculationTimeMs, - ExploredNodes = exploredNodes + ExploredNodes = exploredNodes, }; } diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 0633e92..cd850f0 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -171,7 +171,24 @@ namespace AGVNavigationCore.PathFinding.Planning //다음 노드ID를 확인해서 마그넷 방향 데이터를 찾는다. if (node.MagnetDirections.ContainsKey(nextNode.Id) == false) { - return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0); + //대상노드가 위에있고 해당 노드위로 갈림길이 하나라면 s 로 반환한다. (y축값이 일정이상 차이가 나야한다) + byte realconncount = 0; + if (nextNode.ConnectedMapNodes.Count == 1 && Math.Abs(nextNode.Position.Y - node.Position.Y) > 20) + { + foreach (var cnode in node.ConnectedMapNodes) + { + var ydiff = Math.Abs(cnode.Position.Y - node.Position.Y); + if ((ydiff > 20)) //오차가 있는경우 + { + if (cnode.Position.Y < node.Position.Y && nextNode.Position.Y < node.Position.Y) realconncount += 1; + if (cnode.Position.Y > node.Position.Y && nextNode.Position.Y > node.Position.Y) realconncount += 1; + } + } + } + if (realconncount != 1) + return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0); + else + magnetDirection = MagnetDirection.Straight; } else { @@ -473,7 +490,7 @@ namespace AGVNavigationCore.PathFinding.Planning Path = new List(); Path.AddRange(_path); } - + } public List GetMapZonePathData() @@ -825,109 +842,162 @@ namespace AGVNavigationCore.PathFinding.Planning //모니터의 방향이 동일하고 20F -> 70B (목표노드의 방향에 따라서 목적지으 ㅣFB는 결정한다 일반노드라면 방향상관없이 검색한다 - 기본은 시작이 방향과 동일하게 한다) //경로의 시작이 경로의 끝보다 index가 먼저 나와야한다. //현재 진행방향의 경로와, 반대방향의 경로를 2개 추출해서.. 이전에 지나온 경로를 체크한다. - string SearchTagS, SearchTagE, SearchTagE1; - - for (int i = 0; i < 2; i++) - { - //진입순서에 따라서 검색하는 대상을 바꿔준다(진행방향 반대방향) - if (i == 0) + // 1) zonepath에서 기본 기준(모니터 방향 일치, 시작/도착 노드 존재 여부)으로 필터링 + var validCandidates = zonepath + .Where(d => d.Monitor == monitorMode) + .Select(d => { - SearchTagS = $"{startNode.RfidId}{motDir}"; - SearchTagE = $"{targetNode.RfidId}"; + // 정확한 RFID 번호만 먼저 확인 (예: "36F"에서 "36" 추출) + var parsedPath = d.Path.Select(tag => new { Tag = tag, IdStr = new string(tag.Where(char.IsDigit).ToArray()) }).ToList(); + + int sIdx = parsedPath.FindIndex(p => p.IdStr == startNode.RfidId.ToString()); + + int eIdx = -1; if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt) { - SearchTagE += "B"; //모든 스테이션은 후진으로 도킹을 해야한다 - SearchTagE1 = ""; + // Station은 반드시 후진(B)으로 도착해야 한다. + eIdx = parsedPath.FindLastIndex(p => p.Tag == $"{targetNode.RfidId}B"); } else { - SearchTagE += motDir; - SearchTagE1 = targetTag + (motDir == 'F' ? "B" : "F"); + // 일반 노드는 방향(F/B) 상관없이 마지막 출현 위치 찾기 + eIdx = parsedPath.FindLastIndex(p => p.IdStr == targetNode.RfidId.ToString()); } + + return new { PathData = d, StartIdx = sIdx, EndIdx = eIdx, Parsed = parsedPath }; + }) + .Where(x => x.StartIdx != -1 && x.EndIdx != -1 && x.StartIdx < x.EndIdx) + .Select(x => + { + var slicedTags = x.PathData.Path.Skip(x.StartIdx).Take(x.EndIdx - x.StartIdx + 1).ToList(); + return new { x.PathData, Sliced = slicedTags, Length = x.EndIdx - x.StartIdx }; + }) + .ToList(); + + // 2) 물리적인 모순 방지 (Backtracking 방향성 필터링) + // 바로 이전에 온 길(prevNode)로 되돌아가려 할 때, 온 방향(prevDir)과 앞으로 갈 모터 방향(F/B)이 같다면 + // 물리적으로 불가능한 움직임(결국 제자리에서 반대 방향으로 못 가고 직진해버림)이므로 필터링에서 제외한다. + var physicallyValidCandidates = validCandidates.Where(c => + { + if (c.Sliced.Count > 1 && prevNode != null) + { + // 다음 이동 경로가 방금 전에 있던 prevNode 라면? + string nextNodeIdStr = new string(c.Sliced[1].Where(char.IsDigit).ToArray()); + if (nextNodeIdStr == prevNode.RfidId.ToString()) + { + // 첫 이동의 모터 방향 확인 + char firstMoveMotDir = c.Sliced[0].Last(); + char prevMotDirChar = prevDir == AgvDirection.Backward ? 'B' : 'F'; + + // 오던 방향(F)으로 계속 가면서, 왔던 길로 되돌아갈 수는 없다! (물리적 모순) + if (firstMoveMotDir == prevMotDirChar) + { + return false; // 이 경로는 무효! (반대 방향인 'B'로 시작되는 올바른 경로만 남겨야 함) + } + } + } + return true; + }).ToList(); + + // 3) 조건에 부합하는 가장 짧은(최적) 경로 한 개 반환 + if (physicallyValidCandidates.Any()) + { + var bestCandidate = physicallyValidCandidates.OrderBy(c => c.Length).First(); + return ConvertHardcodedPathToResult(bestCandidate.Sliced, startNode, prevNode, prevDir); + } + + + + //(버퍼존의 경우엔은 추가로 처리해준다.) + if (startZone == MapZone.Buffer && targetZone == MapZone.Buffer) + { + //모니터가 왼쪽이라면 턴을 해야하낟. + + if (monitorMode == MonDir.LeftTop) + { + //위치는 현재위치이나 모니터방향이 일치하지 않으므로 턴을 한후 경로를 다시 찾아야한다. 오버슛이 필요하지 않다 + var BufferPath = ("36B,35B,31B,32B,33B,34B,20B,9B,8B,7B,11B,3T,3B,11B,7B,8B,9B,20B,34B,33B,32B,31B,35B,36B").Split(','); + var startTagB = startNode.RfidId + "B"; //이 경우에는 반드시 우측으로 가야하니 Back 이동을 해야 한다 + var endTagB = targetNode.RfidId + "B"; + int firstIdx = Array.IndexOf(BufferPath, startTagB); + int lastIdx = Array.LastIndexOf(BufferPath, endTagB); + if (firstIdx != -1 && lastIdx != -1 && firstIdx < lastIdx) + { + var slicedPath = BufferPath.Skip(firstIdx).Take(lastIdx - firstIdx + 1).ToList(); + return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); + } + return AGVPathResult.CreateFailure("버퍼 공용 경로에서 정기 턴 경로를 생성할 수 없습니다."); } else { - SearchTagS = $"{startNode.RfidId}{(motDir == 'F' ? 'B' : 'F')}"; - SearchTagE = $"{targetNode.RfidId}"; - if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt) + //여긴 모니터가 우측방향에 있는 경우이며, 우측에 있다면 큰 문제없이 좌로 이동해서 목적지를 설정하면 된다 + if (startNode.Id == targetNode.Id) { - SearchTagE += "B"; //모든 스테이션은 후진으로 도킹을 해야한다 - SearchTagE1 = ""; + //방향과 모두 일치하므로 더이상 이동할 필요가 없다 - 현재위치를 그대로 반환한다 + var result = new AGVPathResult { Success = true }; + result.Path = new List { startNode }; + result.DetailedPath = new List { new NodeMotorInfo(1, startNode.Id, startNode.RfidId, AgvDirection.Backward, null, MagnetDirection.Straight, false) }; + result.TotalDistance = 0; + return result; } else { - SearchTagE += motDir; - SearchTagE1 = targetTag + (motDir == 'F' ? "B" : "F"); // 오히려 반대로 처리해준다. - } - } - - - - candidates = zonepath.Where(d => - d.Monitor == monitorMode && - d.Path.Contains(SearchTagS) && - d.Path.Contains(SearchTagE) - ).Where(d => - { - int startIndex = d.Path.FindIndex(p => p.Equals(SearchTagS)); - int endIndex = d.Path.FindLastIndex(p => p.Equals(SearchTagE)); - if (endIndex == -1 && SearchTagE1 != "") - endIndex = d.Path.FindLastIndex(p => p.Equals(SearchTagE1)); - - return startIndex != -1 && endIndex != -1 && startIndex < endIndex; - }).ToList(); - - //찾아진 값이 있다면 slice 해서 그 경로를반환한다. - if (candidates.Any()) - { - PathData bestPath = null; - int bestStartIndex = -1; - int bestEndIndex = -1; - int minPathLength = int.MaxValue; - - foreach (var candidate in candidates) - { - int startIndex = candidate.Path.FindIndex(p => p.Equals(SearchTagS)); - int endIndex = candidate.Path.FindLastIndex(p => p.Equals(SearchTagE)); - if (endIndex == -1 && SearchTagE1 != "") - endIndex = candidate.Path.FindLastIndex(p => p.Equals(SearchTagE1)); - - int length = endIndex - startIndex; - if (length < minPathLength) + //버퍼위치에서 다른 버퍼위치로 이동하는 경우인데. 목표위치가 좌측에 있다면 그대로 이동하면된다. + bool isTargetLeft = targetNode.Position.X < startNode.Position.X; + if (isTargetLeft) { - minPathLength = length; - bestPath = candidate; - bestStartIndex = startIndex; - bestEndIndex = endIndex; - } - } - - if (bestPath != null) - { - var slicedPath = bestPath.Path.Skip(bestStartIndex).Take(bestEndIndex - bestStartIndex + 1).ToList(); - - // 검증: 첫 번째 이동이 이전 노드(prevNode) 방향으로 가는 것이라면, 모터 방향을 반전시켜야 할 수 있음. - if (slicedPath.Count > 1 && prevNode != null) - { - var nextNodeInPath = _mapNodes.FirstOrDefault(n => n.RfidId.ToString() == new string(slicedPath[1].Where(char.IsDigit).ToArray())); - if (nextNodeInPath != null && nextNodeInPath.Id == prevNode.Id) + //대상이 좌측에 있으므로 기본 경로내에서 + var BufferPath = ("7B,8B,9B,20B,34B,33B,32B,31B,35B,36B").Split(','); + var startTagB = startNode.RfidId + "B"; + var endTagB = targetNode.RfidId + "B"; + int firstIdx = Array.IndexOf(BufferPath, startTagB); + int lastIdx = Array.LastIndexOf(BufferPath, endTagB); + if (firstIdx != -1 && lastIdx != -1 && firstIdx < lastIdx) { - // 되돌아가는 상황: 첫 노드의 방향 플래그를 반전시킨다. - string firstTag = slicedPath[0]; - char currentFlag = firstTag.Last(); - if (currentFlag == 'F' || currentFlag == 'B') - { - char reversedFlag = currentFlag == 'F' ? 'B' : 'F'; - slicedPath[0] = firstTag.Substring(0, firstTag.Length - 1) + reversedFlag; - } + var slicedPath = BufferPath.Skip(firstIdx).Take(lastIdx - firstIdx + 1).ToList(); + return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); } + return AGVPathResult.CreateFailure("버퍼 공용 경로에서 정기 턴 경로를 생성할 수 없습니다."); + } + 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; } - return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); } } - } @@ -935,29 +1005,30 @@ namespace AGVNavigationCore.PathFinding.Planning - //이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다 - //그런경우는 복잡하게 추가 계산할 필요가 없으니까 - 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())); + ////이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다 + ////그런경우는 복잡하게 추가 계산할 필요가 없으니까 + //var exactMatchList = zonepath.Where(d => + // d.NodeSta == startNode.StationType && + // d.NodeEnd == targetNode.StationType && + // d.Monitor == monitorMode); - 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())); + //var exactMatch = exactMatchList.FirstOrDefault(d => + // d.Path.First().StartsWith(startNode.RfidId.ToString()) && + // d.Path.Last().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); - } - } + //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); + // } + //} @@ -1023,97 +1094,6 @@ namespace AGVNavigationCore.PathFinding.Planning } } - //추가로 처리해준다. - if (startZone == MapZone.Buffer && targetZone == MapZone.Buffer) - { - //모니터가 왼쪽이라면 턴을 해야하낟. - - if (monitorMode == MonDir.LeftTop) - { - //위치는 현재위치이나 모니터방향이 일치하지 않으므로 턴을 한후 경로를 다시 찾아야한다. 오버슛이 필요하지 않다 - var BufferPath = ("36B,35B,31B,32B,33B,34B,20B,9B,8B,7B,11B,3T,3B,11B,7B,8B,9B,20B,34B,33B,32B,31B,35B,36B").Split(','); - var startTagB = startNode.RfidId + "B"; //이 경우에는 반드시 우측으로 가야하니 Back 이동을 해야 한다 - var endTagB = targetNode.RfidId + "B"; - int firstIdx = Array.IndexOf(BufferPath, startTagB); - int lastIdx = Array.LastIndexOf(BufferPath, endTagB); - if (firstIdx != -1 && lastIdx != -1 && firstIdx < lastIdx) - { - var slicedPath = BufferPath.Skip(firstIdx).Take(lastIdx - firstIdx + 1).ToList(); - return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); - } - return AGVPathResult.CreateFailure("버퍼 공용 경로에서 정기 턴 경로를 생성할 수 없습니다."); - } - else - { - //여긴 모니터가 우측방향에 있는 경우이며, 우측에 있다면 큰 문제없이 좌로 이동해서 목적지를 설정하면 된다 - if (startNode.Id == targetNode.Id) - { - //방향과 모두 일치하므로 더이상 이동할 필요가 없다 - 현재위치를 그대로 반환한다 - 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 - { - //버퍼위치에서 다른 버퍼위치로 이동하는 경우인데. 목표위치가 좌측에 있다면 그대로 이동하면된다. - bool isTargetLeft = targetNode.Position.X < startNode.Position.X; - if (isTargetLeft) - { - //대상이 좌측에 있으므로 기본 경로내에서 - var BufferPath = ("7B,8B,9B,20B,34B,33B,32B,31B,35B,36B").Split(','); - var startTagB = startNode.RfidId + "B"; - var endTagB = targetNode.RfidId + "B"; - int firstIdx = Array.IndexOf(BufferPath, startTagB); - int lastIdx = Array.LastIndexOf(BufferPath, endTagB); - if (firstIdx != -1 && lastIdx != -1 && firstIdx < lastIdx) - { - var slicedPath = BufferPath.Skip(firstIdx).Take(lastIdx - firstIdx + 1).ToList(); - return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); - } - return AGVPathResult.CreateFailure("버퍼 공용 경로에서 정기 턴 경로를 생성할 수 없습니다."); - } - 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 { // diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/test.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/test.cs new file mode 100644 index 0000000..169e248 --- /dev/null +++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/test.cs @@ -0,0 +1,11 @@ +using System; +using System.Collections.Generic; +using System.Linq; + +class Program { + static void Main() { + var paths = new List { new[] { \"20F\", \"21F\", \"70B\" } }; + var valid = paths.Select(p => p.Select(t => new { Tag = t, IdStr = new string(t.Where(char.IsDigit).ToArray()) }).ToList()).ToList(); + Console.WriteLine(\"Compiled\"); + } +} diff --git a/AGVLogic/AGVSimulator/fMain.cs b/AGVLogic/AGVSimulator/fMain.cs index 466df95..3ea63b2 100644 --- a/AGVLogic/AGVSimulator/fMain.cs +++ b/AGVLogic/AGVSimulator/fMain.cs @@ -1774,7 +1774,7 @@ namespace AGVSimulator.Forms var startNode = (_startNodeCombo.SelectedItem as ComboBoxItem)?.Value; var targetNode = (_targetNodeCombo.SelectedItem as ComboBoxItem)?.Value; var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; - var calcResult = CalcPath(startNode, targetNode, this._simulatorCanvas.Nodes, selectedAGV.PrevNode, selectedAGV.PrevDirection); + var calcResult = CalcPath_New(startNode, targetNode, this._simulatorCanvas.Nodes, selectedAGV.PrevNode, selectedAGV.PrevDirection); //// 테스트 결과 생성 testResult = CreateTestResultFromUI(nodeA, dockingTarget, directionName, calcResult);