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

refactor: PathFinding 폴더 구조 개선 및 도킹 에러 기능 추가

Browse Source 
- PathFinding 폴더를 Core, Validation, Planning, Analysis로 세분화
- 네임스페이스 정리 및 using 문 업데이트
- UnifiedAGVCanvas에 SetDockingError 메서드 추가
- 도킹 검증 시스템 인프라 구축
- DockingValidator 유틸리티 클래스 추가
- 빌드 오류 수정 및 안정성 개선

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

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
ChiKyun Kim
2025-09-16 14:46:53 +09:00
parent debbf712d4
commit ef72b77f1c
37 changed files with 1085 additions and 2796 deletions
Download Patch File Download Diff File Expand all files Collapse all files

862
Cs_HMI/AGVNavigationCore/PathFinding/AGVPathfinder.cs
View File

@@ -1,862 +0,0 @@
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 PathfindingOptions Options { get; set; } = PathfindingOptions.Default;
/// <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)
{
return FindAGVPath(startNodeId, endNodeId, targetDirection, Options);
}
/// <summary>
/// AGV 경로 계산 (현재 방향 및 옵션 지정 가능)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <param name="currentDirection">현재 AGV 방향</param>
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
/// <param name="options">경로 탐색 옵션</param>
/// <returns>AGV 경로 계산 결과</returns>
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? currentDirection, AgvDirection? targetDirection, PathfindingOptions options)
{
var result = FindAGVPath(startNodeId, endNodeId, targetDirection, options);
if (!result.Success || currentDirection == null || result.Commands.Count == 0)
return result;
// 경로의 첫 번째 방향과 현재 방향 비교
var firstDirection = result.Commands[0];
if (RequiresDirectionChange(currentDirection.Value, firstDirection))
{
return InsertDirectionChangeCommands(result, currentDirection.Value, firstDirection);
}
return result;
}
/// <summary>
/// AGV 경로 계산 (옵션 지정 가능)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
/// <param name="options">경로 탐색 옵션</param>
/// <returns>AGV 경로 계산 결과</returns>
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options)
{
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, options, stopwatch);
}
else
{
return FindNormalPath(startNodeId, endNodeId, targetDirection, options, 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, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
{
PathResult result;
// 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
if (options.AvoidRotationNodes)
{
result = FindPathAvoidingRotation(startNodeId, endNodeId, options);
// 회전 회피 경로를 찾지 못하면 일반 경로 계산
if (!result.Success)
{
result = _pathfinder.FindPath(startNodeId, endNodeId);
}
}
else
{
result = _pathfinder.FindPath(startNodeId, endNodeId);
}
if (!result.Success)
{
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
}
var agvCommands = GenerateAGVCommands(result.Path, targetDirection ?? AgvDirection.Forward);
var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
}
/// <summary>
/// 특수 노드(도킹/충전)로의 경로 계산
/// </summary>
private AGVPathResult FindPathToSpecialNode(string startNodeId, MapNode endNode, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
{
var requiredDirection = GetRequiredDirectionForNode(endNode);
var actualTargetDirection = targetDirection ?? requiredDirection;
PathResult result;
// 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
if (options.AvoidRotationNodes)
{
result = FindPathAvoidingRotation(startNodeId, endNode.NodeId, options);
// 회전 회피 경로를 찾지 못하면 일반 경로 계산
if (!result.Success)
{
result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
}
}
else
{
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);
var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, 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>
private List<NodeMotorInfo> GenerateNodeMotorInfos(List<string> path)
{
var nodeMotorInfos = new List<NodeMotorInfo>();
if (path.Count < 2) return nodeMotorInfos;
// 전체 경로에 대한 방향 전환 계획 수립
var directionPlan = PlanDirectionChanges(path);
for (int i = 0; i < path.Count; i++)
{
var currentNodeId = path[i];
string nextNodeId = i < path.Count - 1 ? path[i + 1] : null;
// 계획된 방향 사용
var motorDirection = directionPlan.ContainsKey(currentNodeId)
? directionPlan[currentNodeId]
: AgvDirection.Forward;
// 노드 특성 정보 수집
bool canRotate = false;
bool isDirectionChangePoint = false;
bool requiresSpecialAction = false;
string specialActionDescription = "";
if (_nodeMap.ContainsKey(currentNodeId))
{
var currentNode = _nodeMap[currentNodeId];
canRotate = currentNode.CanRotate;
// 방향 전환 감지
if (i > 0 && directionPlan.ContainsKey(path[i - 1]))
{
var prevDirection = directionPlan[path[i - 1]];
isDirectionChangePoint = prevDirection != motorDirection;
}
// 특수 동작 필요 여부 감지
if (!canRotate && isDirectionChangePoint)
{
requiresSpecialAction = true;
specialActionDescription = "갈림길 전진/후진 반복";
}
else if (canRotate && isDirectionChangePoint)
{
specialActionDescription = "회전 노드 방향전환";
}
}
var nodeMotorInfo = new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId,
canRotate, isDirectionChangePoint, MagnetDirection.Straight, requiresSpecialAction, specialActionDescription);
nodeMotorInfos.Add(nodeMotorInfo);
}
return nodeMotorInfos;
}
/// <summary>
/// 경로 전체에 대한 방향 전환 계획 수립
/// </summary>
/// <param name="path">경로 노드 목록</param>
/// <returns>노드별 모터 방향 계획</returns>
private Dictionary<string, AgvDirection> PlanDirectionChanges(List<string> path)
{
var directionPlan = new Dictionary<string, AgvDirection>();
if (path.Count < 2) return directionPlan;
// 1단계: 목적지 노드의 요구사항 분석
var targetNodeId = path[path.Count - 1];
var targetDirection = AgvDirection.Forward;
if (_nodeMap.ContainsKey(targetNodeId))
{
var targetNode = _nodeMap[targetNodeId];
targetDirection = GetRequiredDirectionForNode(targetNode);
}
// 2단계: 역방향으로 방향 전환점 찾기
var currentDirection = targetDirection;
directionPlan[targetNodeId] = currentDirection;
// 마지막에서 두 번째부터 역순으로 처리
for (int i = path.Count - 2; i >= 0; i--)
{
var currentNodeId = path[i];
var nextNodeId = path[i + 1];
if (_nodeMap.ContainsKey(currentNodeId))
{
var currentNode = _nodeMap[currentNodeId];
// 방법1: 회전 가능 노드에서 방향 전환
if (currentNode.CanRotate && NeedDirectionChange(currentNodeId, nextNodeId, currentDirection))
{
// 회전 가능한 노드에서 방향 전환 수행
var optimalDirection = CalculateOptimalDirection(currentNodeId, nextNodeId, path, i);
directionPlan[currentNodeId] = optimalDirection;
currentDirection = optimalDirection;
}
else
{
// 일반 노드: 연속성 유지
directionPlan[currentNodeId] = currentDirection;
}
}
else
{
directionPlan[currentNodeId] = currentDirection;
}
}
// 3단계: 방법2 적용 - 불가능한 방향 전환 감지 및 수정
ApplyDirectionChangeCorrection(path, directionPlan);
return directionPlan;
}
/// <summary>
/// 방향 전환이 필요한지 판단
/// </summary>
private bool NeedDirectionChange(string currentNodeId, string nextNodeId, AgvDirection currentDirection)
{
if (!_nodeMap.ContainsKey(nextNodeId)) return false;
var nextNode = _nodeMap[nextNodeId];
var requiredDirection = GetRequiredDirectionForNode(nextNode);
return currentDirection != requiredDirection;
}
/// <summary>
/// 회전 가능 노드에서 최적 방향 계산
/// </summary>
private AgvDirection CalculateOptimalDirection(string nodeId, string nextNodeId, List<string> path, int nodeIndex)
{
if (!_nodeMap.ContainsKey(nextNodeId)) return AgvDirection.Forward;
var nextNode = _nodeMap[nextNodeId];
// 목적지까지의 경로를 고려한 최적 방향 결정
if (nextNode.Type == NodeType.Charging)
{
return AgvDirection.Forward; // 충전기는 전진 접근
}
else if (nextNode.Type == NodeType.Docking)
{
return AgvDirection.Backward; // 도킹은 후진 접근
}
else
{
// 경로 패턴 분석을 통한 방향 결정
return AnalyzePathPattern(path, nodeIndex);
}
}
/// <summary>
/// 경로 패턴 분석을 통한 방향 결정
/// </summary>
private AgvDirection AnalyzePathPattern(List<string> path, int startIndex)
{
// 남은 경로에서 도킹/충전 스테이션이 있는지 확인
for (int i = startIndex + 1; i < path.Count; i++)
{
if (_nodeMap.ContainsKey(path[i]))
{
var node = _nodeMap[path[i]];
if (node.Type == NodeType.Docking)
{
return AgvDirection.Backward; // 도킹 준비를 위해 후진 방향
}
else if (node.Type == NodeType.Charging)
{
return AgvDirection.Forward; // 충전 준비를 위해 전진 방향
}
}
}
return AgvDirection.Forward; // 기본값
}
/// <summary>
/// 방법2: 불가능한 방향 전환 감지 및 보정 (갈림길 전진/후진 반복)
/// </summary>
private void ApplyDirectionChangeCorrection(List<string> path, Dictionary<string, AgvDirection> directionPlan)
{
for (int i = 0; i < path.Count - 1; i++)
{
var currentNodeId = path[i];
var nextNodeId = path[i + 1];
if (directionPlan.ContainsKey(currentNodeId) && directionPlan.ContainsKey(nextNodeId))
{
var currentDir = directionPlan[currentNodeId];
var nextDir = directionPlan[nextNodeId];
// 급격한 방향 전환 감지 (전진→후진, 후진→전진)
if (IsImpossibleDirectionChange(currentDir, nextDir))
{
var currentNode = _nodeMap.ContainsKey(currentNodeId) ? _nodeMap[currentNodeId] : null;
if (currentNode != null && !currentNode.CanRotate)
{
// 회전 불가능한 노드에서 방향 전환 시도 → 특수 동작 추가
AddTurnAroundSequence(currentNodeId, directionPlan);
}
}
}
}
}
/// <summary>
/// 불가능한 방향 전환인지 확인
/// </summary>
private bool IsImpossibleDirectionChange(AgvDirection current, AgvDirection next)
{
return (current == AgvDirection.Forward && next == AgvDirection.Backward) ||
(current == AgvDirection.Backward && next == AgvDirection.Forward);
}
/// <summary>
/// 회전 불가능한 노드에서 방향 전환을 위한 특수 동작 시퀀스 추가
/// </summary>
private void AddTurnAroundSequence(string nodeId, Dictionary<string, AgvDirection> directionPlan)
{
// 갈림길에서 전진/후진 반복 작업으로 방향 변경
// 실제 구현시에는 NodeMotorInfo에 특수 플래그나 추가 동작 정보 포함 필요
// 현재는 안전한 방향으로 보정
if (directionPlan.ContainsKey(nodeId))
{
// 일단 연속성을 유지하도록 보정 (실제로는 특수 회전 동작 필요)
directionPlan[nodeId] = AgvDirection.Forward;
}
}
/// <summary>
/// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산 (레거시 - 새로운 PlanDirectionChanges 사용)
/// </summary>
/// <param name="currentNodeId">현재 노드 ID</param>
/// <param name="nextNodeId">다음 노드 ID</param>
/// <returns>모터방향</returns>
private AgvDirection CalculateMotorDirection(string currentNodeId, string nextNodeId)
{
if (!_nodeMap.ContainsKey(currentNodeId) || !_nodeMap.ContainsKey(nextNodeId))
{
return AgvDirection.Forward;
}
var nextNode = _nodeMap[nextNodeId];
// 다음 노드가 특수 노드인지 확인
if (nextNode.Type == NodeType.Charging)
{
// 충전기: 전진으로 도킹
return AgvDirection.Forward;
}
else if (nextNode.Type == NodeType.Docking)
{
// 도킹 스테이션: 후진으로 도킹
return AgvDirection.Backward;
}
else
{
// 일반 이동: 기본적으로 전진 (실제로는 PlanDirectionChanges에서 결정됨)
return AgvDirection.Forward;
}
}
/// <summary>
/// 회전 노드를 회피하는 경로 탐색
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <param name="options">경로 탐색 옵션</param>
/// <returns>회전 회피 경로 결과</returns>
private PathResult FindPathAvoidingRotation(string startNodeId, string endNodeId, PathfindingOptions options)
{
try
{
// 회전 가능한 노드들을 필터링하여 임시로 비활성화
var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate).Select(n => n.NodeId).ToList();
var originalConnections = new Dictionary<string, List<string>>();
// 시작과 끝 노드가 회전 노드인 경우는 제외
var nodesToDisable = rotationNodes.Where(nodeId => nodeId != startNodeId && nodeId != endNodeId).ToList();
foreach (var nodeId in nodesToDisable)
{
// 임시로 연결 해제 (실제로는 pathfinder에서 해당 노드를 높은 비용으로 설정해야 함)
originalConnections[nodeId] = _nodeMap[nodeId].ConnectedNodes.ToList();
}
// 회전 노드 회피 경로 계산 시도
var result = _pathfinder.FindPath(startNodeId, endNodeId);
// 결과에서 회전 노드가 포함되어 있으면 실패로 간주
if (result.Success && result.Path != null)
{
var pathContainsRotation = result.Path.Any(nodeId => nodesToDisable.Contains(nodeId));
if (pathContainsRotation)
{
return PathResult.CreateFailure("회전 노드를 회피하는 경로를 찾을 수 없습니다.", 0, 0);
}
}
return result;
}
catch (Exception ex)
{
return PathResult.CreateFailure($"회전 회피 경로 계산 중 오류: {ex.Message}", 0, 0);
}
}
/// <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;
}
/// <summary>
/// 현재 방향에서 목표 방향으로 변경하기 위해 방향 전환이 필요한지 판단
/// </summary>
/// <param name="currentDirection">현재 방향</param>
/// <param name="targetDirection">목표 방향</param>
/// <returns>방향 전환 필요 여부</returns>
private bool RequiresDirectionChange(AgvDirection currentDirection, AgvDirection targetDirection)
{
// 같은 방향이면 변경 불필요
if (currentDirection == targetDirection)
return false;
// 전진 <-> 후진 전환은 방향 전환 필요
return (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward) ||
(currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward);
}
/// <summary>
/// 방향 전환을 위한 명령어를 기존 경로에 삽입
/// </summary>
/// <param name="originalResult">원래 경로 결과</param>
/// <param name="currentDirection">현재 AGV 방향</param>
/// <param name="targetDirection">목표 방향</param>
/// <returns>방향 전환 명령이 포함된 경로 결과</returns>
private AGVPathResult InsertDirectionChangeCommands(AGVPathResult originalResult, AgvDirection currentDirection, AgvDirection targetDirection)
{
if (originalResult.Path.Count < 1)
return originalResult;
// 시작 노드를 찾아서 회전 가능한지 확인
var startNodeId = originalResult.Path[0];
if (!_nodeMap.ContainsKey(startNodeId))
return originalResult;
var startNode = _nodeMap[startNodeId];
// 방향 전환을 위한 가장 가까운 교차로 찾기
var targetNodeId = originalResult.Path.LastOrDefault();
var junctionNode = FindNearestJunctionForDirectionChange(startNodeId, targetNodeId);
if (junctionNode == null)
{
return AGVPathResult.CreateFailure("방향 전환을 위한 교차로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 교차로로의 경로 추가
var pathToJunction = _pathfinder.FindPath(startNodeId, junctionNode.NodeId);
if (!pathToJunction.Success)
{
return AGVPathResult.CreateFailure("방향 전환 교차로로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 교차로에서 원래 목적지로의 경로 계산
var pathFromJunction = _pathfinder.FindPath(junctionNode.NodeId, originalResult.Path.Last());
if (!pathFromJunction.Success)
{
return AGVPathResult.CreateFailure("방향 전환 후 목적지로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 전체 경로 조합
var combinedPath = new List<string>();
combinedPath.AddRange(pathToJunction.Path);
combinedPath.AddRange(pathFromJunction.Path.Skip(1)); // 중복 노드 제거
var combinedDistance = pathToJunction.TotalDistance + pathFromJunction.TotalDistance;
var combinedCommands = GenerateAGVCommandsWithDirectionChange(combinedPath, currentDirection, targetDirection, junctionNode.NodeId);
var nodeMotorInfos = GenerateNodeMotorInfos(combinedPath);
return AGVPathResult.CreateSuccess(combinedPath, combinedCommands, nodeMotorInfos, combinedDistance, originalResult.CalculationTimeMs);
}
/// <summary>
/// 방향 전환을 위한 가장 가까운 교차로 찾기
/// </summary>
/// <param name="fromNodeId">시작 노드 ID</param>
/// <param name="targetNodeId">목적지 노드 ID (경로상 교차로 우선 검색용)</param>
/// <returns>가장 가까운 교차로 노드</returns>
private MapNode FindNearestJunctionForDirectionChange(string fromNodeId, string targetNodeId = null)
{
// 1단계: 시작점에서 목적지까지 직접 경로상의 교차로 우선 검색
if (!string.IsNullOrEmpty(targetNodeId))
{
var directPath = _pathfinder.FindPath(fromNodeId, targetNodeId);
if (directPath.Success)
{
foreach (var nodeId in directPath.Path)
{
var node = _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
if (node != null && IsJunction(node))
{
return node;
}
}
}
}
// 2단계: 경로상에 교차로가 없으면 거리가 가장 가까운 교차로 찾기
var junctionNodes = _nodeMap.Values.Where(n => n.IsActive && IsJunction(n)).ToList();
MapNode nearestNode = null;
var shortestDistance = float.MaxValue;
foreach (var junctionNode in junctionNodes)
{
var pathResult = _pathfinder.FindPath(fromNodeId, junctionNode.NodeId);
if (pathResult.Success && pathResult.TotalDistance < shortestDistance)
{
shortestDistance = pathResult.TotalDistance;
nearestNode = junctionNode;
}
}
return nearestNode;
}
/// <summary>
/// 노드가 교차로인지 판단 (3개 이상 연결된 노드)
/// </summary>
/// <param name="node">검사할 노드</param>
/// <returns>교차로이면 true</returns>
private bool IsJunction(MapNode node)
{
// 연결된 노드 수 계산 (단방향 + 양방향)
var connectedCount = node.ConnectedNodes.Count;
// 역방향 연결도 고려
foreach (var otherNode in _nodeMap.Values)
{
if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
{
connectedCount++;
}
}
// 3개 이상 연결되어 있으면 교차로
return connectedCount >= 3;
}
/// <summary>
/// 방향 전환을 포함한 AGV 명령어 생성
/// </summary>
/// <param name="path">경로</param>
/// <param name="currentDirection">현재 방향</param>
/// <param name="targetDirection">목표 방향</param>
/// <param name="rotationNodeId">방향 전환 노드 ID</param>
/// <returns>AGV 명령어 목록</returns>
private List<AgvDirection> GenerateAGVCommandsWithDirectionChange(List<string> path, AgvDirection currentDirection, AgvDirection targetDirection, string rotationNodeId)
{
var commands = new List<AgvDirection>();
int rotationIndex = path.IndexOf(rotationNodeId);
// 회전 노드까지는 현재 방향으로 이동
for (int i = 0; i < rotationIndex; i++)
{
commands.Add(currentDirection);
}
// 회전 명령 추가 (전진->후진 또는 후진->전진)
if (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward)
{
commands.Add(AgvDirection.Left);
commands.Add(AgvDirection.Left); // 180도 회전
}
else if (currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward)
{
commands.Add(AgvDirection.Right);
commands.Add(AgvDirection.Right); // 180도 회전
}
// 회전 노드부터 목적지까지는 목표 방향으로 이동
for (int i = rotationIndex; i < path.Count - 1; i++)
{
commands.Add(targetDirection);
}
return commands;
}
}
}

3
Cs_HMI/AGVNavigationCore/PathFinding/JunctionAnalyzer.cs → Cs_HMI/AGVNavigationCore/PathFinding/Analysis/JunctionAnalyzer.cs
View File

@@ -3,8 +3,9 @@ using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Analysis
{
/// <summary>
/// AGV 갈림길 분석 및 마그넷 센서 방향 계산 시스템

111
Cs_HMI/AGVNavigationCore/PathFinding/AGVPathResult.cs → Cs_HMI/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
View File

@@ -2,8 +2,10 @@ using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// AGV 경로 계산 결과 (방향성 및 명령어 포함)
@@ -43,7 +45,16 @@ namespace AGVNavigationCore.PathFinding
/// <summary>
/// 탐색된 노드 수
/// </summary>
public int ExploredNodes { get; set; }
public int ExploredNodeCount { get; set; }
/// <summary>
/// 탐색된 노드 수 (호환성용)
/// </summary>
public int ExploredNodes
{
get => ExploredNodeCount;
set => ExploredNodeCount = value;
}
/// <summary>
/// 예상 소요 시간 (초)
@@ -60,6 +71,31 @@ namespace AGVNavigationCore.PathFinding
/// </summary>
public string ErrorMessage { get; set; }
/// <summary>
/// 도킹 검증 결과
/// </summary>
public DockingValidationResult DockingValidation { get; set; }
/// <summary>
/// 상세 경로 정보 (NodeMotorInfo 목록)
/// </summary>
public List<NodeMotorInfo> DetailedPath { get; set; }
/// <summary>
/// 계획 설명
/// </summary>
public string PlanDescription { get; set; }
/// <summary>
/// 방향 전환 필요 여부
/// </summary>
public bool RequiredDirectionChange { get; set; }
/// <summary>
/// 방향 전환 노드 ID
/// </summary>
public string DirectionChangeNode { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
@@ -69,12 +105,17 @@ namespace AGVNavigationCore.PathFinding
Path = new List<string>();
Commands = new List<AgvDirection>();
NodeMotorInfos = new List<NodeMotorInfo>();
DetailedPath = new List<NodeMotorInfo>();
TotalDistance = 0;
CalculationTimeMs = 0;
ExploredNodes = 0;
EstimatedTimeSeconds = 0;
RotationCount = 0;
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
RequiredDirectionChange = false;
DirectionChangeNode = string.Empty;
DockingValidation = DockingValidationResult.CreateNotRequired();
}
/// <summary>
@@ -141,6 +182,56 @@ namespace AGVNavigationCore.PathFinding
};
}
/// <summary>
/// 실패 결과 생성 (확장)
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <returns>실패 결과</returns>
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs, int exploredNodes)
{
return new AGVPathResult
{
Success = false,
ErrorMessage = errorMessage,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes
};
}
/// <summary>
/// 성공 결과 생성 (상세 경로용)
/// </summary>
/// <param name="detailedPath">상세 경로</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <param name="planDescription">계획 설명</param>
/// <param name="directionChange">방향 전환 여부</param>
/// <param name="changeNode">방향 전환 노드</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<NodeMotorInfo> detailedPath, float totalDistance, long calculationTimeMs, int exploredNodes, string planDescription, bool directionChange = false, string changeNode = null)
{
var path = detailedPath?.Select(n => n.NodeId).ToList() ?? new List<string>();
var result = new AGVPathResult
{
Success = true,
Path = path,
DetailedPath = detailedPath ?? new List<NodeMotorInfo>(),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes,
PlanDescription = planDescription ?? string.Empty,
RequiredDirectionChange = directionChange,
DirectionChangeNode = changeNode ?? string.Empty
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 경로 메트릭 계산
/// </summary>
@@ -248,20 +339,18 @@ namespace AGVNavigationCore.PathFinding
$"계산시간: {CalculationTimeMs}ms";
}
/// <summary>
/// PathResult로 변환 (호환성을 위해)
/// 단순 경로 목록 반환 (호환성용)
/// </summary>
/// <returns>PathResult 객체</returns>
public PathResult ToPathResult()
/// <returns>노드 ID 목록</returns>
public List<string> GetSimplePath()
{
if (Success)
if (DetailedPath != null && DetailedPath.Count > 0)
{
return PathResult.CreateSuccess(Path, TotalDistance, CalculationTimeMs, 0);
}
else
{
return PathResult.CreateFailure(ErrorMessage, CalculationTimeMs, 0);
return DetailedPath.Select(n => n.NodeId).ToList();
}
return Path ?? new List<string>();
}
/// <summary>

25
Cs_HMI/AGVNavigationCore/PathFinding/AStarPathfinder.cs → Cs_HMI/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
View File

@@ -4,7 +4,7 @@ using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// A* 알고리즘 기반 경로 탐색기
@@ -87,7 +87,7 @@ namespace AGVNavigationCore.PathFinding
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public PathResult FindPath(string startNodeId, string endNodeId)
public AGVPathResult FindPath(string startNodeId, string endNodeId)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
@@ -95,17 +95,18 @@ namespace AGVNavigationCore.PathFinding
{
if (!_nodeMap.ContainsKey(startNodeId))
{
return PathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(endNodeId))
{
return PathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (startNodeId == endNodeId)
{
return PathResult.CreateSuccess(new List<string> { startNodeId }, 0, stopwatch.ElapsedMilliseconds, 1);
var singlePath = new List<string> { startNodeId };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
var startNode = _nodeMap[startNodeId];
@@ -131,7 +132,7 @@ namespace AGVNavigationCore.PathFinding
{
var path = ReconstructPath(currentNode);
var totalDistance = CalculatePathDistance(path);
return PathResult.CreateSuccess(path, totalDistance, stopwatch.ElapsedMilliseconds, exploredCount);
return AGVPathResult.CreateSuccess(path, new List<AgvDirection>(), totalDistance, stopwatch.ElapsedMilliseconds);
}
foreach (var neighborId in currentNode.ConnectedNodes)
@@ -157,11 +158,11 @@ namespace AGVNavigationCore.PathFinding
}
}
return PathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
return AGVPathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
}
catch (Exception ex)
{
return PathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
@@ -171,14 +172,14 @@ namespace AGVNavigationCore.PathFinding
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
/// <returns>경로 계산 결과</returns>
public PathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
{
if (targetNodeIds == null || targetNodeIds.Count == 0)
{
return PathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
}
PathResult bestResult = null;
AGVPathResult bestResult = null;
foreach (var targetId in targetNodeIds)
{
var result = FindPath(startNodeId, targetId);
@@ -188,7 +189,7 @@ namespace AGVNavigationCore.PathFinding
}
}
return bestResult ?? PathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
return bestResult ?? AGVPathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
}
/// <summary>

2
Cs_HMI/AGVNavigationCore/PathFinding/PathNode.cs → Cs_HMI/AGVNavigationCore/PathFinding/Core/PathNode.cs
View File

@@ -1,7 +1,7 @@
using System;
using System.Drawing;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// A* 알고리즘에서 사용하는 경로 노드

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

@@ -1,107 +0,0 @@
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";
}
}
}
}

