Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: Add motor direction parameter to magnet direction calculation in pathfinding

Browse Source 
- Fixed critical issue in ConvertToDetailedPath where motor direction was not passed to GetRequiredMagnetDirection
- Motor direction is essential for backward movement as Left/Right directions must be inverted
- Modified AGVPathfinder.cs line 280 to pass currentDirection parameter
- Ensures backward motor direction properly inverts magnet sensor directions

feat: Add waypoint support to pathfinding system

- Added FindPath overload with params string[] waypointNodeIds in AStarPathfinder
- Supports sequential traversal through multiple intermediate nodes
- Validates waypoints and prevents duplicates in sequence
- Returns combined path result with aggregated metrics

feat: Implement path result merging with DetailedPath preservation

- Added CombineResults method in AStarPathfinder for intelligent path merging
- Automatically deduplicates nodes when last of previous path equals first of current
- Preserves DetailedPath information including motor and magnet directions
- Essential for multi-segment path operations

feat: Integrate magnet direction with motor direction awareness

- Modified JunctionAnalyzer.GetRequiredMagnetDirection to accept AgvDirection parameter
- Inverts Left/Right magnet directions when moving Backward
- Properly handles motor direction context throughout pathfinding

feat: Add automatic start node selection in simulator

- Added SetStartNodeToCombo method to SimulatorForm
- Automatically selects start node combo box when AGV position is set via RFID
- Improves UI usability and workflow efficiency

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
2025-10-24 15:46:16 +09:00
parent 3ddecf63ed
commit d932b8d332
47 changed files with 7473 additions and 1088 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/AGVDirectionCalculator.cs Normal file
View File

@@ -0,0 +1,125 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// AGV 방향 기반 다음 노드 계산기
/// VirtualAGV 또는 실제 AGV 시스템에서 현재 방향을 알 때, 다음 목적지 노드를 결정
/// </summary>
public class AGVDirectionCalculator
{
private DirectionalPathfinder _pathfinder;
public AGVDirectionCalculator(DirectionalPathfinder.DirectionWeights weights = null)
{
_pathfinder = new DirectionalPathfinder(weights);
}
/// <summary>
/// 이전 RFID 위치 + 현재 위치 + 현재 방향을 기반으로 다음 노드 ID를 반환
///
/// 사용 예시:
/// - 001에서 002로 이동 후 GetNextNodeId(001_pos, 002_node, 002_pos, Forward) → 003
/// - 003에서 004로 이동 후, Left 선택 → 030
/// - 004에서 003으로 이동(Backward) 후, GetNextNodeId(..., Backward) → 002
/// </summary>
/// <param name="previousRfidPos">이전 RFID 감지 위치</param>
/// <param name="currentNode">현재 RFID 노드</param>
/// <param name="currentRfidPos">현재 RFID 감지 위치</param>
/// <param name="direction">이동 방향</param>
/// <param name="allNodes">맵의 모든 노드</param>
/// <returns>다음 노드 ID (실패 시 null)</returns>
public string GetNextNodeId(
Point previousRfidPos,
MapNode currentNode,
Point currentRfidPos,
AgvDirection direction,
List<MapNode> allNodes)
{
// 유효성 검사
if (previousRfidPos == Point.Empty)
{
throw new ArgumentException("previousRfidPos는 빈 값일 수 없습니다. 최소 2개의 위치 히스토리가 필요합니다.");
}
if (currentNode == null)
{
throw new ArgumentNullException(nameof(currentNode), "currentNode는 null일 수 없습니다.");
}
if (allNodes == null || allNodes.Count == 0)
{
throw new ArgumentException("allNodes는 비어있을 수 없습니다.");
}
return _pathfinder.GetNextNodeId(
previousRfidPos,
currentNode,
currentRfidPos,
direction,
allNodes
);
}
/// <summary>
/// 현재 모터 상태를 기반으로 실제 선택된 방향을 분석
/// VirtualAGV의 현재/이전 상태로부터 선택된 방향을 역추적
/// </summary>
public AgvDirection AnalyzeSelectedDirection(
Point previousPos,
Point currentPos,
MapNode selectedNextNode,
List<MapNode> connectedNodes)
{
if (previousPos == Point.Empty || currentPos == Point.Empty || selectedNextNode == null)
{
return AgvDirection.Forward;
}
// 이동 벡터
var movementVector = new PointF(
currentPos.X - previousPos.X,
currentPos.Y - previousPos.Y
);
// 다음 노드 벡터
var nextVector = new PointF(
selectedNextNode.Position.X - currentPos.X,
selectedNextNode.Position.Y - currentPos.Y
);
// 내적 계산 (유사도)
float dotProduct = (movementVector.X * nextVector.X) +
(movementVector.Y * nextVector.Y);
// 외적 계산 (좌우 판별)
float crossProduct = (movementVector.X * nextVector.Y) -
(movementVector.Y * nextVector.X);
// 진행 방향 판별
if (dotProduct > 0) // 같은 방향으로 진행
{
if (Math.Abs(crossProduct) < 0.1f) // 거의 직진
{
return AgvDirection.Forward;
}
else if (crossProduct > 0) // 좌측으로 회전
{
return AgvDirection.Left;
}
else // 우측으로 회전
{
return AgvDirection.Right;
}
}
else // 반대 방향으로 진행 (후진)
{
return AgvDirection.Backward;
}
}
}
}

