diff --git a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index a3cc7ca..0633e92 100644 --- a/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -473,6 +473,7 @@ namespace AGVNavigationCore.PathFinding.Planning Path = new List(); Path.AddRange(_path); } + } public List GetMapZonePathData() @@ -795,11 +796,22 @@ namespace AGVNavigationCore.PathFinding.Planning // 시작 태그 검색용 (예: "91F") string startTag = $"{startNode.RfidId}{motDir}"; + string targetTag = $"{targetNode.RfidId}"; + string targetTag1; + if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt) + { + targetTag += "B"; //모든 스테이션은 후진으로 도킹을 해야한다 + targetTag1 = ""; + } + else + { + targetTag1 = targetTag + (motDir == 'F' ? "B" : "F"); + targetTag += motDir; + } // 모니터방향이 일치하고 대상노드가 동일한 경로를 찾는다 var zonepath = GetMapZonePathData(); - //목표가 일반노드라면 단순 경로 생성해서 반환한다. if (targetNode.StationType == Station.Normal) { @@ -807,9 +819,122 @@ namespace AGVNavigationCore.PathFinding.Planning return simplepath; } - IEnumerable candidates; + //시작과 목표가 포함된 경로르 모두 찾아서 이용하도록 한다(zonepath 내에서 모두 검색한다) + //모니터의 방향이 동일하고 20F -> 70B (목표노드의 방향에 따라서 목적지으 ㅣFB는 결정한다 일반노드라면 방향상관없이 검색한다 - 기본은 시작이 방향과 동일하게 한다) + //경로의 시작이 경로의 끝보다 index가 먼저 나와야한다. + //현재 진행방향의 경로와, 반대방향의 경로를 2개 추출해서.. 이전에 지나온 경로를 체크한다. + string SearchTagS, SearchTagE, SearchTagE1; + + for (int i = 0; i < 2; i++) + { + //진입순서에 따라서 검색하는 대상을 바꿔준다(진행방향 반대방향) + if (i == 0) + { + SearchTagS = $"{startNode.RfidId}{motDir}"; + SearchTagE = $"{targetNode.RfidId}"; + if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt) + { + SearchTagE += "B"; //모든 스테이션은 후진으로 도킹을 해야한다 + SearchTagE1 = ""; + } + else + { + SearchTagE += motDir; + SearchTagE1 = targetTag + (motDir == 'F' ? "B" : "F"); + } + } + else + { + SearchTagS = $"{startNode.RfidId}{(motDir == 'F' ? 'B' : 'F')}"; + SearchTagE = $"{targetNode.RfidId}"; + if (targetNode.StationType != Station.Normal && targetNode.StationType != Station.Lmt) + { + SearchTagE += "B"; //모든 스테이션은 후진으로 도킹을 해야한다 + SearchTagE1 = ""; + } + 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) + { + 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) + { + // 되돌아가는 상황: 첫 노드의 방향 플래그를 반전시킨다. + 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; + } + } + } + + return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir); + } + } + + + } + + + + + + //이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다 //그런경우는 복잡하게 추가 계산할 필요가 없으니까 var exactMatchList = zonepath.Where(d => @@ -1134,7 +1259,7 @@ namespace AGVNavigationCore.PathFinding.Planning if (startNode.RfidId == 8 || startNode.RfidId == 9 || startNode.RfidId == 20) { - if(prevNode.Position.X > startNode.Position.X) //오른쪽에서 왔다. + if (prevNode.Position.X > startNode.Position.X) //오른쪽에서 왔다. { if (prevDir == AgvDirection.Forward) return MonDir.LeftTop; //오른쪽에서 전진으로 왔다면 모니터는 좌측에있다. else return MonDir.RightBtm; //오른쪽에서 후진으로 왔다면 모니터는 우측에 있다. @@ -1142,7 +1267,7 @@ namespace AGVNavigationCore.PathFinding.Planning else //왼쪽에서 왔다 { if (prevDir == AgvDirection.Forward) return MonDir.RightBtm; - else return MonDir.LeftTop; + else return MonDir.LeftTop; } }