67
Cs_HMI/AGVNavigationCore/PathFinding/PathfindingOptions.cs
View File

@@ -1,67 +0,0 @@
using System;
namespace AGVNavigationCore.PathFinding
{
/// <summary>
/// 경로 탐색 옵션 설정
/// </summary>
public class PathfindingOptions
{
/// <summary>
/// 회전 가능 노드 회피 여부 (기본값: false - 회전 허용)
/// </summary>
public bool AvoidRotationNodes { get; set; } = false;
/// <summary>
/// 회전 회피 시 추가 비용 가중치 (회전 노드를 완전 차단하지 않고 높은 비용으로 설정)
/// </summary>
public float RotationAvoidanceCost { get; set; } = 1000.0f;
/// <summary>
/// 회전 비용 가중치 (기존 회전 비용)
/// </summary>
public float RotationCostWeight { get; set; } = 50.0f;
/// <summary>
/// 도킹 접근 거리
/// </summary>
public float DockingApproachDistance { get; set; } = 100.0f;
/// <summary>
/// 기본 옵션 생성
/// </summary>
public static PathfindingOptions Default => new PathfindingOptions();
/// <summary>
/// 회전 회피 옵션 생성
/// </summary>
public static PathfindingOptions AvoidRotation => new PathfindingOptions
{
AvoidRotationNodes = true
};
/// <summary>
/// 옵션 복사
/// </summary>
public PathfindingOptions Clone()
{
return new PathfindingOptions
{
AvoidRotationNodes = this.AvoidRotationNodes,
RotationAvoidanceCost = this.RotationAvoidanceCost,
RotationCostWeight = this.RotationCostWeight,
DockingApproachDistance = this.DockingApproachDistance
};
}
/// <summary>
/// 설정 정보 문자열
/// </summary>
public override string ToString()
{
return $"회전회피: {(AvoidRotationNodes ? "ON" : "OFF")}, " +
$"회전비용: {RotationCostWeight}, " +
$"회피비용: {RotationAvoidanceCost}";
}
}
}