197
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DirectionalPathfinderTest.cs Normal file
View File

@@ -0,0 +1,197 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
using Newtonsoft.Json;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// DirectionalPathfinder 테스트 클래스
/// NewMap.agvmap을 로드하여 방향별 다음 노드를 검증
/// </summary>
public class DirectionalPathfinderTest
{
private List<MapNode> _allNodes;
private Dictionary<string, MapNode> _nodesByRfidId;
private AGVDirectionCalculator _calculator;
public DirectionalPathfinderTest()
{
_nodesByRfidId = new Dictionary<string, MapNode>();
_calculator = new AGVDirectionCalculator();
}
/// <summary>
/// NewMap.agvmap 파일 로드
/// </summary>
public bool LoadMapFile(string filePath)
{
try
{
if (!File.Exists(filePath))
{
Console.WriteLine($"파일을 찾을 수 없습니다: {filePath}");
return false;
}
string jsonContent = File.ReadAllText(filePath);
var mapData = JsonConvert.DeserializeObject<MapFileData>(jsonContent);
if (mapData?.Nodes == null || mapData.Nodes.Count == 0)
{
Console.WriteLine("맵 파일이 비어있습니다.");
return false;
}
_allNodes = mapData.Nodes;
// RFID ID로 인덱싱
foreach (var node in _allNodes)
{
if (!string.IsNullOrEmpty(node.RfidId))
{
_nodesByRfidId[node.RfidId] = node;
}
}
Console.WriteLine($"✓ 맵 파일 로드 성공: {_allNodes.Count}개 노드 로드");
return true;
}
catch (Exception ex)
{
Console.WriteLine($"✗ 맵 파일 로드 실패: {ex.Message}");
return false;
}
}
/// <summary>
/// 테스트: RFID 번호로 노드를 찾고, 다음 노드를 계산
/// </summary>
public void TestDirectionalMovement(string previousRfidId, string currentRfidId, AgvDirection direction)
{
Console.WriteLine($"\n========================================");
Console.WriteLine($"테스트: {previousRfidId} → {currentRfidId} (방향: {direction})");
Console.WriteLine($"========================================");
// RFID ID로 노드 찾기
if (!_nodesByRfidId.TryGetValue(previousRfidId, out var previousNode))
{
Console.WriteLine($"✗ 이전 RFID를 찾을 수 없습니다: {previousRfidId}");
return;
}
if (!_nodesByRfidId.TryGetValue(currentRfidId, out var currentNode))
{
Console.WriteLine($"✗ 현재 RFID를 찾을 수 없습니다: {currentRfidId}");
return;
}
Console.WriteLine($"이전 노드: {previousNode.NodeId} (RFID: {previousNode.RfidId}) - 위치: {previousNode.Position}");
Console.WriteLine($"현재 노드: {currentNode.NodeId} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
Console.WriteLine($"이동 벡터: ({currentNode.Position.X - previousNode.Position.X}, " +
$"{currentNode.Position.Y - previousNode.Position.Y})");
// 다음 노드 계산
string nextNodeId = _calculator.GetNextNodeId(
previousNode.Position,
currentNode,
currentNode.Position,
direction,
_allNodes
);
if (string.IsNullOrEmpty(nextNodeId))
{
Console.WriteLine($"✗ 다음 노드를 찾을 수 없습니다.");
return;
}
// 다음 노드 정보 출력
var nextNode = _allNodes.FirstOrDefault(n => n.NodeId == nextNodeId);
if (nextNode != null)
{
Console.WriteLine($"✓ 다음 노드: {nextNode.NodeId} (RFID: {nextNode.RfidId}) - 위치: {nextNode.Position}");
Console.WriteLine($" ├─ 노드 타입: {GetNodeTypeName(nextNode.Type)}");
Console.WriteLine($" └─ 연결된 노드: {string.Join(", ", nextNode.ConnectedNodes)}");
}
else
{
Console.WriteLine($"✗ 다음 노드 정보를 찾을 수 없습니다: {nextNodeId}");
}
}
/// <summary>
/// 모든 노드 정보 출력
/// </summary>
public void PrintAllNodes()
{
Console.WriteLine("\n========== 모든 노드 정보 ==========");
foreach (var node in _allNodes.OrderBy(n => n.RfidId))
{
Console.WriteLine($"{node.RfidId:D3} → {node.NodeId} ({GetNodeTypeName(node.Type)})");
Console.WriteLine($" 위치: {node.Position}, 연결: {string.Join(", ", node.ConnectedNodes)}");
}
}
/// <summary>
/// 특정 RFID 노드의 상세 정보 출력
/// </summary>
public void PrintNodeInfo(string rfidId)
{
if (!_nodesByRfidId.TryGetValue(rfidId, out var node))
{
Console.WriteLine($"노드를 찾을 수 없습니다: {rfidId}");
return;
}
Console.WriteLine($"\n========== RFID {rfidId} 상세 정보 ==========");
Console.WriteLine($"노드 ID: {node.NodeId}");
Console.WriteLine($"이름: {node.Name}");
Console.WriteLine($"위치: {node.Position}");
Console.WriteLine($"타입: {GetNodeTypeName(node.Type)}");
Console.WriteLine($"회전 가능: {node.CanRotate}");
Console.WriteLine($"활성: {node.IsActive}");
Console.WriteLine($"연결된 노드:");
if (node.ConnectedNodes.Count == 0)
{
Console.WriteLine(" (없음)");
}
else
{
foreach (var connectedId in node.ConnectedNodes)
{
var connectedNode = _allNodes.FirstOrDefault(n => n.NodeId == connectedId);
if (connectedNode != null)
{
Console.WriteLine($" → {connectedId} (RFID: {connectedNode.RfidId}) - 위치: {connectedNode.Position}");
}
else
{
Console.WriteLine($" → {connectedId} (노드 찾을 수 없음)");
}
}
}
}
private string GetNodeTypeName(NodeType type)
{
return type.ToString();
}
// JSON 파일 매핑을 위한 임시 클래스
[Serializable]
private class MapFileData
{
[JsonProperty("Nodes")]
public List<MapNode> Nodes { get; set; }
[JsonProperty("RfidMappings")]
public List<dynamic> RfidMappings { get; set; }
}
}
}

