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

refactor: Consolidate RFID mapping and add bidirectional pathfinding

Browse Source 
Major improvements to AGV navigation system:

• Consolidated RFID management into MapNode, removing duplicate RfidMapping class
• Enhanced MapNode with RFID metadata fields (RfidStatus, RfidDescription)
• Added automatic bidirectional connection generation in pathfinding algorithms
• Updated all components to use unified MapNode-based RFID system
• Added command line argument support for AGVMapEditor auto-loading files
• Fixed pathfinding failures by ensuring proper node connectivity

Technical changes:
- Removed RfidMapping class and dependencies across all projects
- Updated AStarPathfinder with EnsureBidirectionalConnections() method
- Modified MapLoader to use AssignAutoRfidIds() for RFID automation
- Enhanced UnifiedAGVCanvas, SimulatorForm, and MainForm for MapNode integration
- Improved data consistency and reduced memory footprint

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

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
ChiKyun Kim
2025-09-11 16:41:52 +09:00
parent 7567602479
commit de0e39e030
50 changed files with 9578 additions and 1854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

244
Cs_HMI/AGVNavigationCore/PathFinding/AGVPathResult.cs Normal file
View File

@@ -0,0 +1,244 @@
using System;
using System.Collections.Generic;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// AGV 경로 계산 결과 (방향성 및 명령어 포함)
/// </summary>
public class AGVPathResult
{
/// <summary>
/// 경로 찾기 성공 여부
/// </summary>
public bool Success { get; set; }
/// <summary>
/// 경로 노드 ID 목록 (시작 → 목적지 순서)
/// </summary>
public List<string> Path { get; set; }
/// <summary>
/// AGV 명령어 목록 (이동 방향 시퀀스)
/// </summary>
public List<AgvDirection> Commands { get; set; }
/// <summary>
/// 총 거리
/// </summary>
public float TotalDistance { get; set; }
/// <summary>
/// 계산 소요 시간 (밀리초)
/// </summary>
public long CalculationTimeMs { get; set; }
/// <summary>
/// 예상 소요 시간 (초)
/// </summary>
public float EstimatedTimeSeconds { get; set; }
/// <summary>
/// 회전 횟수
/// </summary>
public int RotationCount { get; set; }
/// <summary>
/// 오류 메시지 (실패시)
/// </summary>
public string ErrorMessage { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public AGVPathResult()
{
Success = false;
Path = new List<string>();
Commands = new List<AgvDirection>();
TotalDistance = 0;
CalculationTimeMs = 0;
EstimatedTimeSeconds = 0;
RotationCount = 0;
ErrorMessage = string.Empty;
}
/// <summary>
/// 성공 결과 생성
/// </summary>
/// <param name="path">경로</param>
/// <param name="commands">AGV 명령어 목록</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<string> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<string>(path),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 실패 결과 생성
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>실패 결과</returns>
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs)
{
return new AGVPathResult
{
Success = false,
ErrorMessage = errorMessage,
CalculationTimeMs = calculationTimeMs
};
}
/// <summary>
/// 경로 메트릭 계산
/// </summary>
private void CalculateMetrics()
{
RotationCount = CountRotations();
EstimatedTimeSeconds = CalculateEstimatedTime();
}
/// <summary>
/// 회전 횟수 계산
/// </summary>
private int CountRotations()
{
int count = 0;
foreach (var command in Commands)
{
if (command == AgvDirection.Left || command == AgvDirection.Right)
{
count++;
}
}
return count;
}
/// <summary>
/// 예상 소요 시간 계산
/// </summary>
/// <param name="agvSpeed">AGV 속도 (픽셀/초, 기본값: 100)</param>
/// <param name="rotationTime">회전 시간 (초, 기본값: 3)</param>
/// <returns>예상 소요 시간 (초)</returns>
private float CalculateEstimatedTime(float agvSpeed = 100.0f, float rotationTime = 3.0f)
{
float moveTime = TotalDistance / agvSpeed;
float totalRotationTime = RotationCount * rotationTime;
return moveTime + totalRotationTime;
}
/// <summary>
/// 명령어 요약 생성
/// </summary>
/// <returns>명령어 요약 문자열</returns>
public string GetCommandSummary()
{
if (!Success) return "실패";
var summary = new List<string>();
var currentCommand = AgvDirection.Stop;
var count = 0;
foreach (var command in Commands)
{
if (command == currentCommand)
{
count++;
}
else
{
if (count > 0)
{
summary.Add($"{GetCommandText(currentCommand)}×{count}");
}
currentCommand = command;
count = 1;
}
}
if (count > 0)
{
summary.Add($"{GetCommandText(currentCommand)}×{count}");
}
return string.Join(" → ", summary);
}
/// <summary>
/// 명령어 텍스트 반환
/// </summary>
private string GetCommandText(AgvDirection command)
{
switch (command)
{
case AgvDirection.Forward: return "전진";
case AgvDirection.Backward: return "후진";
case AgvDirection.Left: return "좌회전";
case AgvDirection.Right: return "우회전";
case AgvDirection.Stop: return "정지";
default: return command.ToString();
}
}
/// <summary>
/// 상세 경로 정보 반환
/// </summary>
/// <returns>상세 정보 문자열</returns>
public string GetDetailedInfo()
{
if (!Success)
{
return $"경로 계산 실패: {ErrorMessage} (계산시간: {CalculationTimeMs}ms)";
}
return $"경로: {Path.Count}개 노드, 거리: {TotalDistance:F1}px, " +
$"회전: {RotationCount}회, 예상시간: {EstimatedTimeSeconds:F1}초, " +
$"계산시간: {CalculationTimeMs}ms";
}
/// <summary>
/// PathResult로 변환 (호환성을 위해)
/// </summary>
/// <returns>PathResult 객체</returns>
public PathResult ToPathResult()
{
if (Success)
{
return PathResult.CreateSuccess(Path, TotalDistance, CalculationTimeMs, 0);
}
else
{
return PathResult.CreateFailure(ErrorMessage, CalculationTimeMs, 0);
}
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
if (Success)
{
return $"Success: {Path.Count} nodes, {TotalDistance:F1}px, {RotationCount} rotations, {EstimatedTimeSeconds:F1}s";
}
else
{
return $"Failed: {ErrorMessage}";
}
}
}
}