99
Cs_HMI/AGVNavigationCore/PathFinding/AdvancedAGVPathfinder.cs → Cs_HMI/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -2,78 +2,25 @@ using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// 고급 AGV 경로 계획기
/// AGV 경로 계획기
/// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
/// </summary>
public class AdvancedAGVPathfinder
public class AGVPathfinder
{
/// <summary>
/// 고급 AGV 경로 계산 결과
/// </summary>
public class AdvancedPathResult
{
public bool Success { get; set; }
public List<NodeMotorInfo> DetailedPath { get; set; }
public float TotalDistance { get; set; }
public long CalculationTimeMs { get; set; }
public int ExploredNodeCount { get; set; }
public string ErrorMessage { get; set; }
public string PlanDescription { get; set; }
public bool RequiredDirectionChange { get; set; }
public string DirectionChangeNode { get; set; }
public AdvancedPathResult()
{
DetailedPath = new List<NodeMotorInfo>();
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
}
public static AdvancedPathResult CreateSuccess(List<NodeMotorInfo> path, float distance, long time, int explored, string description, bool directionChange = false, string changeNode = null)
{
return new AdvancedPathResult
{
Success = true,
DetailedPath = path,
TotalDistance = distance,
CalculationTimeMs = time,
ExploredNodeCount = explored,
PlanDescription = description,
RequiredDirectionChange = directionChange,
DirectionChangeNode = changeNode
};
}
public static AdvancedPathResult CreateFailure(string error, long time, int explored)
{
return new AdvancedPathResult
{
Success = false,
ErrorMessage = error,
CalculationTimeMs = time,
ExploredNodeCount = explored
};
}
/// <summary>
/// 단순 경로 목록 반환 (호환성용)
/// </summary>
public List<string> GetSimplePath()
{
return DetailedPath.Select(n => n.NodeId).ToList();
}
}
private readonly List<MapNode> _mapNodes;
private readonly AStarPathfinder _basicPathfinder;
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly DirectionChangePlanner _directionChangePlanner;
public AdvancedAGVPathfinder(List<MapNode> mapNodes)
public AGVPathfinder(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_basicPathfinder = new AStarPathfinder();
@@ -83,9 +30,9 @@ namespace AGVNavigationCore.PathFinding
}
/// <summary>
/// 고급 AGV 경로 계산
/// AGV 경로 계산
/// </summary>
public AdvancedPathResult FindAdvancedPath(string startNodeId, string targetNodeId, AgvDirection currentDirection = AgvDirection.Forward)
public AGVPathResult FindPath(string startNodeId, string targetNodeId, AgvDirection currentDirection = AgvDirection.Forward)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
@@ -97,7 +44,7 @@ namespace AGVNavigationCore.PathFinding
// 2. 방향 전환이 필요한지 확인
bool needDirectionChange = (currentDirection != requiredDirection);
AdvancedPathResult result;
AGVPathResult result;
if (needDirectionChange)
{
// 방향 전환이 필요한 경우
@@ -110,30 +57,37 @@ namespace AGVNavigationCore.PathFinding
}
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
// 도킹 검증 수행
if (result.Success && _mapNodes != null)
{
result.DockingValidation = DockingValidator.ValidateDockingDirection(result, _mapNodes, currentDirection);
}
return result;
}
catch (Exception ex)
{
return AdvancedPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
/// <summary>
/// 직접 경로 계획
/// </summary>
private AdvancedPathResult PlanDirectPath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
private AGVPathResult PlanDirectPath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
{
var basicResult = _basicPathfinder.FindPath(startNodeId, targetNodeId);
if (!basicResult.Success)
{
return AdvancedPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
}
// 기본 경로를 상세 경로로 변환
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
return AdvancedPathResult.CreateSuccess(
return AGVPathResult.CreateSuccess(
detailedPath,
basicResult.TotalDistance,
basicResult.CalculationTimeMs,
@@ -145,13 +99,13 @@ namespace AGVNavigationCore.PathFinding
/// <summary>
/// 방향 전환을 포함한 경로 계획
/// </summary>
private AdvancedPathResult PlanPathWithDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
private AGVPathResult PlanPathWithDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);
if (!directionChangePlan.Success)
{
return AdvancedPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
}
// 방향 전환 경로를 상세 경로로 변환
@@ -160,7 +114,7 @@ namespace AGVNavigationCore.PathFinding
// 거리 계산
float totalDistance = CalculatePathDistance(detailedPath);
return AdvancedPathResult.CreateSuccess(
return AGVPathResult.CreateSuccess(
detailedPath,
totalDistance,
0,
@@ -347,7 +301,7 @@ namespace AGVNavigationCore.PathFinding
/// <summary>
/// 경로 최적화
/// </summary>
public AdvancedPathResult OptimizePath(AdvancedPathResult originalResult)
public AGVPathResult OptimizePath(AGVPathResult originalResult)
{
if (!originalResult.Success)
return originalResult;
@@ -363,7 +317,7 @@ namespace AGVNavigationCore.PathFinding
/// <summary>
/// 디버깅용 경로 정보
/// </summary>
public string GetPathSummary(AdvancedPathResult result)
public string GetPathSummary(AGVPathResult result)
{
if (!result.Success)
return $"경로 계산 실패: {result.ErrorMessage}";
@@ -387,5 +341,6 @@ namespace AGVNavigationCore.PathFinding
return string.Join("\n", summary);
}
}
}

4
Cs_HMI/AGVNavigationCore/PathFinding/DirectionChangePlanner.cs → Cs_HMI/AGVNavigationCore/PathFinding/Planning/DirectionChangePlanner.cs
View File

@@ -2,8 +2,10 @@ using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 방향 전환 경로 계획 시스템

2
Cs_HMI/AGVNavigationCore/PathFinding/NodeMotorInfo.cs → Cs_HMI/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs
View File

@@ -1,6 +1,6 @@
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 마그넷 센서 방향 제어

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

@@ -1,275 +0,0 @@
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
View File

@@ -1,229 +0,0 @@
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}";
}
}
}
}