44
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
View File

@@ -20,7 +20,7 @@ namespace AGVNavigationCore.Utils
/// <param name="mapNodes">맵 노드 목록</param>
/// <param name="currentDirection">AGV 현재 방향</param>
/// <returns>도킹 검증 결과</returns>
public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes, AgvDirection currentDirection)
public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes)
{
// 경로가 없거나 실패한 경우
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
@@ -31,53 +31,59 @@ namespace AGVNavigationCore.Utils
// 목적지 노드 찾기
string targetNodeId = pathResult.Path[pathResult.Path.Count - 1];
var targetNode = mapNodes?.FirstOrDefault(n => n.NodeId == targetNodeId);
var LastNode = mapNodes?.FirstOrDefault(n => n.NodeId == targetNodeId);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {targetNodeId}");
if (targetNode == null)
if (LastNode == null)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드 찾을 수 없음: {targetNodeId}");
return DockingValidationResult.CreateNotRequired();
}
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드 타입: {targetNode.Type} ({(int)targetNode.Type})");
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {targetNodeId} 타입:{LastNode.Type} ({(int)LastNode.Type})");
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
if (!IsDockingRequired(targetNode.DockDirection))
if (LastNode.DockDirection == DockingDirection.DontCare)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {targetNode.DockDirection}");
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {LastNode.DockDirection}");
return DockingValidationResult.CreateNotRequired();
}
// 필요한 도킹 방향 확인
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
var requiredDirection = GetRequiredDockingDirection(LastNode.DockDirection);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 필요한 도킹 방향: {requiredDirection}");
// 경로 기반 최종 방향 계산
var calculatedDirection = CalculateFinalDirection(pathResult.Path, mapNodes, currentDirection);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 계산된 최종 방향: {calculatedDirection}");
System.Diagnostics.Debug.WriteLine($"[DockingValidator] AGV 현재 방향: {currentDirection}");
var LastNodeInfo = pathResult.DetailedPath.Last();
if (LastNodeInfo.NodeId != LastNode.NodeId)
{
string error = $"마지막 노드의 도킹방향과 경로정보의 노드ID 불일치: 필요={LastNode.NodeId}, 계산됨={LastNodeInfo.NodeId }";
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
return DockingValidationResult.CreateInvalid(
targetNodeId,
LastNode.Type,
requiredDirection,
LastNodeInfo.MotorDirection,
error);
}
// 검증 수행
if (calculatedDirection == requiredDirection)
if (LastNodeInfo.MotorDirection == requiredDirection)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ✅ 도킹 검증 성공");
return DockingValidationResult.CreateValid(
targetNodeId,
targetNode.Type,
LastNode.Type,
requiredDirection,
calculatedDirection);
LastNodeInfo.MotorDirection);
}
else
{
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(calculatedDirection)}";
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(LastNodeInfo.MotorDirection)}";
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
return DockingValidationResult.CreateInvalid(
targetNodeId,
targetNode.Type,
LastNode.Type,
requiredDirection,
calculatedDirection,
LastNodeInfo.MotorDirection,
error);
}
}