287
Cs_HMI/AGVNavigationCore/PathFinding/AGVPathfinder.cs Normal file
View File

@@ -0,0 +1,287 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// AGV 특화 경로 탐색기 (방향성 및 도킹 제약 고려)
/// </summary>
public class AGVPathfinder
{
private AStarPathfinder _pathfinder;
private Dictionary<string, MapNode> _nodeMap;
/// <summary>
/// AGV 현재 방향
/// </summary>
public AgvDirection CurrentDirection { get; set; } = AgvDirection.Forward;
/// <summary>
/// 회전 비용 가중치 (회전이 비싼 동작임을 반영)
/// </summary>
public float RotationCostWeight { get; set; } = 50.0f;
/// <summary>
/// 도킹 접근 거리 (픽셀 단위)
/// </summary>
public float DockingApproachDistance { get; set; } = 100.0f;
/// <summary>
/// 생성자
/// </summary>
public AGVPathfinder()
{
_pathfinder = new AStarPathfinder();
_nodeMap = new Dictionary<string, MapNode>();
}
/// <summary>
/// 맵 노드 설정
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
public void SetMapNodes(List<MapNode> mapNodes)
{
_pathfinder.SetMapNodes(mapNodes);
_nodeMap.Clear();
foreach (var node in mapNodes ?? new List<MapNode>())
{
_nodeMap[node.NodeId] = node;
}
}
/// <summary>
/// AGV 경로 계산 (방향성 및 도킹 제약 고려)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
/// <returns>AGV 경로 계산 결과</returns>
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection = null)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(startNodeId))
{
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds);
}
if (!_nodeMap.ContainsKey(endNodeId))
{
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds);
}
var endNode = _nodeMap[endNodeId];
if (IsSpecialNode(endNode))
{
return FindPathToSpecialNode(startNodeId, endNode, targetDirection, stopwatch);
}
else
{
return FindNormalPath(startNodeId, endNodeId, targetDirection, stopwatch);
}
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"AGV 경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds);
}
}
/// <summary>
/// 충전 스테이션으로의 경로 찾기
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <returns>AGV 경로 계산 결과</returns>
public AGVPathResult FindPathToChargingStation(string startNodeId)
{
var chargingStations = _nodeMap.Values
.Where(n => n.Type == NodeType.Charging && n.IsActive)
.Select(n => n.NodeId)
.ToList();
if (chargingStations.Count == 0)
{
return AGVPathResult.CreateFailure("사용 가능한 충전 스테이션이 없습니다", 0);
}
var nearestResult = _pathfinder.FindNearestPath(startNodeId, chargingStations);
if (!nearestResult.Success)
{
return AGVPathResult.CreateFailure("충전 스테이션으로의 경로를 찾을 수 없습니다", nearestResult.CalculationTimeMs);
}
var targetNodeId = nearestResult.Path.Last();
return FindAGVPath(startNodeId, targetNodeId, AgvDirection.Forward);
}
/// <summary>
/// 특정 타입의 도킹 스테이션으로의 경로 찾기
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="stationType">장비 타입</param>
/// <returns>AGV 경로 계산 결과</returns>
public AGVPathResult FindPathToDockingStation(string startNodeId, StationType stationType)
{
var dockingStations = _nodeMap.Values
.Where(n => n.Type == NodeType.Docking && n.StationType == stationType && n.IsActive)
.Select(n => n.NodeId)
.ToList();
if (dockingStations.Count == 0)
{
return AGVPathResult.CreateFailure($"{stationType} 타입의 사용 가능한 도킹 스테이션이 없습니다", 0);
}
var nearestResult = _pathfinder.FindNearestPath(startNodeId, dockingStations);
if (!nearestResult.Success)
{
return AGVPathResult.CreateFailure($"{stationType} 도킹 스테이션으로의 경로를 찾을 수 없습니다", nearestResult.CalculationTimeMs);
}
var targetNodeId = nearestResult.Path.Last();
return FindAGVPath(startNodeId, targetNodeId, AgvDirection.Backward);
}
/// <summary>
/// 일반 노드로의 경로 계산
/// </summary>
private AGVPathResult FindNormalPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, System.Diagnostics.Stopwatch stopwatch)
{
var result = _pathfinder.FindPath(startNodeId, endNodeId);
if (!result.Success)
{
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
}
var agvCommands = GenerateAGVCommands(result.Path, targetDirection ?? AgvDirection.Forward);
return AGVPathResult.CreateSuccess(result.Path, agvCommands, result.TotalDistance, stopwatch.ElapsedMilliseconds);
}
/// <summary>
/// 특수 노드(도킹/충전)로의 경로 계산
/// </summary>
private AGVPathResult FindPathToSpecialNode(string startNodeId, MapNode endNode, AgvDirection? targetDirection, System.Diagnostics.Stopwatch stopwatch)
{
var requiredDirection = GetRequiredDirectionForNode(endNode);
var actualTargetDirection = targetDirection ?? requiredDirection;
var result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
if (!result.Success)
{
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
}
if (actualTargetDirection != requiredDirection)
{
return AGVPathResult.CreateFailure($"{endNode.NodeId}는 {requiredDirection} 방향으로만 접근 가능합니다", stopwatch.ElapsedMilliseconds);
}
var agvCommands = GenerateAGVCommands(result.Path, actualTargetDirection);
return AGVPathResult.CreateSuccess(result.Path, agvCommands, result.TotalDistance, stopwatch.ElapsedMilliseconds);
}
/// <summary>
/// 노드가 특수 노드(도킹/충전)인지 확인
/// </summary>
private bool IsSpecialNode(MapNode node)
{
return node.Type == NodeType.Docking || node.Type == NodeType.Charging;
}
/// <summary>
/// 노드에 필요한 접근 방향 반환
/// </summary>
private AgvDirection GetRequiredDirectionForNode(MapNode node)
{
switch (node.Type)
{
case NodeType.Charging:
return AgvDirection.Forward;
case NodeType.Docking:
return node.DockDirection == DockingDirection.Forward ? AgvDirection.Forward : AgvDirection.Backward;
default:
return AgvDirection.Forward;
}
}
/// <summary>
/// 경로에서 AGV 명령어 생성
/// </summary>
private List<AgvDirection> GenerateAGVCommands(List<string> path, AgvDirection targetDirection)
{
var commands = new List<AgvDirection>();
if (path.Count < 2) return commands;
var currentDir = CurrentDirection;
for (int i = 0; i < path.Count - 1; i++)
{
var currentNodeId = path[i];
var nextNodeId = path[i + 1];
if (_nodeMap.ContainsKey(currentNodeId) && _nodeMap.ContainsKey(nextNodeId))
{
var currentNode = _nodeMap[currentNodeId];
var nextNode = _nodeMap[nextNodeId];
if (currentNode.CanRotate && ShouldRotate(currentDir, targetDirection))
{
commands.Add(GetRotationCommand(currentDir, targetDirection));
currentDir = targetDirection;
}
commands.Add(currentDir);
}
}
return commands;
}
/// <summary>
/// 회전이 필요한지 판단
/// </summary>
private bool ShouldRotate(AgvDirection current, AgvDirection target)
{
return current != target && (current == AgvDirection.Forward && target == AgvDirection.Backward ||
current == AgvDirection.Backward && target == AgvDirection.Forward);
}
/// <summary>
/// 회전 명령어 반환
/// </summary>
private AgvDirection GetRotationCommand(AgvDirection from, AgvDirection to)
{
if (from == AgvDirection.Forward && to == AgvDirection.Backward)
return AgvDirection.Right;
if (from == AgvDirection.Backward && to == AgvDirection.Forward)
return AgvDirection.Right;
return AgvDirection.Right;
}
/// <summary>
/// 경로 유효성 검증
/// </summary>
/// <param name="path">검증할 경로</param>
/// <returns>유효성 검증 결과</returns>
public bool ValidatePath(List<string> path)
{
if (path == null || path.Count < 2) return true;
for (int i = 0; i < path.Count - 1; i++)
{
if (!_pathfinder.AreNodesConnected(path[i], path[i + 1]))
{
return false;
}
}
return true;
}
}
}