103
Cs_HMI/AGVNavigationCore/PathFinding/Validation/DockingValidationResult.cs Normal file
View File

@@ -0,0 +1,103 @@
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Validation
{
/// <summary>
/// 도킹 검증 결과
/// </summary>
public class DockingValidationResult
{
/// <summary>
/// 도킹 검증이 필요한지 여부 (목적지가 도킹 대상인 경우)
/// </summary>
public bool IsValidationRequired { get; set; }
/// <summary>
/// 도킹 검증 통과 여부
/// </summary>
public bool IsValid { get; set; }
/// <summary>
/// 목적지 노드 ID
/// </summary>
public string TargetNodeId { get; set; }
/// <summary>
/// 목적지 노드 타입
/// </summary>
public NodeType TargetNodeType { get; set; }
/// <summary>
/// 필요한 도킹 방향
/// </summary>
public AgvDirection RequiredDockingDirection { get; set; }
/// <summary>
/// 계산된 경로의 마지막 방향
/// </summary>
public AgvDirection CalculatedFinalDirection { get; set; }
/// <summary>
/// 검증 오류 메시지 (실패시)
/// </summary>
public string ValidationError { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public DockingValidationResult()
{
IsValidationRequired = false;
IsValid = true;
TargetNodeId = string.Empty;
RequiredDockingDirection = AgvDirection.Forward;
CalculatedFinalDirection = AgvDirection.Forward;
ValidationError = string.Empty;
}
/// <summary>
/// 검증 불필요한 경우 생성
/// </summary>
public static DockingValidationResult CreateNotRequired()
{
return new DockingValidationResult
{
IsValidationRequired = false,
IsValid = true
};
}
/// <summary>
/// 검증 성공 결과 생성
/// </summary>
public static DockingValidationResult CreateValid(string targetNodeId, NodeType nodeType, AgvDirection requiredDirection, AgvDirection calculatedDirection)
{
return new DockingValidationResult
{
IsValidationRequired = true,
IsValid = true,
TargetNodeId = targetNodeId,
TargetNodeType = nodeType,
RequiredDockingDirection = requiredDirection,
CalculatedFinalDirection = calculatedDirection
};
}
/// <summary>
/// 검증 실패 결과 생성
/// </summary>
public static DockingValidationResult CreateInvalid(string targetNodeId, NodeType nodeType, AgvDirection requiredDirection, AgvDirection calculatedDirection, string error)
{
return new DockingValidationResult
{
IsValidationRequired = true,
IsValid = false,
TargetNodeId = targetNodeId,
TargetNodeType = nodeType,
RequiredDockingDirection = requiredDirection,
CalculatedFinalDirection = calculatedDirection,
ValidationError = error
};
}
}
}