342
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/GetNextNodeIdTest.cs Normal file
View File

@@ -0,0 +1,342 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// GetNextNodeId() 메서드의 동작을 검증하는 테스트 클래스
///
/// 테스트 시나리오:
/// - 001(65,229) → 002(206,244) → Forward → 003이 나와야 함
/// - 001(65,229) → 002(206,244) → Backward → 001이 나와야 함
/// - 002(206,244) → 003(278,278) → Forward → 004가 나와야 함
/// - 002(206,244) → 003(278,278) → Backward → 002가 나와야 함
/// </summary>
public class GetNextNodeIdTest
{
/// <summary>
/// 가상의 VirtualAGV 상태를 시뮬레이션하여 GetNextNodeId 테스트
/// </summary>
public void TestGetNextNodeId()
{
Console.WriteLine("\n================================================");
Console.WriteLine("GetNextNodeId() 동작 검증");
Console.WriteLine("================================================\n");
// 테스트 노드 생성
var node001 = new MapNode { NodeId = "N001", RfidId = "001", Position = new Point(65, 229), ConnectedNodes = new List<string> { "N002" } };
var node002 = new MapNode { NodeId = "N002", RfidId = "002", Position = new Point(206, 244), ConnectedNodes = new List<string> { "N001", "N003" } };
var node003 = new MapNode { NodeId = "N003", RfidId = "003", Position = new Point(278, 278), ConnectedNodes = new List<string> { "N002", "N004" } };
var node004 = new MapNode { NodeId = "N004", RfidId = "004", Position = new Point(380, 340), ConnectedNodes = new List<string> { "N003", "N022", "N031" } };
var allNodes = new List<MapNode> { node001, node002, node003, node004 };
// VirtualAGV 시뮬레이션 (실제 인스턴스 생성 불가하므로 로직만 재현)
Console.WriteLine("테스트 시나리오 1: 001 → 002 → Forward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Forward 이동: 001에서 002로, 다음은 Forward",
node001.Position, node002, node003,
AgvDirection.Forward, allNodes,
"003 (예상)"
);
Console.WriteLine("\n테스트 시나리오 2: 001 → 002 → Backward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Backward 이동: 001에서 002로, 다음은 Backward",
node001.Position, node002, node001,
AgvDirection.Backward, allNodes,
"001 (예상)"
);
Console.WriteLine("\n테스트 시나리오 3: 002 → 003 → Forward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Forward 이동: 002에서 003으로, 다음은 Forward",
node002.Position, node003, node004,
AgvDirection.Forward, allNodes,
"004 (예상)"
);
Console.WriteLine("\n테스트 시나리오 4: 002 → 003 Forward → Backward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Forward 이동: 002에서 003으로, 다음은 Backward (경로 반대)",
node002.Position, node003, node002,
AgvDirection.Backward, allNodes,
"002 (예상 - 경로 반대)"
);
Console.WriteLine("\n테스트 시나리오 5: 002 → 003 Backward → Forward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Backward 이동: 002에서 003으로, 다음은 Forward (경로 반대)",
node002.Position, node003, node002,
AgvDirection.Forward, allNodes,
"002 (예상 - 경로 반대)",
AgvDirection.Backward // 현재 모터 방향
);
Console.WriteLine("\n테스트 시나리오 6: 002 → 003 Backward → Backward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Backward 이동: 002에서 003으로, 다음은 Backward (경로 계속)",
node002.Position, node003, node004,
AgvDirection.Backward, allNodes,
"004 (예상 - 경로 계속)",
AgvDirection.Backward // 현재 모터 방향
);
Console.WriteLine("\n\n================================================");
Console.WriteLine("테스트 완료");
Console.WriteLine("================================================\n");
}
/// <summary>
/// 개별 테스트 시나리오 실행
/// </summary>
private void TestScenario(
string description,
Point prevPos,
MapNode currentNode,
MapNode expectedNextNode,
AgvDirection direction,
List<MapNode> allNodes,
string expectedNodeIdStr,
AgvDirection? currentMotorDirection = null)
{
// 현재 모터 방향이 지정되지 않으면 direction과 동일하다고 가정
AgvDirection motorDir = currentMotorDirection ?? direction;
Console.WriteLine($"설명: {description}");
Console.WriteLine($"이전 위치: {prevPos} (RFID: {allNodes.First(n => n.Position == prevPos)?.RfidId ?? "?"})");
Console.WriteLine($"현재 노드: {currentNode.NodeId} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
Console.WriteLine($"현재 모터 방향: {motorDir}");
Console.WriteLine($"요청 방향: {direction}");
// 이동 벡터 계산
var movementVector = new PointF(
currentNode.Position.X - prevPos.X,
currentNode.Position.Y - prevPos.Y
);
Console.WriteLine($"이동 벡터: ({movementVector.X}, {movementVector.Y})");
// 각 후보 노드에 대한 점수 계산
Console.WriteLine($"\n현재 노드({currentNode.NodeId})의 ConnectedNodes: {string.Join(", ", currentNode.ConnectedNodes)}");
Console.WriteLine($"가능한 다음 노드들:");
var candidateNodes = allNodes.Where(n =>
currentNode.ConnectedNodes.Contains(n.NodeId) && n.NodeId != currentNode.NodeId
).ToList();
foreach (var candidate in candidateNodes)
{
var score = CalculateScoreAndPrint(movementVector, currentNode.Position, candidate, direction);
string isExpected = (candidate.NodeId == expectedNextNode.NodeId) ? " ← 예상 노드" : "";
Console.WriteLine($" {candidate.NodeId} (RFID: {candidate.RfidId}) - 위치: {candidate.Position} - 점수: {score:F1}{isExpected}");
}
// 최고 점수 노드 선택
var bestCandidate = GetBestCandidate(movementVector, currentNode.Position, candidateNodes, direction);
Console.WriteLine($"\n✓ 선택된 노드: {bestCandidate.NodeId} (RFID: {bestCandidate.RfidId})");
if (bestCandidate.NodeId == expectedNextNode.NodeId)
{
Console.WriteLine($"✅ 정답! ({expectedNodeIdStr})");
}
else
{
Console.WriteLine($"❌ 오답! 예상: {expectedNextNode.NodeId}, 실제: {bestCandidate.NodeId}");
}
}
/// <summary>
/// 점수 계산 및 상세 정보 출력
/// </summary>
private float CalculateScoreAndPrint(PointF movementVector, Point currentPos, MapNode candidate, AgvDirection direction)
{
// 벡터 정규화
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 다음 벡터
var toNextVector = new PointF(
candidate.Position.X - currentPos.X,
candidate.Position.Y - currentPos.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 내적 및 외적 계산
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
(normalizedMovement.Y * normalizedToNext.X);
float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);
return score;
}
/// <summary>
/// 점수 계산 (VirtualAGV.CalculateDirectionalScore()와 동일)
/// </summary>
private float CalculateDirectionalScore(float dotProduct, float crossProduct, AgvDirection direction)
{
float baseScore = 0;
switch (direction)
{
case AgvDirection.Forward:
if (dotProduct > 0.9f)
baseScore = 100.0f;
else if (dotProduct > 0.5f)
baseScore = 80.0f;
else if (dotProduct > 0.0f)
baseScore = 50.0f;
else if (dotProduct > -0.5f)
baseScore = 20.0f;
break;
case AgvDirection.Backward:
if (dotProduct < -0.9f)
baseScore = 100.0f;
else if (dotProduct < -0.5f)
baseScore = 80.0f;
else if (dotProduct < 0.0f)
baseScore = 50.0f;
else if (dotProduct < 0.5f)
baseScore = 20.0f;
break;
case AgvDirection.Left:
if (dotProduct > 0.0f)
{
if (crossProduct > 0.5f)
baseScore = 100.0f;
else if (crossProduct > 0.0f)
baseScore = 70.0f;
else if (crossProduct > -0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
else
{
if (crossProduct < -0.5f)
baseScore = 100.0f;
else if (crossProduct < 0.0f)
baseScore = 70.0f;
else if (crossProduct < 0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
break;
case AgvDirection.Right:
if (dotProduct > 0.0f)
{
if (crossProduct < -0.5f)
baseScore = 100.0f;
else if (crossProduct < 0.0f)
baseScore = 70.0f;
else if (crossProduct < 0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
else
{
if (crossProduct > 0.5f)
baseScore = 100.0f;
else if (crossProduct > 0.0f)
baseScore = 70.0f;
else if (crossProduct > -0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
break;
}
return baseScore;
}
/// <summary>
/// 최고 점수 노드 반환
/// </summary>
private MapNode GetBestCandidate(PointF movementVector, Point currentPos, List<MapNode> candidates, AgvDirection direction)
{
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
MapNode bestCandidate = null;
float bestScore = -1;
foreach (var candidate in candidates)
{
var toNextVector = new PointF(
candidate.Position.X - currentPos.X,
candidate.Position.Y - currentPos.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
(normalizedMovement.Y * normalizedToNext.X);
float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);
if (score > bestScore)
{
bestScore = score;
bestCandidate = candidate;
}
}
return bestCandidate;
}
}
}

153
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/ImageConverterUtil.cs Normal file
View File

@@ -0,0 +1,153 @@
using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// 이미지와 문자열 간 변환을 위한 유틸리티 클래스
/// Base64 인코딩을 사용하여 이미지를 문자열로 변환하거나 그 반대로 수행
/// </summary>
public static class ImageConverterUtil
{
/// <summary>
/// Image 객체를 Base64 문자열로 변환
/// </summary>
/// <param name="image">변환할 이미지</param>
/// <param name="format">이미지 포맷 (기본값: PNG)</param>
/// <returns>Base64 인코딩된 문자열, null인 경우 빈 문자열 반환</returns>
public static string ImageToBase64(Image image, ImageFormat format = null)
{
if (image == null)
return string.Empty;
try
{
format = format ?? ImageFormat.Png;
using (var memoryStream = new MemoryStream())
{
image.Save(memoryStream, format);
byte[] imageBytes = memoryStream.ToArray();
return Convert.ToBase64String(imageBytes);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"이미지 변환 실패: {ex.Message}");
return string.Empty;
}
}
/// <summary>
/// 파일 경로의 이미지를 Base64 문자열로 변환
/// </summary>
/// <param name="filePath">이미지 파일 경로</param>
/// <param name="format">변환할 포맷 (기본값: PNG, 원본 포맷 유지하려면 null)</param>
/// <returns>Base64 인코딩된 문자열</returns>
public static string FileToBase64(string filePath, ImageFormat format = null)
{
if (string.IsNullOrEmpty(filePath) || !File.Exists(filePath))
return string.Empty;
try
{
using (var image = Image.FromFile(filePath))
{
return ImageToBase64(image, format ?? ImageFormat.Png);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"파일 변환 실패: {ex.Message}");
return string.Empty;
}
}
/// <summary>
/// Base64 문자열을 Image 객체로 변환
/// </summary>
/// <param name="base64String">Base64 인코딩된 문자열</param>
/// <returns>변환된 Image 객체, 실패 시 null</returns>
public static Image Base64ToImage(string base64String)
{
if (string.IsNullOrEmpty(base64String))
return null;
try
{
byte[] imageBytes = Convert.FromBase64String(base64String);
using (var memoryStream = new MemoryStream(imageBytes))
{
return Image.FromStream(memoryStream);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"Base64 이미지 변환 실패: {ex.Message}");
return null;
}
}
/// <summary>
/// Base64 문자열을 Bitmap 객체로 변환
/// Image 대신 Bitmap을 반환하므로 메모리 관리가 더 안정적
/// </summary>
/// <param name="base64String">Base64 인코딩된 문자열</param>
/// <returns>변환된 Bitmap 객체, 실패 시 null</returns>
public static Bitmap Base64ToBitmap(string base64String)
{
if (string.IsNullOrEmpty(base64String))
return null;
try
{
byte[] imageBytes = Convert.FromBase64String(base64String);
using (var memoryStream = new MemoryStream(imageBytes))
{
return new Bitmap(memoryStream);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"Base64 Bitmap 변환 실패: {ex.Message}");
return null;
}
}
/// <summary>
/// Base64 문자열이 유효한지 확인
/// </summary>
/// <param name="base64String">검증할 Base64 문자열</param>
/// <returns>유효하면 true, 그 외 false</returns>
public static bool IsValidBase64(string base64String)
{
if (string.IsNullOrWhiteSpace(base64String))
return false;
try
{
Convert.FromBase64String(base64String);
return true;
}
catch
{
return false;
}
}
/// <summary>
/// Base64 이미지 데이터의 크기를 대략적으로 계산 (바이트 단위)
/// </summary>
/// <param name="base64String">Base64 문자열</param>
/// <returns>예상 바이트 크기</returns>
public static long GetApproximateSize(string base64String)
{
if (string.IsNullOrEmpty(base64String))
return 0;
// Base64는 원본 데이터보다 약 33% 더 큼
return (long)(base64String.Length * 0.75);
}
}
}

56
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/TestRunner.cs Normal file
View File

@@ -0,0 +1,56 @@
using System;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// DirectionalPathfinder 테스트 실행 프로그램
///
/// 사용법:
/// var runner = new TestRunner();
/// runner.RunTests();
/// </summary>
public class TestRunner
{
public void RunTests()
{
string mapFilePath = @"C:\Data\Source\(5613#) ENIG AGV\Source\Cs_HMI\Data\NewMap.agvmap";
var tester = new DirectionalPathfinderTest();
// 맵 파일 로드
if (!tester.LoadMapFile(mapFilePath))
{
Console.WriteLine("맵 파일 로드 실패!");
return;
}
// 모든 노드 정보 출력
tester.PrintAllNodes();
// 테스트 시나리오 1: 001 → 002 → Forward (003 기대)
tester.PrintNodeInfo("001");
tester.PrintNodeInfo("002");
tester.TestDirectionalMovement("001", "002", AgvDirection.Forward);
// 테스트 시나리오 2: 002 → 001 → Backward (000 또는 이전 기대)
tester.TestDirectionalMovement("002", "001", AgvDirection.Backward);
// 테스트 시나리오 3: 002 → 003 → Forward
tester.PrintNodeInfo("003");
tester.TestDirectionalMovement("002", "003", AgvDirection.Forward);
// 테스트 시나리오 4: 003 → 004 → Forward
tester.PrintNodeInfo("004");
tester.TestDirectionalMovement("003", "004", AgvDirection.Forward);
// 테스트 시나리오 5: 003 → 004 → Right (030 기대)
tester.TestDirectionalMovement("003", "004", AgvDirection.Right);
// 테스트 시나리오 6: 004 → 003 → Backward
tester.TestDirectionalMovement("004", "003", AgvDirection.Backward);
Console.WriteLine("\n\n=== 테스트 완료 ===");
}
}
}