로직 확인 중

AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs

@@ -165,7 +165,7 @@ namespace AGVNavigationCore.PathFinding.Core
                 Success = false,
                 Message = errorMessage,
                 CalculationTimeMs = calculationTimeMs,
-                ExploredNodes = exploredNodes
+                ExploredNodes = exploredNodes,
             };
         }
 

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,6 +490,7 @@ namespace AGVNavigationCore.PathFinding.Planning
                 Path = new List<string>();
                 Path.AddRange(_path);
             }
+
         }
 
         public List<PathData> GetMapZonePathData()
@@ -795,11 +813,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,35 +836,202 @@ namespace AGVNavigationCore.PathFinding.Planning
                 return simplepath;
             }
 
-
             IEnumerable<PathData> candidates;
 
-            //이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다
+            //시작과 목표가 포함된 경로르 모두 찾아서 이용하도록 한다(zonepath 내에서 모두 검색한다)
-            //그런경우는 복잡하게 추가 계산할 필요가 없으니까
+            //모니터의 방향이 동일하고 20F -> 70B (목표노드의 방향에 따라서 목적지으 ㅣFB는 결정한다 일반노드라면 방향상관없이 검색한다 - 기본은 시작이 방향과 동일하게 한다)
-            var exactMatchList = zonepath.Where(d =>
+            //경로의 시작이 경로의 끝보다 index가 먼저 나와야한다.
-                d.NodeSta == startNode.StationType &&
+            //현재 진행방향의 경로와, 반대방향의 경로를 2개 추출해서.. 이전에 지나온 경로를 체크한다.
-                d.NodeEnd == targetNode.StationType &&
+            // 1) zonepath에서 기본 기준(모니터 방향 일치, 시작/도착 노드 존재 여부)으로 필터링
-                d.Monitor == monitorMode);
+            var validCandidates = zonepath
-
+                .Where(d => d.Monitor == monitorMode)
-            var exactMatch = exactMatchList.FirstOrDefault(d =>
+                .Select(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();
+                    // 정확한 RFID 번호만 먼저 확인 (예: "36F"에서 "36" 추출)
-                    return ConvertHardcodedPathToResult(slicedPath, startNode, prevNode, prevDir);
+                    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)
+                    {
+                        // Station은 반드시 후진(B)으로 도착해야 한다.
+                        eIdx = parsedPath.FindLastIndex(p => p.Tag == $"{targetNode.RfidId}B");
+                    }
+                    else
+                    {
+                        // 일반 노드는 방향(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
+                {
+                    //여긴 모니터가 우측방향에 있는 경우이며, 우측에 있다면 큰 문제없이 좌로 이동해서 목적지를 설정하면 된다
+                    if (startNode.Id == targetNode.Id)
+                    {
+                        //방향과 모두 일치하므로 더이상 이동할 필요가 없다 - 현재위치를 그대로 반환한다
+                        var result = new AGVPathResult { Success = true };
+                        result.Path = new List<MapNode> { startNode };
+                        result.DetailedPath = new List<NodeMotorInfo> { new NodeMotorInfo(1, startNode.Id, startNode.RfidId,  AgvDirection.Backward, 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;
+                        }
+
+                    }
+                }
+
+            }
+
+
+
+
+
+
+
+            ////이곳에서 시작,종료노드가 완전히 일치하는 경로를 찾고 있다면 그것을 바로 반환한다
+            ////그런경우는 복잡하게 추가 계산할 필요가 없으니까
+            //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)
             {
@@ -898,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<MapNode> { startNode };
-                        result.DetailedPath = new List<NodeMotorInfo> { 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
             {
                 //
@@ -1134,7 +1239,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 +1247,7 @@ namespace AGVNavigationCore.PathFinding.Planning
                 else //왼쪽에서 왔다
                 {
                     if (prevDir == AgvDirection.Forward) return MonDir.RightBtm;
-                    else return MonDir.LeftTop;  
+                    else return MonDir.LeftTop;
                 }
             }
 

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<string[]> { 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\");
+    }
+}

AGVLogic/AGVSimulator/fMain.cs

@@ -1774,7 +1774,7 @@ namespace AGVSimulator.Forms
                             var startNode = (_startNodeCombo.SelectedItem as ComboBoxItem<MapNode>)?.Value;
                             var targetNode = (_targetNodeCombo.SelectedItem as ComboBoxItem<MapNode>)?.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);