291
Cs_HMI/AGVNavigationCore/PathFinding/AStarPathfinder.cs Normal file
View File

@@ -0,0 +1,291 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// A* 알고리즘 기반 경로 탐색기
/// </summary>
public class AStarPathfinder
{
private Dictionary<string, PathNode> _nodeMap;
private List<MapNode> _mapNodes;
/// <summary>
/// 휴리스틱 가중치 (기본값: 1.0)
/// 값이 클수록 목적지 방향을 우선시하나 최적 경로를 놓칠 수 있음
/// </summary>
public float HeuristicWeight { get; set; } = 1.0f;
/// <summary>
/// 최대 탐색 노드 수 (무한 루프 방지)
/// </summary>
public int MaxSearchNodes { get; set; } = 1000;
/// <summary>
/// 생성자
/// </summary>
public AStarPathfinder()
{
_nodeMap = new Dictionary<string, PathNode>();
_mapNodes = new List<MapNode>();
}
/// <summary>
/// 맵 노드 설정
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
public void SetMapNodes(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_nodeMap.Clear();
// 1단계: 모든 네비게이션 노드를 PathNode로 변환
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode())
{
var pathNode = new PathNode(mapNode.NodeId, mapNode.Position);
pathNode.ConnectedNodes = new List<string>(mapNode.ConnectedNodes);
_nodeMap[mapNode.NodeId] = pathNode;
}
}
// 2단계: 양방향 연결 자동 생성 (A→B 연결이 있으면 B→A도 추가)
EnsureBidirectionalConnections();
}
/// <summary>
/// 단방향 연결을 양방향으로 자동 변환
/// A→B 연결이 있으면 B→A 연결도 자동 생성
/// </summary>
private void EnsureBidirectionalConnections()
{
foreach (var nodeId in _nodeMap.Keys.ToList())
{
var node = _nodeMap[nodeId];
foreach (var connectedNodeId in node.ConnectedNodes.ToList())
{
// 연결된 노드가 존재하고 네비게이션 가능한 노드인지 확인
if (_nodeMap.ContainsKey(connectedNodeId))
{
var connectedNode = _nodeMap[connectedNodeId];
// 역방향 연결이 없으면 추가
if (!connectedNode.ConnectedNodes.Contains(nodeId))
{
connectedNode.ConnectedNodes.Add(nodeId);
}
}
}
}
}
/// <summary>
/// 경로 찾기 (A* 알고리즘)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public PathResult FindPath(string startNodeId, string endNodeId)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(startNodeId))
{
return PathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(endNodeId))
{
return PathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (startNodeId == endNodeId)
{
return PathResult.CreateSuccess(new List<string> { startNodeId }, 0, stopwatch.ElapsedMilliseconds, 1);
}
var startNode = _nodeMap[startNodeId];
var endNode = _nodeMap[endNodeId];
var openSet = new List<PathNode>();
var closedSet = new HashSet<string>();
var exploredCount = 0;
startNode.GCost = 0;
startNode.HCost = CalculateHeuristic(startNode, endNode);
startNode.Parent = null;
openSet.Add(startNode);
while (openSet.Count > 0 && exploredCount < MaxSearchNodes)
{
var currentNode = GetLowestFCostNode(openSet);
openSet.Remove(currentNode);
closedSet.Add(currentNode.NodeId);
exploredCount++;
if (currentNode.NodeId == endNodeId)
{
var path = ReconstructPath(currentNode);
var totalDistance = CalculatePathDistance(path);
return PathResult.CreateSuccess(path, totalDistance, stopwatch.ElapsedMilliseconds, exploredCount);
}
foreach (var neighborId in currentNode.ConnectedNodes)
{
if (closedSet.Contains(neighborId) || !_nodeMap.ContainsKey(neighborId))
continue;
var neighbor = _nodeMap[neighborId];
var tentativeGCost = currentNode.GCost + currentNode.DistanceTo(neighbor);
if (!openSet.Contains(neighbor))
{
neighbor.Parent = currentNode;
neighbor.GCost = tentativeGCost;
neighbor.HCost = CalculateHeuristic(neighbor, endNode);
openSet.Add(neighbor);
}
else if (tentativeGCost < neighbor.GCost)
{
neighbor.Parent = currentNode;
neighbor.GCost = tentativeGCost;
}
}
}
return PathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
}
catch (Exception ex)
{
return PathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
/// <summary>
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
/// <returns>경로 계산 결과</returns>
public PathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
{
if (targetNodeIds == null || targetNodeIds.Count == 0)
{
return PathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
}
PathResult bestResult = null;
foreach (var targetId in targetNodeIds)
{
var result = FindPath(startNodeId, targetId);
if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
{
bestResult = result;
}
}
return bestResult ?? PathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
}
/// <summary>
/// 휴리스틱 거리 계산 (유클리드 거리)
/// </summary>
private float CalculateHeuristic(PathNode from, PathNode to)
{
return from.DistanceTo(to) * HeuristicWeight;
}
/// <summary>
/// F cost가 가장 낮은 노드 선택
/// </summary>
private PathNode GetLowestFCostNode(List<PathNode> nodes)
{
PathNode lowest = nodes[0];
foreach (var node in nodes)
{
if (node.FCost < lowest.FCost ||
(Math.Abs(node.FCost - lowest.FCost) < 0.001f && node.HCost < lowest.HCost))
{
lowest = node;
}
}
return lowest;
}
/// <summary>
/// 경로 재구성 (부모 노드를 따라 역추적)
/// </summary>
private List<string> ReconstructPath(PathNode endNode)
{
var path = new List<string>();
var current = endNode;
while (current != null)
{
path.Add(current.NodeId);
current = current.Parent;
}
path.Reverse();
return path;
}
/// <summary>
/// 경로의 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<string> path)
{
if (path.Count < 2) return 0;
float totalDistance = 0;
for (int i = 0; i < path.Count - 1; i++)
{
if (_nodeMap.ContainsKey(path[i]) && _nodeMap.ContainsKey(path[i + 1]))
{
totalDistance += _nodeMap[path[i]].DistanceTo(_nodeMap[path[i + 1]]);
}
}
return totalDistance;
}
/// <summary>
/// 두 노드가 연결되어 있는지 확인
/// </summary>
/// <param name="nodeId1">노드 1 ID</param>
/// <param name="nodeId2">노드 2 ID</param>
/// <returns>연결 여부</returns>
public bool AreNodesConnected(string nodeId1, string nodeId2)
{
if (!_nodeMap.ContainsKey(nodeId1) || !_nodeMap.ContainsKey(nodeId2))
return false;
return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
}
/// <summary>
/// 네비게이션 가능한 노드 목록 반환
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetNavigationNodes()
{
return _nodeMap.Keys.ToList();
}
/// <summary>
/// 노드 정보 반환
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <returns>노드 정보 또는 null</returns>
public PathNode GetNode(string nodeId)
{
return _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
}
}
}

