diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs index d9925be..4da985a 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs @@ -126,8 +126,9 @@ namespace AGVNavigationCore.PathFinding.Planning return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0); //정방향/역방향 이동 시 다음 노드 확인 - var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, _mapNodes); - var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, _mapNodes); + // 경로 계획 단계에서는 마그넷 방향이 미리 알려지지 않으므로 Straight로 기본값 사용 + var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, MagnetDirection.Straight, _mapNodes); + var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes); //2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행) diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs index 4e2d36f..c88967b 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs @@ -3,37 +3,56 @@ using System.Collections.Generic; using System.Drawing; using System.Linq; using AGVNavigationCore.Models; +using AGVNavigationCore.PathFinding.Analysis; +using AGVNavigationCore.PathFinding.Planning; namespace AGVNavigationCore.Utils { /// /// AGV 방향 계산 헬퍼 유틸리티 - /// 현재 위치에서 주어진 모터 방향으로 이동할 때 다음 노드를 계산 - /// 이전 이동 방향을 고려하여 더 정확한 경로 예측 + /// 현재 위치에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 계산 + /// 이전 이동 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측 /// public static class DirectionalHelper { - + private static JunctionAnalyzer _junctionAnalyzer; + /// - /// 현재 노드에서 주어진 방향(Forward/Backward)으로 이동할 때 다음 노드를 반환 (개선된 버전) - /// 이전 모터 방향 정보를 고려하여 더 정확한 경로 예측 + /// JunctionAnalyzer 초기화 (첫 호출 시) + /// + private static void InitializeJunctionAnalyzer(List allNodes) + { + if (_junctionAnalyzer == null && allNodes != null) + { + _junctionAnalyzer = new JunctionAnalyzer(allNodes); + } + } + + /// + /// 현재 노드에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 반환 + /// 이전 모터 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측 /// /// 현재 노드 /// 이전 노드 (진행 방향 기준점) - /// 이동 방향 (Forward 또는 Backward) + /// 이전 구간의 모터 방향 + /// 현재 모터 방향 (Forward 또는 Backward) + /// 현재 마그넷 방향 (Straight/Left/Right) /// 모든 맵 노드 - /// 이전 구간에서의 모터 방향 (선택사항) /// 다음 노드 (또는 null) public static MapNode GetNextNodeByDirection( MapNode currentNode, MapNode prevNode, AgvDirection prevDirection, AgvDirection direction, + MagnetDirection magnetDirection, List allNodes) { if (currentNode == null || prevNode == null || allNodes == null) return null; + // JunctionAnalyzer 초기화 + InitializeJunctionAnalyzer(allNodes); + // 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링 var connectedNodeIds = currentNode.ConnectedNodes; if (connectedNodeIds == null || connectedNodeIds.Count == 0) @@ -113,6 +132,16 @@ namespace AGVNavigationCore.Utils dotProduct ); + // 마그넷 방향을 고려한 점수 조정 + score = ApplyMagnetDirectionBonus( + score, + magnetDirection, + normalizedMovement, + normalizedToNext, + currentNode, + candidate + ); + if (score > bestScore) { bestScore = score; @@ -166,6 +195,71 @@ namespace AGVNavigationCore.Utils return adjustedScore; } + /// + /// 마그넷 방향을 고려한 점수 보정 + /// Straight/Left/Right 마그넷 방향에 따라 후보 노드를 평가 + /// + /// 기본 점수 + /// 마그넷 방향 (Straight/Left/Right) + /// 정규화된 이동 벡터 + /// 정규화된 다음 이동 벡터 + /// 현재 노드 + /// 후보 노드 + /// 조정된 점수 + private static float ApplyMagnetDirectionBonus( + float baseScore, + MagnetDirection magnetDirection, + PointF normalizedMovement, + PointF normalizedToNext, + MapNode currentNode, + MapNode candidate) + { + float adjustedScore = baseScore; + + // Straight: 일직선 방향 (높은 내적 보너스) + if (magnetDirection == MagnetDirection.Straight) + { + const float STRAIGHT_BONUS = 0.3f; + adjustedScore += STRAIGHT_BONUS; + + System.Diagnostics.Debug.WriteLine( + $"[DirectionalHelper] 마그넷 Straight 보너스: {baseScore:F3} → {adjustedScore:F3}"); + } + // Left 또는 Right: 회전 방향 판단을 위해 외적 사용 + else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right) + { + // 2D 외적: movement × toNext = movement.X * toNext.Y - movement.Y * toNext.X + // 양수 = 좌회전, 음수 = 우회전 + float crossProduct = (normalizedMovement.X * normalizedToNext.Y) - + (normalizedMovement.Y * normalizedToNext.X); + + bool isLeftTurn = crossProduct > 0; + bool isRightTurn = crossProduct < 0; + + if ((magnetDirection == MagnetDirection.Left && isLeftTurn) || + (magnetDirection == MagnetDirection.Right && isRightTurn)) + { + // 올바른 회전 방향: 보너스 + const float CORRECT_TURN_BONUS = 0.25f; + adjustedScore += CORRECT_TURN_BONUS; + + System.Diagnostics.Debug.WriteLine( + $"[DirectionalHelper] 마그넷 {magnetDirection} 회전 보너스: {baseScore:F3} → {adjustedScore:F3}"); + } + else + { + // 잘못된 회전 방향: 페널티 + const float WRONG_TURN_PENALTY = 0.2f; + adjustedScore -= WRONG_TURN_PENALTY; + + System.Diagnostics.Debug.WriteLine( + $"[DirectionalHelper] 마그넷 {magnetDirection} 회전 페널티 (실제: {(isLeftTurn ? "Left" : "Right")}): {baseScore:F3} → {adjustedScore:F3}"); + } + } + + return adjustedScore; + } + /// /// 모터 방향을 고려한 다음 노드 선택 (디버깅/분석용) /// diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs index 0829ca3..cf954c0 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs @@ -74,6 +74,7 @@ namespace AGVNavigationCore.Utils prevNode, prevDir, pathResult.DetailedPath[i].MotorDirection, + pathResult.DetailedPath[i].MagnetDirection, mapNodes );