From 880dc526da88b2277cfcb7d9f57703ba9c93fb8e Mon Sep 17 00:00:00 2001 From: backuppc Date: Fri, 9 Jan 2026 17:25:53 +0900 Subject: [PATCH] .. --- .../Controls/UnifiedAGVCanvas.Events.cs | 80 ++- .../Controls/UnifiedAGVCanvas.cs | 6 + .../PathFinding/Planning/AGVPathfinder.cs | 48 +- .../AGVLogic/AGVSimulator/AGVSimulator.csproj | 2 + .../AGVSimulator/Forms/ComboBoxItem.cs | 23 + .../AGVSimulator/Forms/DirectionItem.cs | 24 + .../Forms/SimulatorForm.Designer.cs | 81 ++- .../AGVSimulator/Forms/SimulatorForm.cs | 676 ++++++++++++++---- 길목재계산.md | 15 +- 9 files changed, 749 insertions(+), 206 deletions(-) create mode 100644 Cs_HMI/AGVLogic/AGVSimulator/Forms/ComboBoxItem.cs create mode 100644 Cs_HMI/AGVLogic/AGVSimulator/Forms/DirectionItem.cs diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs index fc1cc7b..acffaf0 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs @@ -421,7 +421,7 @@ namespace AGVNavigationCore.Controls (int)(point.Y + arrowSize * Math.Sin(angle + 4 * Math.PI / 3)) ); - var arrowColor = direction == AgvDirection.Forward ? Color.Blue : Color.Red; + var arrowColor = direction == AgvDirection.Forward ? Color.Blue : Color.Yellow; var arrowBrush = new SolidBrush(arrowColor); // 정삼각형으로 화살표 그리기 (내부 채움) @@ -525,7 +525,15 @@ namespace AGVNavigationCore.Controls var angle = Math.Atan2(nextNode.Position.Y - currentNode.Position.Y, nextNode.Position.X - currentNode.Position.X); - DrawDirectionArrow(g, midPoint, angle, AgvDirection.Forward); + + // 상세 경로 정보가 있으면 해당 방향 사용, 없으면 Forward + AgvDirection arrowDir = AgvDirection.Forward; + if (path.DetailedPath != null && i < path.DetailedPath.Count) + { + arrowDir = path.DetailedPath[i].MotorDirection; + } + + DrawDirectionArrow(g, midPoint, angle, arrowDir); } } @@ -535,57 +543,85 @@ namespace AGVNavigationCore.Controls /// /// 경로에 포함된 교차로(3개 이상의 노드가 연결된 노드)를 파란색으로 강조 표시 /// + /// + /// 경로에 포함된 특정 노드(Gateway 등)를 강조 표시 + /// HighlightNodeId가 설정된 경우 해당 노드만 표시하고, 없으면 기존대로 교차로 표시(또는 표시 안함) + /// 사용자가 "교차로 대신 게이트웨이만 강조"를 원하므로 우선순위 적용 + /// private void HighlightJunctionsInPath(Graphics g, AGVPathResult path) { if (path?.Path == null || _nodes == null || _nodes.Count == 0) return; - const int JUNCTION_CONNECTIONS = 3; // 교차로 판정 기준: 3개 이상의 연결 + // 1. HighlightNodeId가 설정되어 있다면 해당 노드만 강조 + if (!string.IsNullOrEmpty(HighlightNodeId)) + { + var targetNode = path.Path.FirstOrDefault(n => n.Id == HighlightNodeId); + if (targetNode != null) + { + DrawJunctionHighlight(g, targetNode, true); // true = Gateway 강조 색상 사용 + } + // HighlightNodeId가 설정된 경우 다른 교차로는 표시하지 않음 (사용자 요청) + return; + } + + // 2. 설정이 없다면 기존 로직 (교차로 표시) 유지 여부 결정 + // 사용자가 "게이트웨이만 강조해줘"라고 했으므로, 혼란을 피하기 위해 + // HighlightNodeId가 없을 때는 아무것도 표시하지 않거나, 필요한 경우 복구. + // 현재는 사용자 요청에 따라 Gateway 지정이 안된 경우(일반 경로)에는 교차로 강조를 끄는 것이 맞아 보임. + // 하지만 일반 주행시에도 교차로 정보가 필요할 수 있으니 일단 둡니다. + // 단, Gateway 로직을 타는 경우(HighlightNodeId가 Set됨)에는 위에서 return 되므로 OK. + + /* + const int JUNCTION_CONNECTIONS = 3; foreach (var node in path.Path) { if (node == null) continue; - // 교차로 판정: 3개 이상의 노드가 연결된 경우 if (node.ConnectedMapNodes != null && node.ConnectedMapNodes.Count >= JUNCTION_CONNECTIONS) { - DrawJunctionHighlight(g, node); + DrawJunctionHighlight(g, node, false); } } + */ } /// - /// 교차로 노드를 파란색 반투명 배경으로 강조 표시 + /// 노드 강조 표시 /// - private void DrawJunctionHighlight(Graphics g, MapNode junctionNode) + private void DrawJunctionHighlight(Graphics g, MapNode junctionNode, bool isGateway) { if (junctionNode == null) return; - const int JUNCTION_HIGHLIGHT_RADIUS = 25; // 강조 표시 반경 + int radius = isGateway ? 35 : 25; // 게이트웨이는 좀 더 크게 - // 파란색 반투명 브러시로 배경 원 그리기 - using (var highlightBrush = new SolidBrush(Color.FromArgb(80, 70, 130, 200))) // 파란색 (70, 130, 200) 알파 80 - using (var highlightPen = new Pen(Color.FromArgb(150, 100, 150, 220), 2)) // 파란 테두리 + // 색상 결정: Gateway=진한 주황/골드, 일반 교차로=기존 파랑 + Color fillColor = isGateway ? Color.FromArgb(100, 255, 140, 0) : Color.FromArgb(80, 70, 130, 200); + Color penColor = isGateway ? Color.OrangeRed : Color.FromArgb(150, 100, 150, 220); + + using (var highlightBrush = new SolidBrush(fillColor)) + using (var highlightPen = new Pen(penColor, 3)) { g.FillEllipse( highlightBrush, - junctionNode.Position.X - JUNCTION_HIGHLIGHT_RADIUS, - junctionNode.Position.Y - JUNCTION_HIGHLIGHT_RADIUS, - JUNCTION_HIGHLIGHT_RADIUS * 2, - JUNCTION_HIGHLIGHT_RADIUS * 2 + junctionNode.Position.X - radius, + junctionNode.Position.Y - radius, + radius * 2, + radius * 2 ); + // 테두리 점선 효과 (Gateway 인 경우) + if (isGateway) highlightPen.DashStyle = DashStyle.Dot; + g.DrawEllipse( highlightPen, - junctionNode.Position.X - JUNCTION_HIGHLIGHT_RADIUS, - junctionNode.Position.Y - JUNCTION_HIGHLIGHT_RADIUS, - JUNCTION_HIGHLIGHT_RADIUS * 2, - JUNCTION_HIGHLIGHT_RADIUS * 2 + junctionNode.Position.X - radius, + junctionNode.Position.Y - radius, + radius * 2, + radius * 2 ); } - - // 교차로 라벨 추가 - //DrawJunctionLabel(g, junctionNode); } /// diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs index bfc27ef..259e50b 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs @@ -211,6 +211,12 @@ namespace AGVNavigationCore.Controls } } + /// + /// 강조해서 표시할 특정 노드 ID (예: Gateway) + /// 이 값이 설정되면 해당 노드만 강조 표시됩니다. + /// + public string HighlightNodeId { get; set; } + public void RemoveItem(NodeBase item) { if (item is MapImage img) RemoveImage(img); diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index 9556516..0bdba8e 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -116,7 +116,11 @@ namespace AGVNavigationCore.PathFinding.Planning // 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진) return FindPath(startNode, targetNode, startNode, AgvDirection.Forward, AgvDirection.Forward, false); } - + public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode) + { + // 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진) + return _basicPathfinder.FindPathAStar(startNode.Id, targetNode.Id); + } public AGVPathResult FindPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDirection, AgvDirection currentDirection, bool crossignore = false) { @@ -434,6 +438,48 @@ namespace AGVNavigationCore.PathFinding.Planning /// /// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요 /// + + /// + /// 단순 경로 찾기 (복잡한 제약조건/방향전환 로직 없이 A* 결과만 반환) + /// + public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDirection) + { + // 1. 입력 검증 + if (startNode == null || targetNode == null) + return AGVPathResult.CreateFailure("노드 정보 오류", 0, 0); + + // 2. A* 경로 탐색 + var pathResult = _basicPathfinder.FindPathAStar(startNode.Id, targetNode.Id); + pathResult.PrevNode = prevNode; + pathResult.PrevDirection = prevDirection; + + if (!pathResult.Success) + return AGVPathResult.CreateFailure(pathResult.ErrorMessage ?? "경로 없음", 0, 0); + + // 3. 상세 데이터 생성 (단순화: 방향 전환 없이 현재 방향 유지) + if (pathResult.Path != null && pathResult.Path.Count > 0) + { + var detailedPath = new List(); + for (int i = 0; i < pathResult.Path.Count; i++) + { + var node = pathResult.Path[i]; + string nextNodeId = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1].Id : null; + + // 단순화: 입력된 현재 방향을 그대로 사용 + var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNodeId, MagnetDirection.Straight); + + // 속도 설정 + var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id); + if (mapNode != null) nodeInfo.Speed = mapNode.SpeedLimit; + + detailedPath.Add(nodeInfo); + } + pathResult.DetailedPath = detailedPath; + } + + return pathResult; + } + /// /// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요 /// diff --git a/Cs_HMI/AGVLogic/AGVSimulator/AGVSimulator.csproj b/Cs_HMI/AGVLogic/AGVSimulator/AGVSimulator.csproj index 51c9f7b..e3b37f1 100644 --- a/Cs_HMI/AGVLogic/AGVSimulator/AGVSimulator.csproj +++ b/Cs_HMI/AGVLogic/AGVSimulator/AGVSimulator.csproj @@ -45,6 +45,8 @@ + + Form diff --git a/Cs_HMI/AGVLogic/AGVSimulator/Forms/ComboBoxItem.cs b/Cs_HMI/AGVLogic/AGVSimulator/Forms/ComboBoxItem.cs new file mode 100644 index 0000000..18a1d51 --- /dev/null +++ b/Cs_HMI/AGVLogic/AGVSimulator/Forms/ComboBoxItem.cs @@ -0,0 +1,23 @@ +namespace AGVSimulator.Forms +{ + /// + /// 제네릭 콤보박스 아이템 클래스 + /// + /// 값의 타입 + public class ComboBoxItem + { + public T Value { get; } + public string DisplayText { get; } + + public ComboBoxItem(T value, string displayText) + { + Value = value; + DisplayText = displayText; + } + + public override string ToString() + { + return DisplayText; + } + } +} \ No newline at end of file diff --git a/Cs_HMI/AGVLogic/AGVSimulator/Forms/DirectionItem.cs b/Cs_HMI/AGVLogic/AGVSimulator/Forms/DirectionItem.cs new file mode 100644 index 0000000..adb0e09 --- /dev/null +++ b/Cs_HMI/AGVLogic/AGVSimulator/Forms/DirectionItem.cs @@ -0,0 +1,24 @@ +using AGVNavigationCore.Models; + +namespace AGVSimulator.Forms +{ + /// + /// 방향 콤보박스용 아이템 클래스 + /// + public class DirectionItem + { + public AgvDirection Direction { get; } + public string DisplayText { get; } + + public DirectionItem(AgvDirection direction, string displayText) + { + Direction = direction; + DisplayText = displayText; + } + + public override string ToString() + { + return DisplayText; + } + } +} \ No newline at end of file diff --git a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs index 3683837..77f13e6 100644 --- a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs +++ b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs @@ -86,13 +86,15 @@ namespace AGVSimulator.Forms this._coordLabel = new System.Windows.Forms.ToolStripStatusLabel(); this.prb1 = new System.Windows.Forms.ToolStripProgressBar(); this._controlPanel = new System.Windows.Forms.Panel(); + this.groupBox1 = new System.Windows.Forms.GroupBox(); + this.propertyNode = new System.Windows.Forms.PropertyGrid(); this._statusGroup = new System.Windows.Forms.GroupBox(); this._pathLengthLabel = new System.Windows.Forms.Label(); this._agvCountLabel = new System.Windows.Forms.Label(); this._simulationStatusLabel = new System.Windows.Forms.Label(); this._pathGroup = new System.Windows.Forms.GroupBox(); this._clearPathButton = new System.Windows.Forms.Button(); - this._calculatePathButton = new System.Windows.Forms.Button(); + this.btPath1 = new System.Windows.Forms.Button(); this._targetCalcButton = new System.Windows.Forms.Button(); this._avoidRotationCheckBox = new System.Windows.Forms.CheckBox(); this._targetNodeCombo = new System.Windows.Forms.ComboBox(); @@ -118,18 +120,17 @@ namespace AGVSimulator.Forms this._liftDirectionLabel = new System.Windows.Forms.Label(); this._motorDirectionLabel = new System.Windows.Forms.Label(); this.timer1 = new System.Windows.Forms.Timer(this.components); - this.groupBox1 = new System.Windows.Forms.GroupBox(); - this.propertyNode = new System.Windows.Forms.PropertyGrid(); + this.btPath2 = new System.Windows.Forms.Button(); this._menuStrip.SuspendLayout(); this._toolStrip.SuspendLayout(); this._statusStrip.SuspendLayout(); this._controlPanel.SuspendLayout(); + this.groupBox1.SuspendLayout(); this._statusGroup.SuspendLayout(); this._pathGroup.SuspendLayout(); this._agvControlGroup.SuspendLayout(); this._canvasPanel.SuspendLayout(); this._agvInfoPanel.SuspendLayout(); - this.groupBox1.SuspendLayout(); this.SuspendLayout(); // // _menuStrip @@ -468,6 +469,25 @@ namespace AGVSimulator.Forms this._controlPanel.Size = new System.Drawing.Size(233, 640); this._controlPanel.TabIndex = 3; // + // groupBox1 + // + this.groupBox1.Controls.Add(this.propertyNode); + this.groupBox1.Dock = System.Windows.Forms.DockStyle.Fill; + this.groupBox1.Location = new System.Drawing.Point(0, 546); + this.groupBox1.Name = "groupBox1"; + this.groupBox1.Size = new System.Drawing.Size(233, 94); + this.groupBox1.TabIndex = 4; + this.groupBox1.TabStop = false; + this.groupBox1.Text = "노드 정보"; + // + // propertyNode + // + this.propertyNode.Dock = System.Windows.Forms.DockStyle.Fill; + this.propertyNode.Location = new System.Drawing.Point(3, 17); + this.propertyNode.Name = "propertyNode"; + this.propertyNode.Size = new System.Drawing.Size(227, 74); + this.propertyNode.TabIndex = 0; + // // _statusGroup // this._statusGroup.Controls.Add(this._pathLengthLabel); @@ -510,8 +530,9 @@ namespace AGVSimulator.Forms // // _pathGroup // + this._pathGroup.Controls.Add(this.btPath2); this._pathGroup.Controls.Add(this._clearPathButton); - this._pathGroup.Controls.Add(this._calculatePathButton); + this._pathGroup.Controls.Add(this.btPath1); this._pathGroup.Controls.Add(this._targetCalcButton); this._pathGroup.Controls.Add(this._avoidRotationCheckBox); this._pathGroup.Controls.Add(this._targetNodeCombo); @@ -528,23 +549,23 @@ namespace AGVSimulator.Forms // // _clearPathButton // - this._clearPathButton.Location = new System.Drawing.Point(150, 177); + this._clearPathButton.Location = new System.Drawing.Point(121, 177); this._clearPathButton.Name = "_clearPathButton"; - this._clearPathButton.Size = new System.Drawing.Size(70, 25); + this._clearPathButton.Size = new System.Drawing.Size(111, 25); this._clearPathButton.TabIndex = 6; this._clearPathButton.Text = "경로 지우기"; this._clearPathButton.UseVisualStyleBackColor = true; this._clearPathButton.Click += new System.EventHandler(this.OnClearPath_Click); // - // _calculatePathButton + // btPath1 // - this._calculatePathButton.Location = new System.Drawing.Point(10, 177); - this._calculatePathButton.Name = "_calculatePathButton"; - this._calculatePathButton.Size = new System.Drawing.Size(65, 25); - this._calculatePathButton.TabIndex = 4; - this._calculatePathButton.Text = "경로 계산"; - this._calculatePathButton.UseVisualStyleBackColor = true; - this._calculatePathButton.Click += new System.EventHandler(this.OnCalculatePath_Click); + this.btPath1.Location = new System.Drawing.Point(12, 174); + this.btPath1.Name = "btPath1"; + this.btPath1.Size = new System.Drawing.Size(106, 25); + this.btPath1.TabIndex = 4; + this.btPath1.Text = "경로 계산"; + this.btPath1.UseVisualStyleBackColor = true; + this.btPath1.Click += new System.EventHandler(this.OnCalculatePath_Click); // // _targetCalcButton // @@ -793,24 +814,15 @@ namespace AGVSimulator.Forms this.timer1.Interval = 500; this.timer1.Tick += new System.EventHandler(this.timer1_Tick); // - // groupBox1 + // btPath2 // - this.groupBox1.Controls.Add(this.propertyNode); - this.groupBox1.Dock = System.Windows.Forms.DockStyle.Fill; - this.groupBox1.Location = new System.Drawing.Point(0, 546); - this.groupBox1.Name = "groupBox1"; - this.groupBox1.Size = new System.Drawing.Size(233, 94); - this.groupBox1.TabIndex = 4; - this.groupBox1.TabStop = false; - this.groupBox1.Text = "노드 정보"; - // - // propertyNode - // - this.propertyNode.Dock = System.Windows.Forms.DockStyle.Fill; - this.propertyNode.Location = new System.Drawing.Point(3, 17); - this.propertyNode.Name = "propertyNode"; - this.propertyNode.Size = new System.Drawing.Size(227, 74); - this.propertyNode.TabIndex = 0; + this.btPath2.Location = new System.Drawing.Point(12, 201); + this.btPath2.Name = "btPath2"; + this.btPath2.Size = new System.Drawing.Size(106, 25); + this.btPath2.TabIndex = 10; + this.btPath2.Text = "경로 계산2"; + this.btPath2.UseVisualStyleBackColor = true; + this.btPath2.Click += new System.EventHandler(this.btPath2_Click); // // SimulatorForm // @@ -835,6 +847,7 @@ namespace AGVSimulator.Forms this._statusStrip.ResumeLayout(false); this._statusStrip.PerformLayout(); this._controlPanel.ResumeLayout(false); + this.groupBox1.ResumeLayout(false); this._statusGroup.ResumeLayout(false); this._statusGroup.PerformLayout(); this._pathGroup.ResumeLayout(false); @@ -844,7 +857,6 @@ namespace AGVSimulator.Forms this._canvasPanel.ResumeLayout(false); this._agvInfoPanel.ResumeLayout(false); this._agvInfoPanel.PerformLayout(); - this.groupBox1.ResumeLayout(false); this.ResumeLayout(false); this.PerformLayout(); @@ -889,7 +901,7 @@ namespace AGVSimulator.Forms private System.Windows.Forms.ComboBox _startNodeCombo; private System.Windows.Forms.Label targetNodeLabel; private System.Windows.Forms.ComboBox _targetNodeCombo; - private System.Windows.Forms.Button _calculatePathButton; + private System.Windows.Forms.Button btPath1; private System.Windows.Forms.Button _clearPathButton; private System.Windows.Forms.Button _targetCalcButton; private System.Windows.Forms.CheckBox _avoidRotationCheckBox; @@ -925,5 +937,6 @@ namespace AGVSimulator.Forms private System.Windows.Forms.ToolStripMenuItem 맵다른이름으로저장ToolStripMenuItem; private System.Windows.Forms.GroupBox groupBox1; private System.Windows.Forms.PropertyGrid propertyNode; + private System.Windows.Forms.Button btPath2; } } \ No newline at end of file diff --git a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs index cec54cb..aac6a47 100644 --- a/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs +++ b/Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs @@ -157,7 +157,7 @@ namespace AGVSimulator.Forms _config = SimulatorConfig.Load(); // 데이터 초기화 - + _agvList = new List(); _simulationState = new SimulationState(); _currentMapFilePath = string.Empty; @@ -329,7 +329,7 @@ namespace AGVSimulator.Forms UpdateAGVComboBox(); UpdateUI(); - _statusLabel.Text = $"{agvId} 추가됨"; + _statusLabel.Text = $"{agvId} 추가됨"; _simulatorCanvas.FitToNodes(); } @@ -363,100 +363,7 @@ namespace AGVSimulator.Forms UpdateUI(); } - private void OnCalculatePath_Click(object sender, EventArgs e) - { - var rlt = CalcPath(); - if (rlt.result == false) MessageBox.Show(rlt.message, "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); - } - (bool result, string message) CalcPath() - { - // 시작 RFID가 없으면 AGV 현재 위치로 설정 - if (_startNodeCombo.SelectedItem == null || _startNodeCombo.Text == "선택하세요") - { - SetStartNodeFromAGVPosition(); - } - - if (_startNodeCombo.SelectedItem == null || _targetNodeCombo.SelectedItem == null) - { - return (false, "시작 RFID와 목표 RFID를 선택해주세요."); - } - - var startItem = _startNodeCombo.SelectedItem as ComboBoxItem; - var targetItem = _targetNodeCombo.SelectedItem as ComboBoxItem; - var startNode = startItem?.Value; - var targetNode = targetItem?.Value; - - if (startNode == null || targetNode == null) - { - return (false, "선택한 노드 정보가 올바르지 않습니다."); - } - - - if (_advancedPathfinder == null) - { - _advancedPathfinder = new AGVPathfinder(_simulatorCanvas.Nodes); - } - - // 현재 AGV 방향 가져오기 - var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; - if (selectedAGV == null) - { - return (false, "Virtual AGV 가 없습니다"); - } - var currentDirection = selectedAGV.CurrentDirection; - - // AGV의 이전 위치에서 가장 가까운 노드 찾기 - var prevNode = selectedAGV.PrevNode; - var prevDir = selectedAGV.PrevDirection; - - // 고급 경로 계획 사용 (노드 객체 직접 전달) - var advancedResult = _advancedPathfinder.FindPath(startNode, targetNode, prevNode, prevDir, currentDirection); - - _simulatorCanvas.FitToNodes(); - if (advancedResult.Success) - { - // 도킹 검증이 없는 경우 추가 검증 수행 - if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired) - { - advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _simulatorCanvas.Nodes); - } - - //마지막대상이 버퍼라면 시퀀스처리를 해야한다 - if(targetNode.StationType == StationType.Buffer) - { - var lastDetailPath = advancedResult.DetailedPath.Last(); - if(lastDetailPath.NodeId == targetNode.Id) //마지막노드 재확인 - { - //버퍼에 도킹할때에는 마지막 노드에서 멈추고 시퀀스를 적용해야한다 - advancedResult.DetailedPath = advancedResult.DetailedPath.Take(advancedResult.DetailedPath.Count - 1).ToList(); - Console.WriteLine("최종위치가 버퍼이므로 마지막 RFID에서 멈추도록 합니다"); - } - } - - - _simulatorCanvas.CurrentPath = advancedResult; - _pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}"; - _statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)"; - - // 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요) - selectedAGV.SetPath(advancedResult); - - // 도킹 검증 결과 확인 및 UI 표시 - CheckAndDisplayDockingValidation(advancedResult); - - // 고급 경로 디버깅 정보 표시 - UpdateAdvancedPathDebugInfo(advancedResult); - return (true, string.Empty); - } - else - { - // 경로 실패시 디버깅 정보 초기화 - _pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}"; - - return (false, $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}"); - } - } private void OnClearPath_Click(object sender, EventArgs e) { @@ -842,12 +749,12 @@ namespace AGVSimulator.Forms // RFID 값 확인 var rfidId = _rfidTextBox.Text.Trim(); - if (ushort.TryParse(rfidId,out ushort rfidvalue)==false) + if (ushort.TryParse(rfidId, out ushort rfidvalue) == false) { MessageBox.Show("RFID 값을 입력해주세요.", "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); return; } - + // 선택된 방향 확인 var selectedDirectionItem = _directionCombo.SelectedItem as DirectionItem; var selectedDirection = selectedDirectionItem?.Direction ?? AgvDirection.Forward; @@ -870,7 +777,7 @@ namespace AGVSimulator.Forms return; } - + //이전위치와 동일한지 체크한다. if (selectedAGV.CurrentNodeId == targetNode.Id && selectedAGV.CurrentDirection == selectedDirection) { @@ -1102,7 +1009,7 @@ namespace AGVSimulator.Forms _stopSimulationButton.Enabled = _simulationState.IsRunning; _removeAgvButton.Enabled = _agvListCombo.SelectedItem != null; - _calculatePathButton.Enabled = _startNodeCombo.SelectedItem != null && + btPath1.Enabled = _startNodeCombo.SelectedItem != null && _targetNodeCombo.SelectedItem != null; // RFID 위치 설정 관련 @@ -1670,7 +1577,7 @@ namespace AGVSimulator.Forms MotorDirection = directionName, CurrentPosition = GetNodeDisplayName(currentNode), TargetPosition = GetNodeDisplayName(targetNode), - DockingPosition = targetNode.StationType == StationType.Charger ? "충전기" : "장비" + DockingPosition = targetNode.StationType == StationType.Charger ? "충전기" : "장비" }; if (calcResult.result) @@ -1914,7 +1821,7 @@ namespace AGVSimulator.Forms { if (_agvList == null || _agvList.Count == 0) { - // MessageBox.Show("AGV가 없습니다.", "예측 오류", MessageBoxButtons.OK, MessageBoxIcon.Warning); + // MessageBox.Show("AGV가 없습니다.", "예측 오류", MessageBoxButtons.OK, MessageBoxIcon.Warning); return; } @@ -1923,7 +1830,7 @@ namespace AGVSimulator.Forms var command = agv.Predict(); this.lbPredict.Text = $"Motor:{command.Motor},Magnet:{command.Magnet},Speed:{command.Speed} : {command.Message}"; } - + private void btMakeMap_Click(object sender, EventArgs e) { @@ -1997,7 +1904,7 @@ namespace AGVSimulator.Forms } - + /// /// 맵 데이터를 파일에 저장 (MapLoader 공통 저장 로직 사용) /// @@ -2122,7 +2029,8 @@ namespace AGVSimulator.Forms _portCombo = new ComboBox(); _portCombo.Width = 100; _portCombo.DropDownStyle = ComboBoxStyle.DropDownList; - _portCombo.DropDown += (s, e) => { + _portCombo.DropDown += (s, e) => + { _portCombo.Items.Clear(); _portCombo.Items.AddRange(SerialPort.GetPortNames()); }; @@ -2209,7 +2117,7 @@ namespace AGVSimulator.Forms string buffer = _recvBuffer.ToString(); int stxIndex = buffer.IndexOf((char)0x02); - + while (stxIndex >= 0) { int etxIndex = buffer.IndexOf((char)0x03, stxIndex); @@ -2227,7 +2135,7 @@ namespace AGVSimulator.Forms break; // ETX 아직 안옴 } } - + _recvBuffer.Clear(); _recvBuffer.Append(buffer); } @@ -2301,7 +2209,7 @@ namespace AGVSimulator.Forms // Packet: CMD(3) + DATA(...) + Checksum(2) // But here packet is substring between STX and ETX. // Example: STS...Checksum - + if (packet.Length < 3) return; string cmd = packet.Substring(0, 3); string data = ""; @@ -2312,8 +2220,9 @@ namespace AGVSimulator.Forms // AGV 제어 (첫 번째 AGV 대상) var agv = _agvList.FirstOrDefault(); - - this.Invoke(new Action(() => { + + this.Invoke(new Action(() => + { switch (cmd) { case "CRN": // 기동명령 @@ -2381,7 +2290,7 @@ namespace AGVSimulator.Forms else SetAGV(esystemflag1.Battery_charging, false); } break; - + case "ACK": // Log ACK break; @@ -2405,7 +2314,7 @@ namespace AGVSimulator.Forms barr.Add((byte)'*'); barr.Add((byte)'*'); barr.Add(0x03); - + try { _emulatorPort.Write(barr.ToArray(), 0, barr.Count); @@ -2418,7 +2327,7 @@ namespace AGVSimulator.Forms if (_emulatorPort == null || !_emulatorPort.IsOpen) return; var tagnostr = tagno.ToString("000000"); - + var barr = new List(); barr.Add(0x02); barr.Add((byte)'T'); @@ -2428,7 +2337,7 @@ namespace AGVSimulator.Forms barr.Add((byte)'*'); barr.Add((byte)'*'); barr.Add(0x03); - + try { _emulatorPort.Write(barr.ToArray(), 0, barr.Count); @@ -2441,7 +2350,7 @@ namespace AGVSimulator.Forms if (_emulatorPort == null || !_emulatorPort.IsOpen) return; var agv = _agvList.FirstOrDefault(); - + // Sync state from VirtualAGV if (agv != null) { @@ -2451,7 +2360,7 @@ namespace AGVSimulator.Forms // STS Packet Construction // STS(3) + Volt(3) + Sys0(4) + Sys1(4) + Err(4) + Spd(1) + Bunki(1) + Dir(1) + Sensor(1) + Signal(2) + Checksum(2) - + // Default buffer var sample = "02 53 54 53 32 35 38 46 46 46 46 34 30 30 30 30 30 30 30 4C 53 46 30 30 30 30 30 30 33 41 03"; var barr = sample.Split(' ').Select(t => Convert.ToByte(t, 16)).ToArray(); @@ -2498,55 +2407,528 @@ namespace AGVSimulator.Forms { int sum = 0; foreach (char c in data) sum += c; - + // 16진수 변환 후 뒤 2자리 string hex = sum.ToString("X"); if (hex.Length >= 2) return hex.Substring(hex.Length - 2); return hex.PadLeft(2, '0'); } + (bool result, string message) CalcPath() + { + // 시작 RFID가 없으면 AGV 현재 위치로 설정 + if (_startNodeCombo.SelectedItem == null || _startNodeCombo.Text == "선택하세요") + { + SetStartNodeFromAGVPosition(); + } + + if (_startNodeCombo.SelectedItem == null || _targetNodeCombo.SelectedItem == null) + { + return (false, "시작 RFID와 목표 RFID를 선택해주세요."); + } + + var startItem = _startNodeCombo.SelectedItem as ComboBoxItem; + var targetItem = _targetNodeCombo.SelectedItem as ComboBoxItem; + var startNode = startItem?.Value; + var targetNode = targetItem?.Value; + + if (startNode == null || targetNode == null) + { + return (false, "선택한 노드 정보가 올바르지 않습니다."); + } + + + if (_advancedPathfinder == null) + { + _advancedPathfinder = new AGVPathfinder(_simulatorCanvas.Nodes); + } + + // 현재 AGV 방향 가져오기 + var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; + if (selectedAGV == null) + { + return (false, "Virtual AGV 가 없습니다"); + } + var currentDirection = selectedAGV.CurrentDirection; + + // AGV의 이전 위치에서 가장 가까운 노드 찾기 + var prevNode = selectedAGV.PrevNode; + var prevDir = selectedAGV.PrevDirection; + + // 고급 경로 계획 사용 (노드 객체 직접 전달) + var advancedResult = _advancedPathfinder.FindPath(startNode, targetNode, prevNode, prevDir, currentDirection); + + _simulatorCanvas.FitToNodes(); + if (advancedResult.Success) + { + // 도킹 검증이 없는 경우 추가 검증 수행 + if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired) + { + advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _simulatorCanvas.Nodes); + } + + //마지막대상이 버퍼라면 시퀀스처리를 해야한다 + if (targetNode.StationType == StationType.Buffer) + { + var lastDetailPath = advancedResult.DetailedPath.Last(); + if (lastDetailPath.NodeId == targetNode.Id) //마지막노드 재확인 + { + //버퍼에 도킹할때에는 마지막 노드에서 멈추고 시퀀스를 적용해야한다 + advancedResult.DetailedPath = advancedResult.DetailedPath.Take(advancedResult.DetailedPath.Count - 1).ToList(); + Console.WriteLine("최종위치가 버퍼이므로 마지막 RFID에서 멈추도록 합니다"); + } + } + + + _simulatorCanvas.CurrentPath = advancedResult; + _pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}"; + _statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)"; + + // 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요) + selectedAGV.SetPath(advancedResult); + + // 도킹 검증 결과 확인 및 UI 표시 + CheckAndDisplayDockingValidation(advancedResult); + + // 고급 경로 디버깅 정보 표시 + UpdateAdvancedPathDebugInfo(advancedResult); + return (true, string.Empty); + } + else + { + // 경로 실패시 디버깅 정보 초기화 + _pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}"; + + return (false, $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}"); + } + } #endregion - } - - /// - /// 방향 콤보박스용 아이템 클래스 - /// - public class DirectionItem - { - public AgvDirection Direction { get; } - public string DisplayText { get; } - - public DirectionItem(AgvDirection direction, string displayText) + private void btPath2_Click(object sender, EventArgs e) { - Direction = direction; - DisplayText = displayText; + // 경로계산2 (Gateway Logic) + var rlt = CalcPathGateway(); + if (rlt.result == false) MessageBox.Show(rlt.message, "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); } - public override string ToString() + /// + /// 길목(Gateway) 기반 경로 계산 + /// + private (bool result, string message) CalcPathGateway() { - return DisplayText; - } - } + // 1. 기본 정보 획득 + if (_startNodeCombo.SelectedItem == null || _startNodeCombo.Text == "선택하세요") SetStartNodeFromAGVPosition(); + if (_startNodeCombo.SelectedItem == null || _targetNodeCombo.SelectedItem == null) return (false, "시작/목표 노드 선택 필요"); - /// - /// 제네릭 콤보박스 아이템 클래스 - /// - /// 값의 타입 - public class ComboBoxItem - { - public T Value { get; } - public string DisplayText { get; } + var startNode = (_startNodeCombo.SelectedItem as ComboBoxItem)?.Value; + var targetNode = (_targetNodeCombo.SelectedItem as ComboBoxItem)?.Value; + if (startNode == null || targetNode == null) return (false, "노드 정보 오류"); - public ComboBoxItem(T value, string displayText) - { - Value = value; - DisplayText = displayText; + if (_advancedPathfinder == null) _advancedPathfinder = new AGVPathfinder(_simulatorCanvas.Nodes); + + var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV; + if (selectedAGV == null) return (false, "Virtual AGV 없음"); + + var currentAgvDir = selectedAGV.CurrentDirection; + var prevNode = selectedAGV.PrevNode; + var prevDir = selectedAGV.PrevDirection; + + // 2. Buffer-to-Buffer 예외 처리 + + var node05 = FindNode(5); //05~31사이의 노드는 모두 버퍼이다. + var node31 = FindNode(31); + if (node05 == null || node31 == null) return (false, "버퍼구간 노드가 없습니다(05~31)"); + + var rlt = _advancedPathfinder.FindPathAStar(node05, node31); + if (rlt.Success == false) return (false, "버퍼구간 노드경로 확인 실패(05~31)"); + + //버퍼구간내에 시작과 종료가 모두 포함되어있다 + if (rlt.Path.Contains(startNode) && rlt.Path.Contains(targetNode)) + { + return CalcPathBufferToBuffer(startNode, targetNode, prevNode, prevDir, currentAgvDir, selectedAGV); + } + + + // 3. 목적지별 Gateway 및 진입 조건 확인 + var gatewayNode = GetGatewayNode(targetNode); + if (gatewayNode == null) + { + //게이트웨이가 없는 경우라면 목적지가 도킹포인트가 아니므로, a*알골리즘으로 진행 방향만 맟춰서 이동한다 + var simplePath = _advancedPathfinder.FindBasicPath(startNode, targetNode, prevNode, prevDir); + if (simplePath.Success) + { + _simulatorCanvas.HighlightNodeId = null; // 일반 경로는 강조 없음 + ApplyResultToSimulator(simplePath, selectedAGV); + UpdateAdvancedPathDebugInfo(simplePath); + return (true, "일반 이동 경로(Gateway 없음)"); + } + return (false, $"일반 이동 경로 실패: {simplePath.ErrorMessage}"); + } + + // Gateway Node 찾음 + _simulatorCanvas.HighlightNodeId = gatewayNode.Id; // Gateway 강조 설정 + + // 4. Start -> Gateway 경로 계산 (A*) + var pathToGateway = _advancedPathfinder.FindPath(startNode, gatewayNode, prevNode, prevDir, currentAgvDir); + + if (!pathToGateway.Success) return (false, $"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.ErrorMessage}"); + + // 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함) + var arrivalOrientation = pathToGateway.DetailedPath.Last().MotorDirection; + AGVPathResult finalPath = pathToGateway; + + var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, pathToGateway.Path.Last(), arrivalOrientation); + + if (!gatewayPathResult.Success) return (false, $"{gatewayPathResult.ErrorMessage}"); + + finalPath = CombinePaths(finalPath, gatewayPathResult); + + + // 8. 적용 + ApplyResultToSimulator(finalPath, selectedAGV); + UpdateAdvancedPathDebugInfo(finalPath); + return (true, "성공"); } - public override string ToString() + + + + /// + /// 노드를 찾기위한 함수 + /// + /// + /// + private MapNode FindNode(ushort rfid) { - return DisplayText; + return _simulatorCanvas.Nodes.FirstOrDefault(n => n.RfidId == rfid); } + /// + /// 노드를 찾기위한 함수 + /// + /// + /// + private MapNode FindNode(string nodeid) + { + return _simulatorCanvas.Nodes.FirstOrDefault(n => n.Id == nodeid); + } + + + + /// + /// Gateway 도착 후, Target까지의 경로(회차 및 최종진입 포함)를 계산합니다. + /// + /// 게이트웨이 노드값 + /// 최종 목표값 + /// 게이트웨이 진입 전 노드 + /// 게이트웨이 진입 전 모터방향 + /// + private AGVPathResult GetPathFromGateway(MapNode gatewayNode, MapNode targetNode, MapNode GTprevNode, AgvDirection GTprevDirection) + { + AGVPathResult resultPath = null; + + MapNode currentNode = gatewayNode; + MapNode currentPrev = GTprevNode; // Gateway 바로 이전 노드 (방향 계산용) + AgvDirection currentDir = GTprevDirection; + + //게이트웨이 진입 한 방향을 보고. 목적지와 도킹방향이 일치하는지 결정한다. + var deltaX = gatewayNode.Position.X - GTprevNode.Position.X; + + var isMonitorLeft = false; + bool requiredDir = false; + + switch (targetNode.StationType) + { + case StationType.Buffer: + + //버퍼는 게이트웨이가 6번이고 좌/우로 판단한다 + if (deltaX > 0) //게이트웨이가 더 오른쪽에있으니 좌->우 이동을 한경우이다. 이떄 모터방향이 후진이라면 모니터는 왼쪽이고, 반대는 오른쪽이다 + { + isMonitorLeft = GTprevDirection == AgvDirection.Backward; + } + else + { + isMonitorLeft = GTprevDirection == AgvDirection.Forward; + } + + //버퍼는 모니터가 왼쪽에 있으면 안된다. + List turnPatterns = new List(); + AGVPathResult rlt1 = new AGVPathResult(); + rlt1.Success = true; + + //목적지까지 바로 계산한다 + var pathtarget = _advancedPathfinder.FindBasicPath(gatewayNode, targetNode, GTprevNode, AgvDirection.Backward); + + if (isMonitorLeft) + { + //턴을 하는 + turnPatterns = GetTurnaroundPattern(gatewayNode, targetNode); + if (turnPatterns == null || turnPatterns.Any() == false) return new AGVPathResult { Success = false, ErrorMessage = $"회차 패턴 없음: Dir {currentDir}" }; + foreach (var item in turnPatterns) + { + var rfidvalue = ushort.Parse(item.Substring(0, 4)); + var node = _simulatorCanvas.Nodes.FirstOrDefault(t => t.RfidId == rfidvalue); + + //경로노드추가 + rlt1.Path.Add(node); + + //Detail 정보도 추가한다. + AgvDirection nodedir = item.Substring(4, 1) == "F" ? AgvDirection.Forward : AgvDirection.Backward; + MagnetDirection magnet = MagnetDirection.Straight; + var magchar = item.Substring(5, 1); + if (magchar == "L") magnet = MagnetDirection.Left; + else if (magchar == "R") magnet = MagnetDirection.Right; + rlt1.DetailedPath.Add(new NodeMotorInfo(rlt1.DetailedPath.Count, node.Id, node.RfidId, nodedir, null, magnet) + { + Speed = SpeedLevel.L, + }); + } + + //시작위치가 겹치므로 제거해줘야하낟. + if (pathtarget.DetailedPath.First().NodeId != rlt1.DetailedPath.Last().NodeId || + pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection ) + { + new AGVPathResult { Success = false, ErrorMessage = $"게이트웨이 턴 마지막 주소와, 이 후 주소의 시작 노드ID가 일치하지 않습니다" }; + } + + pathtarget.Path.RemoveAt(0); + pathtarget.DetailedPath.RemoveAt(0); + } + + var lastpath = CombinePaths(rlt1, pathtarget); + return lastpath; + + default: + throw new Exception("ASdf"); + } + + return null; + } + + /// + /// 지정한 노드의 게이트웨이를 반환합니다. + /// 도킹노드가 아닐경우 NULL이 반환됩니다. + /// + /// + /// + private MapNode GetGatewayNode(MapNode node) + { + var rfid = 0; + if (node.RfidId == 1) rfid = 10; + if (node.RfidId == 15) rfid = 9; + if (node.RfidId == 11) rfid = 6; + if (node.RfidId == 8) rfid = 13; + if (node.RfidId == 19) rfid = 13; + if (node.StationType == StationType.Buffer) rfid = 6; + if (rfid == 0) return null; + return this._simulatorCanvas.Nodes.FirstOrDefault(t => t.RfidId == rfid); + } + + private AgvDirection GetRequiredGatewayDirection(string gatewayLogId) + { + switch (gatewayLogId) + { + case "0010": return AgvDirection.Backward; + case "0009": return AgvDirection.Forward; + case "0006": return AgvDirection.Backward; + case "0013": return AgvDirection.Backward; + default: return AgvDirection.Forward; + } + } + + /// + /// 대상노드와 게이트웨이노드를 가지고 턴 노드값을 반환합니다 + /// (이 값은 하드코딩 되어있음) + /// + /// + /// + /// + private List GetTurnaroundPattern(MapNode gatewayNode, MapNode targetNode) + { + switch (gatewayNode.RfidId) + { + case 6: + //버퍼와 11을 다르게하낟. + if (targetNode.StationType == StationType.Buffer) + { + return new List { "0006BL", "0007FS", "0013BL", "0006BL" }; + } + else + { + return new List { "0006BL", "0007FS", "0013BL", "0006BS" }; + } + case 9: return new List { "0009FL", "0010BS", "0007FL", "0009FS" }; + case 10: return new List { "0010BR", "0009FR", "0007BS", "0010BS" }; + case 13: return new List { "0013BL", "0006FL", "0007BS", "0013BS" }; + default: return null; + } + } + + private (bool result, string message) CalcPathBufferToBuffer(MapNode start, MapNode target, MapNode prev, AgvDirection prevDir, AgvDirection currentDir, VirtualAGV agv) + { + // Monitor Side 판단 로직 + // 현재 AGV의 물리적 방향(Monitor Side)이 "Right" 상태여야 버퍼 진입이 용이하다고 가정. + // Monitor Left 상태(부적절한 방향)라면 Gateway로 탈출해야 함. + + // 이동 벡터 X 변화량 + // prev가 없거나 start와 같으면 이동 방향을 알 수 없음 -> 이 경우 보수적으로 기존 로직(Backward면 탈출) 따름 + int deltaX = 0; + if (prev == null) return (false, "이전 노드 정보가 없습니다"); + else deltaX = start.Position.X - prev.Position.X; + + bool isMonitorLeft = false; + + if (deltaX > 0) // 오른쪽(Forward)으로 이동해 옴 (예: 2 -> 4) + { + // 이동방향(Right) + 전진(F) => Monitor Right (Good) + // 이동방향(Right) + 후진(B) => Monitor Left (Bad) + isMonitorLeft = (prevDir == AgvDirection.Backward); + } + else if (deltaX < 0) // 왼쪽(Reverse)으로 이동해 옴 (예: 4 -> 2) + { + // 이동방향(Left) + 전진(F) => Monitor Left (Bad) + // 이동방향(Left) + 후진(B) => Monitor Right (Good) + isMonitorLeft = (prevDir == AgvDirection.Forward); + } + else // 제자리 또는 수직 이동 + { + // 판단 불가 시 기존 로직(Backward면 Gateway) 유지 + return (false, "이전 노드와의 방향을 알 수 없습니다"); + } + + if (isMonitorLeft) + { + // Monitor Left 상태 (방향 불일치) -> Gateway로 탈출 + var GateWayNode = FindNode(6); + var escPath = _advancedPathfinder.FindBasicPath(start, GateWayNode, prev, prevDir); + if (!escPath.Success) return (false, "버퍼 탈출 경로 실패"); + + var lastNode = escPath.Path.Last(); // Should be GW6 + var lastPrev = escPath.Path[escPath.Path.Count - 2]; + var lastDir = escPath.DetailedPath.Last().MotorDirection; + + // Gateway 도착 후, Target(Buffer)으로 이동 + // 여기서부터는 "Monitor Right" 로직(즉, 적절한 방향 진입)을 적용합니다. + // 6번에서 Target이 왼쪽이면 Direct(Backward), 오른쪽이면 Overshoot(Forward->Backward) + + + bool isTargetLeftOfGW = target.Position.X < GateWayNode.Position.X; + AGVPathResult entryPath = null; + + //게이트웨이까지 후진으로 이동했다면 모니터방향이 오른쪽이다 => 방향전환필요 + var gateToTarget = GetPathFromGateway(GateWayNode, target, lastPrev, lastDir); + + escPath.Path.RemoveAt(escPath.Path.Count-1); + escPath.DetailedPath.RemoveAt(escPath.DetailedPath.Count - 1); + + + var final = CombinePaths(escPath, gateToTarget); + ApplyResultToSimulator(final, agv); + UpdateAdvancedPathDebugInfo(final); + return (true, "버퍼 재진입(탈출후)"); + } + else + { + // Monitor Right 상태 (방향 일치) -> 직접 진입 또는 Overshoot + bool isTargetLeft = target.Position.X < start.Position.X; + + if (isTargetLeft) + { + var directPath = _advancedPathfinder.FindBasicPath(start, target, prev, AgvDirection.Backward); + ApplyResultToSimulator(directPath, agv); + UpdateAdvancedPathDebugInfo(directPath); + return (true, "버퍼 좌측이동(직접진입)"); + } + else + { + //대상이 나보다 우측에 있으니 RFID값이 읽어지는 위치까지 이동후에 다시 반대방향으로 마크스탑 해야 함 + //그냥 대상 노드까지 이동을 한다.. RFID값이 실제 멈추는 위치 이전에 있으니 그곳까지 이동하고 역방향 마크스탑하면 동일한 위치이다. 위 식은 그 이전노드까지 확실하게 이동하는 코드이다 + //var overshootNode = target.ConnectedMapNodes.OrderByDescending(n => n.Position.X).FirstOrDefault(); + //if (overshootNode == null || overshootNode.Position.X <= target.Position.X) + // return (false, "Overshoot 공간 부족"); + + var path1 = _advancedPathfinder.FindBasicPath(start, target, prev, AgvDirection.Forward); + if (path1.Path.Count < 2) return (false, "Overshoot 경로 생성 실패"); + + //목표에서 방향바꿔 마크스탑을 해야한다 + path1.Path.Add(path1.Path.Last()); + + //디테일은 방향바꿔서 추가 필요(속도는 저속처리해준다) + var lastDefailt = path1.DetailedPath.Last(); + path1.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, lastDefailt.NodeId, lastDefailt.RfidId, AgvDirection.Backward) + { + Speed = SpeedLevel.L, + IsPass = false, + }); + + //var p1Last = path1.Path.Last(); + //var p1Prev = path1.Path[path1.Path.Count > 1 ? path1.Path.Count - 2 : 0]; // Safety check + //var p1Dir = path1.DetailedPath.Last().MotorDirection; + + //var path2 = _advancedPathfinder.FindPath(start, target, p1Last, p1Dir, AgvDirection.Backward); + + //if (path2.Success && path2.DetailedPath.Last().NodeId == target.Id) + // path2.DetailedPath = path2.DetailedPath.Take(path2.DetailedPath.Count - 1).ToList(); + + //var final = CombinePaths(path1, path2); + ApplyResultToSimulator(path1, agv); + UpdateAdvancedPathDebugInfo(path1); + return (true, "버퍼 우측(Overshoot)"); + } + } + } + + /// + /// p1+p2 + /// + /// + /// + /// + private AGVPathResult CombinePaths(AGVPathResult p1, AGVPathResult p2) + { + var res = new AGVPathResult(); + res.Success = true; + + foreach(var item in p1.Path) + { + res.Path.Add(item); + } + foreach(var item in p2.Path) + { + res.Path.Add(item); + } + + foreach (var item in p1.DetailedPath) + { + var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq); + item.seq = maxseq + 1; + res.DetailedPath.Add(item); + } + foreach (var item in p2.DetailedPath) + { + var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq); + item.seq = maxseq + 1; + res.DetailedPath.Add(item); + } + + return res; + } + + private void ApplyResultToSimulator(AGVPathResult result, VirtualAGV agv) + { + _simulatorCanvas.CurrentPath = result; + _pathLengthLabel.Text = $"Gateway경로: {result.TotalDistance:F1}"; + agv.SetPath(result); + //_simulatorCanvas.CheckAndDisplayDockingValidation(result); // Optional/Needs access + _simulatorCanvas.FitToNodes(); + } + + private void OnCalculatePath_Click(object sender, EventArgs e) + { + var rlt = CalcPath(); + if (rlt.result == false) MessageBox.Show(rlt.message, "알림", MessageBoxButtons.OK, MessageBoxIcon.Information); + } + + + } } \ No newline at end of file diff --git a/길목재계산.md b/길목재계산.md index 7b5cd38..24f595b 100644 --- a/길목재계산.md +++ b/길목재계산.md @@ -6,8 +6,9 @@ 0011 06 역방향 : 0006(BS) 0011(BS), 정방향(회전필요) : [ 0006(BL) 0007(FS) 00013(BL) 0006(BS) ] - 동일 버퍼 06 역방향 : 0006(BL) 0005(BS) ~ 나머지 0031까지는 BS상태로 계속 이동, 정방향(회전필요) : [ 0006(BL) 0007(FS) 0013(BL) 0006(BL) ] - 동일 0008 13 역방향 : 0013(BS) 0019(BS) 0008(BS), 정방향(회전필요) : [ 0013(BL) 0006(FL) 0007(BS) 0013(BS) ] - 동일 +0019 13 역방향 : 0013(BS) 0019(BS), 정방향(회전필요) : [ 0013(BL) 0006(FL) 0007(BS) 0013(BS) ] - 동일 -나머지모든 경유는 각 경유지로 해당 방향으로 그대로 길을 찾는다 +나머지 모든 경유는 각 경유지로 해당 방향으로 그대로 길을 찾는다 목적지를 보고 목적지의 경유지까지 경로로를 계산한다 (A*) 동일노드가 목적지 일때에는 @@ -15,5 +16,15 @@ 도킹방향이 맞고 마크센서가 OFF되어있다면 반대방향으로 한번 이동하고, 다시 역방향 이동하면서 MARK STOP 처리한다. 도킹방향이 맞지 않는 경우 일반 노드 검색처럼 경유지 처리한다 -버퍼에서 버퍼로 이동할때에만 추가 코드를 적용한다 +버퍼에서 버퍼로 이동하는 경우에는 별도 로직으로 처리한다 +모니터가 좌측에 있는 경우라면 도킹방향이 맞지 않으므로 0006까지 이동을 완료 한 후 기존 로직을 적용 +모니터가 우측에 있는 방향이라면 도킹방향이 맞는 경우이다 + 목표가 현재 기준 좌측이라면? + BACK으로 이동하면 좌측이동이므로 목적지 노드까지 A*로 계산하여 이동 후 MARK STOP 한다 + 목표가 현재 기준 우측이라면? + 위치 정밀도를 높이기 위해서 목표 지점을 벗어난 후 다시 좌측으로 마크스탑한다 + A* -> Back -> MARKSTOP + +* 인식된 위치가 전체 경로상에 존재하지 않는 노드라면 모든 경로계산을 다시 계산 한다 +* 경로상 노드에 포함되어있다 해도 진입방향등을 고려하여 일치하지 않으면 경로 이탈로 간주하고 다시 계산 한다 \ No newline at end of file