101
Cs_HMI/AGVNavigationCore/PathFinding/PathNode.cs Normal file
View File

@@ -0,0 +1,101 @@
using System;
using System.Drawing;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// A* 알고리즘에서 사용하는 경로 노드
/// </summary>
public class PathNode
{
/// <summary>
/// 노드 ID
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// 노드 위치
/// </summary>
public Point Position { get; set; }
/// <summary>
/// 시작점으로부터의 실제 거리 (G cost)
/// </summary>
public float GCost { get; set; }
/// <summary>
/// 목적지까지의 추정 거리 (H cost - 휴리스틱)
/// </summary>
public float HCost { get; set; }
/// <summary>
/// 총 비용 (F cost = G cost + H cost)
/// </summary>
public float FCost => GCost + HCost;
/// <summary>
/// 부모 노드 (경로 추적용)
/// </summary>
public PathNode Parent { get; set; }
/// <summary>
/// 연결된 노드 ID 목록
/// </summary>
public System.Collections.Generic.List<string> ConnectedNodes { get; set; }
/// <summary>
/// 생성자
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <param name="position">위치</param>
public PathNode(string nodeId, Point position)
{
NodeId = nodeId;
Position = position;
GCost = 0;
HCost = 0;
Parent = null;
ConnectedNodes = new System.Collections.Generic.List<string>();
}
/// <summary>
/// 다른 노드까지의 유클리드 거리 계산
/// </summary>
/// <param name="other">대상 노드</param>
/// <returns>거리</returns>
public float DistanceTo(PathNode other)
{
float dx = Position.X - other.Position.X;
float dy = Position.Y - other.Position.Y;
return (float)Math.Sqrt(dx * dx + dy * dy);
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
return $"{NodeId} - F:{FCost:F1} G:{GCost:F1} H:{HCost:F1}";
}
/// <summary>
/// 같음 비교 (NodeId 기준)
/// </summary>
public override bool Equals(object obj)
{
if (obj is PathNode other)
{
return NodeId == other.NodeId;
}
return false;
}
/// <summary>
/// 해시코드 (NodeId 기준)
/// </summary>
public override int GetHashCode()
{
return NodeId?.GetHashCode() ?? 0;
}
}
}

