From f4ec63330a79f8d006f56c2879893a990720aa91 Mon Sep 17 00:00:00 2001 From: backuppc Date: Mon, 27 Oct 2025 16:55:13 +0900 Subject: [PATCH] feat: Add magnet direction support to GetNextNodeByDirection function MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Added MagnetDirection parameter to GetNextNodeByDirection() function in DirectionalHelper - Implemented ApplyMagnetDirectionBonus() helper method using vector cross product for turn direction detection - Straight magnet direction: +0.3f bonus for consistent forward direction - Left/Right magnet direction: +0.25f bonus for correct turn, -0.2f penalty for wrong turn direction - Updated DockingValidator to pass actual magnet direction from DetailedPath - Updated AGVPathfinder calls to use MagnetDirection.Straight as default during path planning phase - Includes debug output for scoring decisions with magnet direction analysis ๐Ÿค– Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude --- .../PathFinding/Planning/AGVPathfinder.cs | 5 +- .../Utils/DirectionalHelper.cs | 108 ++++++++++++++++-- .../Utils/DockingValidator.cs | 1 + 3 files changed, 105 insertions(+), 9 deletions(-) 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 );