From 24a14fbd485df59050be04ab42dc5aee089ce0e2 Mon Sep 17 00:00:00 2001 From: backuppc Date: Tue, 28 Oct 2025 17:33:49 +0900 Subject: [PATCH] .. --- .../AGVNavigationCore/Models/IMovableAGV.cs | 5 - .../AGVNavigationCore/Models/VirtualAGV.cs | 69 ++---------- .../PathFinding/Planning/AGVPathfinder.cs | 101 +++++++++++++++--- .../Forms/SimulatorForm.Designer.cs | 13 --- .../AGVSimulator/Forms/SimulatorForm.cs | 23 ---- 5 files changed, 92 insertions(+), 119 deletions(-) diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs index 4ffad33..32360bf 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs @@ -151,11 +151,6 @@ namespace AGVNavigationCore.Models #region Path Execution Methods - /// - /// 경로 실행 - /// - void ExecutePath(AGVPathResult path, List mapNodes); - /// /// 경로 정지 /// diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs index 6387855..4f518ba 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs @@ -258,62 +258,8 @@ namespace AGVNavigationCore.Models #region Public Methods - 경로 실행 - /// - /// 경로 실행 시작 - /// - /// 실행할 경로 - /// 맵 노드 목록 - public void ExecutePath(AGVPathResult path, List mapNodes) - { - if (path == null || !path.Success) - { - OnError("유효하지 않은 경로입니다."); - return; - } - - _currentPath = path; - _remainingNodes = new List(path.Path.Select(n => n.NodeId).ToList()); - _currentNodeIndex = 0; - - // 시작 노드와 목표 노드 설정 - if (_remainingNodes.Count > 0) - { - var startNode = mapNodes.FirstOrDefault(n => n.NodeId == _remainingNodes[0]); - if (startNode != null) - { - _currentNode = startNode; - - // 목표 노드 설정 (경로의 마지막 노드) - if (_remainingNodes.Count > 1) - { - var targetNodeId = _remainingNodes[_remainingNodes.Count - 1]; - var targetNode = mapNodes.FirstOrDefault(n => n.NodeId == targetNodeId); - - // 목표 노드의 타입에 따라 도킹 방향 결정 - if (targetNode != null) - { - _dockingDirection = GetDockingDirection(targetNode.Type); - _prevNode = targetNode; - } - } - - StartMovement(); - } - else - { - OnError($"시작 노드를 찾을 수 없습니다: {_remainingNodes[0]}"); - } - } - } - - /// - /// 간단한 경로 실행 (경로 객체 없이 노드만) - /// - public void StartPath(AGVPathResult path, List mapNodes) - { - ExecutePath(path, mapNodes); - } - + + /// /// 경로 정지 /// @@ -428,14 +374,15 @@ namespace AGVNavigationCore.Models { _prevPosition = _currentPosition; // 이전 위치 _prevNode = _currentNode; - } - - //모터방향이 다르다면 적용한다 - if (_currentDirection != motorDirection) - { _prevDirection = _currentDirection; } + ////모터방향이 다르다면 적용한다 + //if (_currentDirection != motorDirection) + //{ + // _prevDirection = motorDirection; + //} + // 새로운 위치 설정 _currentPosition = node.Position; _currentDirection = motorDirection; diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index b8f6472..afb699c 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -128,8 +128,14 @@ namespace AGVNavigationCore.PathFinding.Planning //정방향/역방향 이동 시 다음 노드 확인 // 경로 계획 단계에서는 마그넷 방향이 미리 알려지지 않으므로 Straight로 기본값 사용 - var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, MagnetDirection.Straight, _mapNodes); - var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes); + + // ✅ 현재 방향 유지: prevDirection = currentDirection (방향 일관성) + var nextNodeForward = DirectionalHelper.GetNextNodeByDirection( + startNode, prevNode, currentDirection, currentDirection, MagnetDirection.Straight, _mapNodes); + + // ✅ 방향 전환: prevDirection = currentDirection, direction = ReverseDirection + var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection( + startNode, prevNode, currentDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes); //2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행) @@ -137,7 +143,7 @@ namespace AGVNavigationCore.PathFinding.Planning (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) || (targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward)) { - if (nextNodeForward.NodeId == pathResult.Path[1].NodeId) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다 + if ((nextNodeForward?.NodeId ?? string.Empty) == pathResult.Path[1].NodeId) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다 { MakeDetailData(pathResult, currentDirection); MakeMagnetDirection(pathResult); @@ -160,17 +166,32 @@ namespace AGVNavigationCore.PathFinding.Planning //뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다. - if (nextNodeBackward != null && pathResult.Path.Count > 1 && nextNodeBackward.NodeId == pathResult.Path[1].NodeId && targetNode.DockDirection == DockingDirection.Backward) + // ⚠️ 단, 현재 방향과 목적지 도킹 방향이 일치해야 함! + if (nextNodeBackward != null && pathResult.Path.Count > 1 && + nextNodeBackward.NodeId == pathResult.Path[1].NodeId) // ✅ 추가: 현재도 Backward여야 함 { - MakeDetailData(pathResult, ReverseDirection); - MakeMagnetDirection(pathResult); - return pathResult; + if (targetNode.DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward || + targetNode.DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward) + { + MakeDetailData(pathResult, ReverseDirection); + MakeMagnetDirection(pathResult); + return pathResult; + } } - if (nextNodeForward != null && pathResult.Path.Count > 1 && nextNodeForward.NodeId == pathResult.Path[1].NodeId && targetNode.DockDirection == DockingDirection.Forward) + + if (nextNodeForward != null && pathResult.Path.Count > 1 && + nextNodeForward.NodeId == pathResult.Path[1].NodeId && + targetNode.DockDirection == DockingDirection.Forward && + currentDirection == AgvDirection.Forward) // ✅ 추가: 현재도 Forward여야 함 { - MakeDetailData(pathResult, currentDirection); - MakeMagnetDirection(pathResult); - return pathResult; + if (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward || + targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward) + { + MakeDetailData(pathResult, currentDirection); + MakeMagnetDirection(pathResult); + return pathResult; + } + } //if(nextNodeForward.NodeId == pathResult.Path[1]) @@ -201,15 +222,16 @@ namespace AGVNavigationCore.PathFinding.Planning path1.PrevNode = prevNode; path1.PrevDirection = prevDirection; - //다음좌표를 보고 정방향인지 역방향인지 체크한다. + //다음좌표를 보고 교차로가 진행방향인지 반대방향인지 체크한다.(!모터의 정/역방향을 말하는것이 아님) bool ReverseCheck = false; if (path1.Path.Count > 1 && nextNodeForward != null && nextNodeForward.NodeId.Equals(path1.Path[1].NodeId)) { + ReverseCheck = false; //현재 진행 방향으로 이동해야한다 MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정) } else if (path1.Path.Count > 1 && nextNodeBackward != null && nextNodeBackward.NodeId.Equals(path1.Path[1].NodeId)) { - ReverseCheck = true; + ReverseCheck = true; //현재 방향의 반대방향으로 이동해야한다 MakeDetailData(path1, ReverseDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정) } else return AGVPathResult.CreateFailure("교차로까지 계산된 경로에 현재 위치정보로 추측을 할 수 없습니다", 0, 0); @@ -224,8 +246,17 @@ namespace AGVNavigationCore.PathFinding.Planning path2.PrevNode = prevNode; path2.PrevDirection = prevDirection; - if (ReverseCheck) MakeDetailData(path2, currentDirection); - else MakeDetailData(path2, ReverseDirection); + //2번paths느최종 목적지 이므로 목적지와 도킹방향 확인해서 결정한다 + if ((path2.Path.Last().DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward) || + (path2.Path.Last().DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward)) + { + MakeDetailData(path2, ReverseDirection); + } + else if ((path2.Path.Last().DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) || + (path2.Path.Last().DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward)) + { + MakeDetailData(path2, currentDirection); + } //3.방향전환을 위환 대체 노드찾기 var tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId, @@ -236,25 +267,30 @@ namespace AGVNavigationCore.PathFinding.Planning if (tempNode == null) return AGVPathResult.CreateFailure("방향 전환을 위한 대체 노드를 찾을 수 없습니다.", 0, 0); + // path1 (시작 → 교차로) var combinedResult = path1; // 교차로 → 대체노드 경로 계산 var pathToTemp = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode.NodeId); + pathToTemp.PrevNode = JunctionInPath; + pathToTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection); if (!pathToTemp.Success) return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0); - if (ReverseCheck) MakeDetailData(pathToTemp, ReverseDirection); else MakeDetailData(pathToTemp, currentDirection); + //교차로찍고 원래방향으로 돌어가야한다. if (pathToTemp.DetailedPath.Count > 1) - pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = ReverseDirection; + pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = currentDirection; // path1 + pathToTemp 합치기 combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp); // 대체노드 → 교차로 경로 계산 (역방향) var pathFromTemp = _basicPathfinder.FindPath(tempNode.NodeId, JunctionInPath.NodeId); + pathFromTemp.PrevNode = JunctionInPath; + pathFromTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection); if (!pathFromTemp.Success) return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0); @@ -264,6 +300,37 @@ namespace AGVNavigationCore.PathFinding.Planning // (path1 + pathToTemp) + pathFromTemp 합치기 combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp); + //대체노드에서 최종 목적지를 다시 확인한다. + if ((currentDirection == AgvDirection.Forward && targetNode.DockDirection != DockingDirection.Forward) || + (currentDirection == AgvDirection.Backward && targetNode.DockDirection != DockingDirection.Backward)) + { + //목적지와 방향이 맞지 않다. 그러므로 대체노드를 추가로 더 찾아야한다. + var tempNode2 = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId, + combinedResult.Path[combinedResult.Path.Count - 2].NodeId, + path2.Path[1].NodeId); + + var pathToTemp2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode2.NodeId); + if (ReverseCheck) MakeDetailData(pathToTemp2, currentDirection); + else MakeDetailData(pathToTemp2, ReverseDirection); + + combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp2); + + //교차로찍고 원래방향으로 돌어가야한다. + if (combinedResult.DetailedPath.Count > 1) + { + if (ReverseCheck) + combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = ReverseDirection; + else + combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = currentDirection; + } + + var pathToTemp3 = _basicPathfinder.FindPath(tempNode2.NodeId, JunctionInPath.NodeId); + if (ReverseCheck) MakeDetailData(pathToTemp3, ReverseDirection); + else MakeDetailData(pathToTemp3, currentDirection); + + combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp3); + } + // (path1 + pathToTemp + pathFromTemp) + path2 합치기 combinedResult = _basicPathfinder.CombineResults(combinedResult, path2); diff --git a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs index b083b58..20f5d69 100644 --- a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs +++ b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs @@ -84,7 +84,6 @@ namespace AGVSimulator.Forms this._simulationStatusLabel = new System.Windows.Forms.Label(); this._pathGroup = new System.Windows.Forms.GroupBox(); this._clearPathButton = new System.Windows.Forms.Button(); - this._startPathButton = new System.Windows.Forms.Button(); this._calculatePathButton = new System.Windows.Forms.Button(); this._targetCalcButton = new System.Windows.Forms.Button(); this._avoidRotationCheckBox = new System.Windows.Forms.CheckBox(); @@ -450,7 +449,6 @@ namespace AGVSimulator.Forms // _pathGroup // this._pathGroup.Controls.Add(this._clearPathButton); - this._pathGroup.Controls.Add(this._startPathButton); this._pathGroup.Controls.Add(this._calculatePathButton); this._pathGroup.Controls.Add(this._targetCalcButton); this._pathGroup.Controls.Add(this._avoidRotationCheckBox); @@ -476,16 +474,6 @@ namespace AGVSimulator.Forms this._clearPathButton.UseVisualStyleBackColor = true; this._clearPathButton.Click += new System.EventHandler(this.OnClearPath_Click); // - // _startPathButton - // - this._startPathButton.Location = new System.Drawing.Point(80, 177); - this._startPathButton.Name = "_startPathButton"; - this._startPathButton.Size = new System.Drawing.Size(65, 25); - this._startPathButton.TabIndex = 5; - this._startPathButton.Text = "경로 시작"; - this._startPathButton.UseVisualStyleBackColor = true; - this._startPathButton.Click += new System.EventHandler(this.OnStartPath_Click); - // // _calculatePathButton // this._calculatePathButton.Location = new System.Drawing.Point(10, 177); @@ -816,7 +804,6 @@ namespace AGVSimulator.Forms private System.Windows.Forms.Label targetNodeLabel; private System.Windows.Forms.ComboBox _targetNodeCombo; private System.Windows.Forms.Button _calculatePathButton; - private System.Windows.Forms.Button _startPathButton; private System.Windows.Forms.Button _clearPathButton; private System.Windows.Forms.Button _targetCalcButton; private System.Windows.Forms.CheckBox _avoidRotationCheckBox; diff --git a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs index 614e513..899c527 100644 --- a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs +++ b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs @@ -342,25 +342,6 @@ namespace AGVSimulator.Forms } } - private void OnStartPath_Click(object sender, EventArgs e) - { - var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; - if (selectedAGV == null) - { - MessageBox.Show("AGV를 선택해주세요.", "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); - return; - } - - if (_simulatorCanvas.CurrentPath == null || !_simulatorCanvas.CurrentPath.Success) - { - MessageBox.Show("먼저 경로를 계산해주세요.", "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); - return; - } - - selectedAGV.StartPath(_simulatorCanvas.CurrentPath, _mapNodes); - _statusLabel.Text = $"{selectedAGV.AgvId} 경로 시작"; - } - private void OnClearPath_Click(object sender, EventArgs e) { _simulatorCanvas.CurrentPath = null; @@ -804,10 +785,6 @@ namespace AGVSimulator.Forms _stopSimulationButton.Enabled = _simulationState.IsRunning; _removeAgvButton.Enabled = _agvListCombo.SelectedItem != null; - _startPathButton.Enabled = _agvListCombo.SelectedItem != null && - _simulatorCanvas.CurrentPath != null && - _simulatorCanvas.CurrentPath.Success; - _calculatePathButton.Enabled = _startNodeCombo.SelectedItem != null && _targetNodeCombo.SelectedItem != null;