107
Cs_HMI/AGVNavigationCore/PathFinding/PathResult.cs Normal file
View File

@@ -0,0 +1,107 @@
using System;
using System.Collections.Generic;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// 경로 계산 결과
/// </summary>
public class PathResult
{
/// <summary>
/// 경로 찾기 성공 여부
/// </summary>
public bool Success { get; set; }
/// <summary>
/// 경로 노드 ID 목록 (시작 → 목적지 순서)
/// </summary>
public List<string> Path { get; set; }
/// <summary>
/// 총 거리
/// </summary>
public float TotalDistance { get; set; }
/// <summary>
/// 계산 소요 시간 (밀리초)
/// </summary>
public long CalculationTimeMs { get; set; }
/// <summary>
/// 탐색한 노드 수
/// </summary>
public int ExploredNodeCount { get; set; }
/// <summary>
/// 오류 메시지 (실패시)
/// </summary>
public string ErrorMessage { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public PathResult()
{
Success = false;
Path = new List<string>();
TotalDistance = 0;
CalculationTimeMs = 0;
ExploredNodeCount = 0;
ErrorMessage = string.Empty;
}
/// <summary>
/// 성공 결과 생성
/// </summary>
/// <param name="path">경로</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodeCount">탐색 노드 수</param>
/// <returns>성공 결과</returns>
public static PathResult CreateSuccess(List<string> path, float totalDistance, long calculationTimeMs, int exploredNodeCount)
{
return new PathResult
{
Success = true,
Path = new List<string>(path),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs,
ExploredNodeCount = exploredNodeCount
};
}
/// <summary>
/// 실패 결과 생성
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodeCount">탐색 노드 수</param>
/// <returns>실패 결과</returns>
public static PathResult CreateFailure(string errorMessage, long calculationTimeMs, int exploredNodeCount)
{
return new PathResult
{
Success = false,
ErrorMessage = errorMessage,
CalculationTimeMs = calculationTimeMs,
ExploredNodeCount = exploredNodeCount
};
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
if (Success)
{
return $"Success: {Path.Count} nodes, {TotalDistance:F1}px, {CalculationTimeMs}ms";
}
else
{
return $"Failed: {ErrorMessage}, {CalculationTimeMs}ms";
}
}
}
}

