From 4a50b4064d4357a669f6db0f0b2e2c9e88bf2c54 Mon Sep 17 00:00:00 2001 From: backuppc Date: Mon, 27 Oct 2025 17:36:29 +0900 Subject: [PATCH] feat: Strengthen magnet direction weighting in path prediction MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 마그넷 방향(Straight/Left/Right) 가중치 강화: - Straight 보너스: 0.3 → 0.5 - 올바른 회전 보너스: 0.25 → 1.0 - 잘못된 회전 페널티: 0.2 → 0.8 이를 통해 GetNextNodeByDirection에서 마그넷 방향이 주요 결정 요소로 작동. 실제 경로 예측 정확도 개선 (Left 마그넷 방향 테스트: 100% 일치). ## 발견된 이슈 (다음 세션) - 기본 내적(dot product) 차이가 큰 경우 (>1.0) 마그넷 보너스(+1.0)로 극복 못함 - 경로 계산 결과는 올바르지만, 검증 단계에서 다르게 평가되는 경우 존재 - 예: N022→N004→N011 경로, 마그넷 Right일 때 - 경로 생성: N011 선택 (내적 -0.1275, 마그넷 Right 일치) - 검증 예측: N031 선택 (내적 0.9646, 마그넷 Right 일치) - 원인: 내적 차이(1.0921) > 마그넷 보너스 효과(1.0) 🤖 Generated with Claude Code Co-Authored-By: Claude --- .../Utils/DirectionalHelper.cs | 57 ++++++++++--------- .../Utils/DockingValidator.cs | 28 ++++----- 2 files changed, 44 insertions(+), 41 deletions(-) diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs index d8cf020..82f2ede 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs @@ -88,17 +88,17 @@ namespace AGVNavigationCore.Utils MapNode bestNode = null; float bestScore = float.MinValue; - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 =========="); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 현재노드: {currentNode.NodeId}({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 이전노드: {prevNode.NodeId}({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 현재방향: {direction}, 이전방향: {prevDirection}, 마그넷방향: {magnetDirection}"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 후보노드 개수: {candidateNodes.Count}"); foreach (var candidate in candidateNodes) @@ -137,15 +137,15 @@ namespace AGVNavigationCore.Utils score = -dotProduct; } - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $"\n [후보] {candidate.NodeId}({candidate.Position.X:F1}, {candidate.Position.Y:F1})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 정규화벡터: ({normalizedToNext.X:F3}, {normalizedToNext.Y:F3})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 내적(dotProduct): {dotProduct:F4}"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 기본점수 ({(direction == prevDirection ? "방향유지" : "방향변경")}): {score:F4}"); // 이전 모터 방향이 제공된 경우: 방향 일관성 보너스 추가 @@ -156,7 +156,7 @@ namespace AGVNavigationCore.Utils prevDirection, dotProduct ); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 모터방향 적용 후: {scoreBeforeMotor:F4} → {score:F4}"); // 마그넷 방향을 고려한 점수 조정 @@ -169,21 +169,21 @@ namespace AGVNavigationCore.Utils currentNode, candidate ); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}"); if (score > bestScore) { bestScore = score; bestNode = candidate; - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" ⭐ 현재 최고점수 선택됨!"); } } - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $"\n 최종선택: {bestNode?.NodeId ?? "null"} (점수: {bestScore:F4})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n"); return bestNode; @@ -256,11 +256,11 @@ namespace AGVNavigationCore.Utils // Straight: 일직선 방향 (높은 내적 보너스) if (magnetDirection == MagnetDirection.Straight) { - const float STRAIGHT_BONUS = 0.3f; + const float STRAIGHT_BONUS = 0.5f; adjustedScore += STRAIGHT_BONUS; - System.Diagnostics.Debug.WriteLine( - $"[DirectionalHelper] 마그넷 Straight 보너스: {baseScore:F3} → {adjustedScore:F3}"); + Console.WriteLine( + $" [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}"); } // Left 또는 Right: 회전 방향 판단을 위해 외적 사용 else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right) @@ -273,24 +273,27 @@ namespace AGVNavigationCore.Utils bool isLeftTurn = crossProduct > 0; bool isRightTurn = crossProduct < 0; + Console.WriteLine( + $" [마그넷 판정] 외적(Cross): {crossProduct:F4}, 좌회전: {isLeftTurn}, 우회전: {isRightTurn}"); + if ((magnetDirection == MagnetDirection.Left && isLeftTurn) || (magnetDirection == MagnetDirection.Right && isRightTurn)) { - // 올바른 회전 방향: 보너스 - const float CORRECT_TURN_BONUS = 0.25f; + // 올바른 회전 방향: 강한 보너스 + const float CORRECT_TURN_BONUS = 1.0f; adjustedScore += CORRECT_TURN_BONUS; - System.Diagnostics.Debug.WriteLine( - $"[DirectionalHelper] 마그넷 {magnetDirection} 회전 보너스: {baseScore:F3} → {adjustedScore:F3}"); + Console.WriteLine( + $" [마그넷 판정] ✅ {magnetDirection} 방향 일치 (보너스 +1.0): {baseScore:F4} → {adjustedScore:F4}"); } else { - // 잘못된 회전 방향: 페널티 - const float WRONG_TURN_PENALTY = 0.2f; + // 잘못된 회전 방향: 강한 페널티 + const float WRONG_TURN_PENALTY = 0.8f; adjustedScore -= WRONG_TURN_PENALTY; - System.Diagnostics.Debug.WriteLine( - $"[DirectionalHelper] 마그넷 {magnetDirection} 회전 페널티 (실제: {(isLeftTurn ? "Left" : "Right")}): {baseScore:F3} → {adjustedScore:F3}"); + Console.WriteLine( + $" [마그넷 판정] ❌ {magnetDirection} 방향 불일치 (실제: {(isLeftTurn ? "Left" : "Right")}, 페널티 -0.8): {baseScore:F4} → {adjustedScore:F4}"); } } diff --git a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs index e44e533..a52154a 100644 --- a/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs +++ b/Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs @@ -69,15 +69,15 @@ namespace AGVNavigationCore.Utils if (prevNode != null) { // DirectionalHelper를 사용하여 예상되는 다음 노드 확인 - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $"\n[ValidateDockingDirection] 경로 검증 단계 {i}:"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 이전→현재→다음: {prevNode.NodeId}({prevNode.RfidId}) → {curNode.NodeId}({curNode.RfidId}) → {nextNode.NodeId}({nextNode.RfidId})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}"); var expectedNextNode = DirectionalHelper.GetNextNodeByDirection( @@ -89,9 +89,9 @@ namespace AGVNavigationCore.Utils mapNodes ); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.NodeId ?? "null"}"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" [실제] DetailedPath 다음 노드: {nextNode.NodeId}"); if (expectedNextNode != null && !expectedNextNode.NodeId.Equals(nextNode.NodeId)) @@ -100,17 +100,17 @@ namespace AGVNavigationCore.Utils $"[DockingValidator] ⚠️ 경로 방향 불일치: " + $"현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " + $"예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]"; - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 이동 벡터:"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})"); - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" 현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})"); - System.Diagnostics.Debug.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}"); + Console.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}"); return DockingValidationResult.CreateInvalid( LastNode.NodeId, LastNode.Type, @@ -122,7 +122,7 @@ namespace AGVNavigationCore.Utils } else { - System.Diagnostics.Debug.WriteLine( + Console.WriteLine( $" ✅ 경로 방향 일치!"); } }