275
Cs_HMI/AGVNavigationCore/PathFinding/RfidBasedPathfinder.cs Normal file
View File

@@ -0,0 +1,275 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// RFID 기반 AGV 경로 탐색기
/// 실제 현장에서 AGV가 RFID를 읽어서 위치를 파악하는 방식에 맞춤
/// </summary>
public class RfidBasedPathfinder
{
private AGVPathfinder _agvPathfinder;
private AStarPathfinder _astarPathfinder;
private Dictionary<string, string> _rfidToNodeMap; // RFID -> NodeId
private Dictionary<string, string> _nodeToRfidMap; // NodeId -> RFID
private List<MapNode> _mapNodes;
/// <summary>
/// AGV 현재 방향
/// </summary>
public AgvDirection CurrentDirection
{
get => _agvPathfinder.CurrentDirection;
set => _agvPathfinder.CurrentDirection = value;
}
/// <summary>
/// 회전 비용 가중치
/// </summary>
public float RotationCostWeight
{
get => _agvPathfinder.RotationCostWeight;
set => _agvPathfinder.RotationCostWeight = value;
}
/// <summary>
/// 생성자
/// </summary>
public RfidBasedPathfinder()
{
_agvPathfinder = new AGVPathfinder();
_astarPathfinder = new AStarPathfinder();
_rfidToNodeMap = new Dictionary<string, string>();
_nodeToRfidMap = new Dictionary<string, string>();
_mapNodes = new List<MapNode>();
}
/// <summary>
/// 맵 노드 설정 (MapNode의 RFID 정보 직접 사용)
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
public void SetMapNodes(List<MapNode> mapNodes)
{
// 기존 pathfinder에 맵 노드 설정
_agvPathfinder.SetMapNodes(mapNodes);
_astarPathfinder.SetMapNodes(mapNodes);
// MapNode의 RFID 정보로 매핑 구성
_mapNodes = mapNodes ?? new List<MapNode>();
_rfidToNodeMap.Clear();
_nodeToRfidMap.Clear();
foreach (var node in _mapNodes.Where(n => n.IsActive && n.HasRfid()))
{
_rfidToNodeMap[node.RfidId] = node.NodeId;
_nodeToRfidMap[node.NodeId] = node.RfidId;
}
}
/// <summary>
/// RFID 기반 AGV 경로 계산
/// </summary>
/// <param name="startRfidId">시작 RFID</param>
/// <param name="endRfidId">목적지 RFID</param>
/// <param name="targetDirection">목적지 도착 방향</param>
/// <returns>RFID 기반 AGV 경로 계산 결과</returns>
public RfidPathResult FindAGVPath(string startRfidId, string endRfidId, AgvDirection? targetDirection = null)
{
try
{
// RFID를 NodeId로 변환
if (!_rfidToNodeMap.TryGetValue(startRfidId, out string startNodeId))
{
return RfidPathResult.CreateFailure($"시작 RFID를 찾을 수 없습니다: {startRfidId}", 0);
}
if (!_rfidToNodeMap.TryGetValue(endRfidId, out string endNodeId))
{
return RfidPathResult.CreateFailure($"목적지 RFID를 찾을 수 없습니다: {endRfidId}", 0);
}
// NodeId 기반으로 경로 계산
var nodeResult = _agvPathfinder.FindAGVPath(startNodeId, endNodeId, targetDirection);
// 결과를 RFID 기반으로 변환
return ConvertToRfidResult(nodeResult, startRfidId, endRfidId);
}
catch (Exception ex)
{
return RfidPathResult.CreateFailure($"RFID 기반 경로 계산 중 오류: {ex.Message}", 0);
}
}
/// <summary>
/// 가장 가까운 충전소로의 RFID 기반 경로 찾기
/// </summary>
/// <param name="startRfidId">시작 RFID</param>
/// <returns>RFID 기반 경로 계산 결과</returns>
public RfidPathResult FindPathToChargingStation(string startRfidId)
{
try
{
if (!_rfidToNodeMap.TryGetValue(startRfidId, out string startNodeId))
{
return RfidPathResult.CreateFailure($"시작 RFID를 찾을 수 없습니다: {startRfidId}", 0);
}
var nodeResult = _agvPathfinder.FindPathToChargingStation(startNodeId);
return ConvertToRfidResult(nodeResult, startRfidId, null);
}
catch (Exception ex)
{
return RfidPathResult.CreateFailure($"충전소 경로 계산 중 오류: {ex.Message}", 0);
}
}
/// <summary>
/// 특정 장비 타입의 도킹 스테이션으로의 RFID 기반 경로 찾기
/// </summary>
/// <param name="startRfidId">시작 RFID</param>
/// <param name="stationType">장비 타입</param>
/// <returns>RFID 기반 경로 계산 결과</returns>
public RfidPathResult FindPathToDockingStation(string startRfidId, StationType stationType)
{
try
{
if (!_rfidToNodeMap.TryGetValue(startRfidId, out string startNodeId))
{
return RfidPathResult.CreateFailure($"시작 RFID를 찾을 수 없습니다: {startRfidId}", 0);
}
var nodeResult = _agvPathfinder.FindPathToDockingStation(startNodeId, stationType);
return ConvertToRfidResult(nodeResult, startRfidId, null);
}
catch (Exception ex)
{
return RfidPathResult.CreateFailure($"도킹 스테이션 경로 계산 중 오류: {ex.Message}", 0);
}
}
/// <summary>
/// 여러 RFID 목적지 중 가장 가까운 곳으로의 경로 찾기
/// </summary>
/// <param name="startRfidId">시작 RFID</param>
/// <param name="targetRfidIds">목적지 후보 RFID 목록</param>
/// <returns>RFID 기반 경로 계산 결과</returns>
public RfidPathResult FindNearestPath(string startRfidId, List<string> targetRfidIds)
{
try
{
if (!_rfidToNodeMap.TryGetValue(startRfidId, out string startNodeId))
{
return RfidPathResult.CreateFailure($"시작 RFID를 찾을 수 없습니다: {startRfidId}", 0);
}
// RFID 목록을 NodeId 목록으로 변환
var targetNodeIds = new List<string>();
foreach (var rfidId in targetRfidIds)
{
if (_rfidToNodeMap.TryGetValue(rfidId, out string nodeId))
{
targetNodeIds.Add(nodeId);
}
}
if (targetNodeIds.Count == 0)
{
return RfidPathResult.CreateFailure("유효한 목적지 RFID가 없습니다", 0);
}
var pathResult = _astarPathfinder.FindNearestPath(startNodeId, targetNodeIds);
if (!pathResult.Success)
{
return RfidPathResult.CreateFailure(pathResult.ErrorMessage, pathResult.CalculationTimeMs);
}
// AGV 명령어 생성을 위해 AGV pathfinder 사용
var endNodeId = pathResult.Path.Last();
var agvResult = _agvPathfinder.FindAGVPath(startNodeId, endNodeId);
return ConvertToRfidResult(agvResult, startRfidId, null);
}
catch (Exception ex)
{
return RfidPathResult.CreateFailure($"최근접 경로 계산 중 오류: {ex.Message}", 0);
}
}
/// <summary>
/// RFID 매핑 상태 확인 (MapNode 기반)
/// </summary>
/// <param name="rfidId">확인할 RFID</param>
/// <returns>MapNode 또는 null</returns>
public MapNode GetRfidMapping(string rfidId)
{
return _mapNodes.FirstOrDefault(n => n.RfidId == rfidId && n.IsActive && n.HasRfid());
}
/// <summary>
/// RFID로 NodeId 조회
/// </summary>
/// <param name="rfidId">RFID</param>
/// <returns>NodeId 또는 null</returns>
public string GetNodeIdByRfid(string rfidId)
{
return _rfidToNodeMap.TryGetValue(rfidId, out string nodeId) ? nodeId : null;
}
/// <summary>
/// NodeId로 RFID 조회
/// </summary>
/// <param name="nodeId">NodeId</param>
/// <returns>RFID 또는 null</returns>
public string GetRfidByNodeId(string nodeId)
{
return _nodeToRfidMap.TryGetValue(nodeId, out string rfidId) ? rfidId : null;
}
/// <summary>
/// 활성화된 RFID 목록 반환
/// </summary>
/// <returns>활성화된 RFID 목록</returns>
public List<string> GetActiveRfidList()
{
return _mapNodes.Where(n => n.IsActive && n.HasRfid()).Select(n => n.RfidId).ToList();
}
/// <summary>
/// NodeId 기반 결과를 RFID 기반 결과로 변환
/// </summary>
private RfidPathResult ConvertToRfidResult(AGVPathResult nodeResult, string startRfidId, string endRfidId)
{
if (!nodeResult.Success)
{
return RfidPathResult.CreateFailure(nodeResult.ErrorMessage, nodeResult.CalculationTimeMs);
}
// NodeId 경로를 RFID 경로로 변환
var rfidPath = new List<string>();
foreach (var nodeId in nodeResult.Path)
{
if (_nodeToRfidMap.TryGetValue(nodeId, out string rfidId))
{
rfidPath.Add(rfidId);
}
else
{
// 매핑이 없는 경우 NodeId를 그대로 사용 (경고 로그 필요)
rfidPath.Add($"[{nodeId}]");
}
}
return RfidPathResult.CreateSuccess(
rfidPath,
nodeResult.Commands,
nodeResult.TotalDistance,
nodeResult.CalculationTimeMs,
nodeResult.EstimatedTimeSeconds,
nodeResult.RotationCount
);
}
}
}

229
Cs_HMI/AGVNavigationCore/PathFinding/RfidPathResult.cs Normal file
View File

@@ -0,0 +1,229 @@
using System;
using System.Collections.Generic;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// RFID 기반 AGV 경로 계산 결과
/// 실제 현장에서 AGV가 RFID를 기준으로 이동하는 방식에 맞춤
/// </summary>
public class RfidPathResult
{
/// <summary>
/// 경로 찾기 성공 여부
/// </summary>
public bool Success { get; set; }
/// <summary>
/// RFID 경로 목록 (시작 → 목적지 순서)
/// </summary>
public List<string> RfidPath { get; set; }
/// <summary>
/// AGV 명령어 목록 (이동 방향 시퀀스)
/// </summary>
public List<AgvDirection> Commands { get; set; }
/// <summary>
/// 총 거리
/// </summary>
public float TotalDistance { get; set; }
/// <summary>
/// 계산 소요 시간 (밀리초)
/// </summary>
public long CalculationTimeMs { get; set; }
/// <summary>
/// 예상 소요 시간 (초)
/// </summary>
public float EstimatedTimeSeconds { get; set; }
/// <summary>
/// 회전 횟수
/// </summary>
public int RotationCount { get; set; }
/// <summary>
/// 오류 메시지 (실패시)
/// </summary>
public string ErrorMessage { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public RfidPathResult()
{
Success = false;
RfidPath = new List<string>();
Commands = new List<AgvDirection>();
TotalDistance = 0;
CalculationTimeMs = 0;
EstimatedTimeSeconds = 0;
RotationCount = 0;
ErrorMessage = string.Empty;
}
/// <summary>
/// 성공 결과 생성
/// </summary>
/// <param name="rfidPath">RFID 경로</param>
/// <param name="commands">AGV 명령어 목록</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="estimatedTimeSeconds">예상 소요 시간</param>
/// <param name="rotationCount">회전 횟수</param>
/// <returns>성공 결과</returns>
public static RfidPathResult CreateSuccess(
List<string> rfidPath,
List<AgvDirection> commands,
float totalDistance,
long calculationTimeMs,
float estimatedTimeSeconds,
int rotationCount)
{
return new RfidPathResult
{
Success = true,
RfidPath = new List<string>(rfidPath),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs,
EstimatedTimeSeconds = estimatedTimeSeconds,
RotationCount = rotationCount
};
}
/// <summary>
/// 실패 결과 생성
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>실패 결과</returns>
public static RfidPathResult CreateFailure(string errorMessage, long calculationTimeMs)
{
return new RfidPathResult
{
Success = false,
ErrorMessage = errorMessage,
CalculationTimeMs = calculationTimeMs
};
}
/// <summary>
/// 명령어 요약 생성
/// </summary>
/// <returns>명령어 요약 문자열</returns>
public string GetCommandSummary()
{
if (!Success) return "실패";
var summary = new List<string>();
var currentCommand = AgvDirection.Stop;
var count = 0;
foreach (var command in Commands)
{
if (command == currentCommand)
{
count++;
}
else
{
if (count > 0)
{
summary.Add($"{GetCommandText(currentCommand)}×{count}");
}
currentCommand = command;
count = 1;
}
}
if (count > 0)
{
summary.Add($"{GetCommandText(currentCommand)}×{count}");
}
return string.Join(" → ", summary);
}
/// <summary>
/// 명령어 텍스트 반환
/// </summary>
private string GetCommandText(AgvDirection command)
{
switch (command)
{
case AgvDirection.Forward: return "전진";
case AgvDirection.Backward: return "후진";
case AgvDirection.Left: return "좌회전";
case AgvDirection.Right: return "우회전";
case AgvDirection.Stop: return "정지";
default: return command.ToString();
}
}
/// <summary>
/// RFID 경로 요약 생성
/// </summary>
/// <returns>RFID 경로 요약 문자열</returns>
public string GetRfidPathSummary()
{
if (!Success || RfidPath.Count == 0) return "경로 없음";
if (RfidPath.Count <= 3)
{
return string.Join(" → ", RfidPath);
}
else
{
return $"{RfidPath[0]} → ... ({RfidPath.Count - 2}개 경유) → {RfidPath[RfidPath.Count - 1]}";
}
}
/// <summary>
/// 상세 경로 정보 반환
/// </summary>
/// <returns>상세 정보 문자열</returns>
public string GetDetailedInfo()
{
if (!Success)
{
return $"RFID 경로 계산 실패: {ErrorMessage} (계산시간: {CalculationTimeMs}ms)";
}
return $"RFID 경로: {RfidPath.Count}개 지점, 거리: {TotalDistance:F1}px, " +
$"회전: {RotationCount}회, 예상시간: {EstimatedTimeSeconds:F1}초, " +
$"계산시간: {CalculationTimeMs}ms";
}
/// <summary>
/// AGV 운영자용 실행 정보 반환
/// </summary>
/// <returns>실행 정보 문자열</returns>
public string GetExecutionInfo()
{
if (!Success) return $"실행 불가: {ErrorMessage}";
return $"[실행준비] {GetRfidPathSummary()}\n" +
$"[명령어] {GetCommandSummary()}\n" +
$"[예상시간] {EstimatedTimeSeconds:F1}초";
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
if (Success)
{
return $"Success: {RfidPath.Count} RFIDs, {TotalDistance:F1}px, {RotationCount} rotations, {EstimatedTimeSeconds:F1}s";
}
else
{
return $"Failed: {ErrorMessage}";
}
}
}
}