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

refactor: Move AGV development projects to separate AGVLogic folder

Browse Source 
- Reorganized AGVMapEditor, AGVNavigationCore, AGVSimulator into AGVLogic folder
- Removed deleted project files from root folder tracking
- Updated CLAUDE.md with AGVLogic-specific development guidelines
- Clean separation of independent project development from main codebase
- Projects now ready for independent development and future integration

🤖 Generated with Claude Code

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
2025-10-23 10:00:40 +09:00
parent ce78752c2c
commit dbaf647d4e
63 changed files with 1398 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

305
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Analysis/JunctionAnalyzer.cs Normal file
View File

@@ -0,0 +1,305 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.PathFinding.Analysis
{
/// <summary>
/// AGV 갈림길 분석 및 마그넷 센서 방향 계산 시스템
/// </summary>
public class JunctionAnalyzer
{
/// <summary>
/// 갈림길 정보
/// </summary>
public class JunctionInfo
{
public string NodeId { get; set; }
public List<string> ConnectedNodes { get; set; }
public Dictionary<string, MagnetDirection> PathDirections { get; set; }
public bool IsJunction => ConnectedNodes.Count > 2;
public JunctionInfo(string nodeId)
{
NodeId = nodeId;
ConnectedNodes = new List<string>();
PathDirections = new Dictionary<string, MagnetDirection>();
}
public override string ToString()
{
if (!IsJunction)
return $"{NodeId}: 일반노드 ({ConnectedNodes.Count}연결)";
var paths = string.Join(", ", PathDirections.Select(p => $"{p.Key}({p.Value})"));
return $"{NodeId}: 갈림길 - {paths}";
}
}
private readonly List<MapNode> _mapNodes;
private readonly Dictionary<string, JunctionInfo> _junctions;
public JunctionAnalyzer(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_junctions = new Dictionary<string, JunctionInfo>();
AnalyzeJunctions();
}
/// <summary>
/// 모든 갈림길 분석
/// </summary>
private void AnalyzeJunctions()
{
foreach (var node in _mapNodes)
{
if (node.IsNavigationNode())
{
var junctionInfo = AnalyzeNode(node);
_junctions[node.NodeId] = junctionInfo;
}
}
}
/// <summary>
/// 개별 노드의 갈림길 정보 분석
/// </summary>
private JunctionInfo AnalyzeNode(MapNode node)
{
var junction = new JunctionInfo(node.NodeId);
// 양방향 연결을 고려하여 모든 연결된 노드 찾기
var connectedNodes = GetAllConnectedNodes(node);
junction.ConnectedNodes = connectedNodes;
if (connectedNodes.Count > 2)
{
// 갈림길인 경우 각 방향별 마그넷 센서 방향 계산
CalculateMagnetDirections(node, connectedNodes, junction);
}
return junction;
}
/// <summary>
/// 양방향 연결을 고려한 모든 연결 노드 검색
/// </summary>
private List<string> GetAllConnectedNodes(MapNode node)
{
var connected = new HashSet<string>();
// 직접 연결된 노드들
foreach (var connectedId in node.ConnectedNodes)
{
connected.Add(connectedId);
}
// 역방향 연결된 노드들 (다른 노드에서 이 노드로 연결)
foreach (var otherNode in _mapNodes)
{
if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
{
connected.Add(otherNode.NodeId);
}
}
return connected.ToList();
}
/// <summary>
/// 갈림길에서 각 방향별 마그넷 센서 방향 계산
/// </summary>
private void CalculateMagnetDirections(MapNode junctionNode, List<string> connectedNodes, JunctionInfo junction)
{
if (connectedNodes.Count < 3) return;
// 각 연결 노드의 각도 계산
var nodeAngles = new List<(string NodeId, double Angle)>();
foreach (var connectedId in connectedNodes)
{
var connectedNode = _mapNodes.FirstOrDefault(n => n.NodeId == connectedId);
if (connectedNode != null)
{
double angle = CalculateAngle(junctionNode.Position, connectedNode.Position);
nodeAngles.Add((connectedId, angle));
}
}
// 각도순으로 정렬
nodeAngles.Sort((a, b) => a.Angle.CompareTo(b.Angle));
// 마그넷 방향 할당
AssignMagnetDirections(nodeAngles, junction);
}
/// <summary>
/// 두 점 사이의 각도 계산 (라디안)
/// </summary>
private double CalculateAngle(Point from, Point to)
{
double deltaX = to.X - from.X;
double deltaY = to.Y - from.Y;
double angle = Math.Atan2(deltaY, deltaX);
// 0~2π 범위로 정규화
if (angle < 0)
angle += 2 * Math.PI;
return angle;
}
/// <summary>
/// 갈림길에서 마그넷 센서 방향 할당
/// </summary>
private void AssignMagnetDirections(List<(string NodeId, double Angle)> sortedNodes, JunctionInfo junction)
{
int nodeCount = sortedNodes.Count;
for (int i = 0; i < nodeCount; i++)
{
string nodeId = sortedNodes[i].NodeId;
MagnetDirection direction;
if (nodeCount == 3)
{
// 3갈래: 직진, 좌측, 우측
switch (i)
{
case 0: direction = MagnetDirection.Straight; break;
case 1: direction = MagnetDirection.Left; break;
case 2: direction = MagnetDirection.Right; break;
default: direction = MagnetDirection.Straight; break;
}
}
else if (nodeCount == 4)
{
// 4갈래: 교차로
switch (i)
{
case 0: direction = MagnetDirection.Straight; break;
case 1: direction = MagnetDirection.Left; break;
case 2: direction = MagnetDirection.Straight; break; // 반대편
case 3: direction = MagnetDirection.Right; break;
default: direction = MagnetDirection.Straight; break;
}
}
else
{
// 5갈래 이상: 각도 기반 배정
double angleStep = 2 * Math.PI / nodeCount;
double normalizedIndex = (double)i / nodeCount;
if (normalizedIndex < 0.33)
direction = MagnetDirection.Left;
else if (normalizedIndex < 0.67)
direction = MagnetDirection.Straight;
else
direction = MagnetDirection.Right;
}
junction.PathDirections[nodeId] = direction;
}
}
/// <summary>
/// 특정 경로에서 요구되는 마그넷 방향 계산 (전진 방향 기준)
/// </summary>
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId)
{
if (!_junctions.ContainsKey(currentNodeId))
return MagnetDirection.Straight;
var junction = _junctions[currentNodeId];
if (!junction.IsJunction)
return MagnetDirection.Straight;
// 실제 각도 기반으로 마그넷 방향 계산
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);
if (fromNode == null || currentNode == null || toNode == null)
return MagnetDirection.Straight;
// 전진 방향(진행 방향) 계산
double incomingAngle = CalculateAngle(fromNode.Position, currentNode.Position);
// 목표 방향 계산
double outgoingAngle = CalculateAngle(currentNode.Position, toNode.Position);
// 각도 차이 계산 (전진 방향 기준)
double angleDiff = outgoingAngle - incomingAngle;
// 각도를 -π ~ π 범위로 정규화
while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;
// 전진 방향 기준으로 마그넷 방향 결정
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
return MagnetDirection.Straight;
else if (angleDiff < 0) // 음수면 왼쪽 회전
return MagnetDirection.Left;
else // 양수면 오른쪽 회전
return MagnetDirection.Right;
}
/// <summary>
/// 방향 전환 가능한 갈림길 검색
/// </summary>
public List<string> FindDirectionChangeJunctions(AgvDirection currentDirection, AgvDirection targetDirection)
{
var availableJunctions = new List<string>();
if (currentDirection == targetDirection)
return availableJunctions;
foreach (var junction in _junctions.Values)
{
if (junction.IsJunction)
{
// 갈림길에서 방향 전환이 가능한지 확인
// (실제로는 더 복잡한 로직이 필요하지만, 일단 모든 갈림길을 후보로 함)
availableJunctions.Add(junction.NodeId);
}
}
return availableJunctions;
}
/// <summary>
/// 갈림길 정보 반환
/// </summary>
public JunctionInfo GetJunctionInfo(string nodeId)
{
return _junctions.ContainsKey(nodeId) ? _junctions[nodeId] : null;
}
/// <summary>
/// 모든 갈림길 목록 반환
/// </summary>
public List<JunctionInfo> GetAllJunctions()
{
return _junctions.Values.Where(j => j.IsJunction).ToList();
}
/// <summary>
/// 디버깅용 갈림길 정보 출력
/// </summary>
public List<string> GetJunctionSummary()
{
var summary = new List<string>();
foreach (var junction in _junctions.Values.Where(j => j.IsJunction))
{
summary.Add(junction.ToString());
}
return summary;
}
}
}

371
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs Normal file
View File

@@ -0,0 +1,371 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.PathFinding.Core
{
/// <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 List<NodeMotorInfo> NodeMotorInfos { 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 int ExploredNodes
{
get => ExploredNodeCount;
set => ExploredNodeCount = value;
}
/// <summary>
/// 예상 소요 시간 (초)
/// </summary>
public float EstimatedTimeSeconds { get; set; }
/// <summary>
/// 회전 횟수
/// </summary>
public int RotationCount { get; set; }
/// <summary>
/// 오류 메시지 (실패시)
/// </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>
public AGVPathResult()
{
Success = false;
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>
/// 성공 결과 생성
/// </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="path">경로</param>
/// <param name="commands">AGV 명령어 목록</param>
/// <param name="nodeMotorInfos">노드별 모터방향 정보</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<string> path, List<AgvDirection> commands, List<NodeMotorInfo> nodeMotorInfos, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<string>(path),
Commands = new List<AgvDirection>(commands),
NodeMotorInfos = new List<NodeMotorInfo>(nodeMotorInfos),
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>
/// <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>
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>
/// 단순 경로 목록 반환 (호환성용)
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetSimplePath()
{
if (DetailedPath != null && DetailedPath.Count > 0)
{
return DetailedPath.Select(n => n.NodeId).ToList();
}
return Path ?? new List<string>();
}
/// <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}";
}
}
}
}

290
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs Normal file
View File

@@ -0,0 +1,290 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Core
{
/// <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();
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode())
{
var pathNode = new PathNode(mapNode.NodeId, mapNode.Position);
_nodeMap[mapNode.NodeId] = pathNode;
}
}
// 단일 연결을 양방향으로 확장
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.NodeId))
{
var pathNode = _nodeMap[mapNode.NodeId];
foreach (var connectedNodeId in mapNode.ConnectedNodes)
{
if (_nodeMap.ContainsKey(connectedNodeId))
{
// 양방향 연결 생성 (단일 연결이 양방향을 의미)
if (!pathNode.ConnectedNodes.Contains(connectedNodeId))
{
pathNode.ConnectedNodes.Add(connectedNodeId);
}
var connectedPathNode = _nodeMap[connectedNodeId];
if (!connectedPathNode.ConnectedNodes.Contains(mapNode.NodeId))
{
connectedPathNode.ConnectedNodes.Add(mapNode.NodeId);
}
}
}
}
}
}
/// <summary>
/// 경로 찾기 (A* 알고리즘)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindPath(string startNodeId, string endNodeId)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(startNodeId))
{
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(endNodeId))
{
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (startNodeId == endNodeId)
{
var singlePath = new List<string> { startNodeId };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
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 AGVPathResult.CreateSuccess(path, new List<AgvDirection>(), totalDistance, stopwatch.ElapsedMilliseconds);
}
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 AGVPathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
/// <summary>
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
{
if (targetNodeIds == null || targetNodeIds.Count == 0)
{
return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
}
AGVPathResult 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 ?? AGVPathResult.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/AGVLogic/AGVNavigationCore/PathFinding/Core/PathNode.cs Normal file
View File

@@ -0,0 +1,101 @@
using System;
using System.Drawing;
namespace AGVNavigationCore.PathFinding.Core
{
/// <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;
}
}
}

377
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs Normal file
View File

@@ -0,0 +1,377 @@
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.Planning
{
/// <summary>
/// AGV 경로 계획기
/// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
/// </summary>
public class AGVPathfinder
{
private readonly List<MapNode> _mapNodes;
private readonly AStarPathfinder _basicPathfinder;
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly DirectionChangePlanner _directionChangePlanner;
public AGVPathfinder(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_basicPathfinder = new AStarPathfinder();
_basicPathfinder.SetMapNodes(_mapNodes);
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
_directionChangePlanner = new DirectionChangePlanner(_mapNodes);
}
/// <summary>
/// AGV 경로 계산
/// </summary>
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode,
MapNode prevNode, AgvDirection currentDirection = AgvDirection.Forward)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
// 입력 검증
if (startNode == null)
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
if (targetNode == null)
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
if (prevNode == null)
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
// 1. 목적지 도킹 방향 요구사항 확인 (노드의 도킹방향 속성에서 확인)
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
// 통합된 경로 계획 함수 사용
AGVPathResult result = PlanPath(startNode, targetNode, prevNode, requiredDirection, currentDirection);
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
// 도킹 검증 수행
if (result.Success && _mapNodes != null)
{
result.DockingValidation = DockingValidator.ValidateDockingDirection(result, _mapNodes, currentDirection);
}
return result;
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
/// <summary>
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
/// </summary>
private AgvDirection? GetRequiredDockingDirection(DockingDirection dockDirection)
{
switch (dockDirection)
{
case DockingDirection.Forward:
return AgvDirection.Forward; // 전진 도킹
case DockingDirection.Backward:
return AgvDirection.Backward; // 후진 도킹
case DockingDirection.DontCare:
default:
return null; // 도킹 방향 상관없음
}
}
/// <summary>
/// 통합 경로 계획 (직접 경로 또는 방향 전환 경로)
/// </summary>
private AGVPathResult PlanPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection? requiredDirection = null, AgvDirection currentDirection = AgvDirection.Forward)
{
bool needDirectionChange = requiredDirection.HasValue && (currentDirection != requiredDirection.Value);
//현재 위치에서 목적지까지의 최단 거리 모록을 찾는다.
var DirectPathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
//이전 위치에서 목적지까지의 최단 거리를 모록을 찾는다.
var DirectPathResultP = _basicPathfinder.FindPath(prevNode.NodeId, targetNode.NodeId);
//
if (DirectPathResultP.Path.Contains(startNode.NodeId))
{
}
if (needDirectionChange)
{
// 방향 전환 경로 계획
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(
startNode.NodeId, targetNode.NodeId, currentDirection, requiredDirection.Value);
if (!directionChangePlan.Success)
{
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
}
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection.Value);
float totalDistance = CalculatePathDistance(detailedPath);
return AGVPathResult.CreateSuccess(
detailedPath,
totalDistance,
0,
0,
directionChangePlan.PlanDescription,
true,
directionChangePlan.DirectionChangeNode
);
}
else
{
// 직접 경로 계획
var basicResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
if (!basicResult.Success)
{
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
}
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
return AGVPathResult.CreateSuccess(
detailedPath,
basicResult.TotalDistance,
basicResult.CalculationTimeMs,
basicResult.ExploredNodeCount,
"직접 경로 - 방향 전환 불필요"
);
}
}
/// <summary>
/// 기본 경로를 상세 경로로 변환
/// </summary>
private List<NodeMotorInfo> ConvertToDetailedPath(List<string> simplePath, AgvDirection initialDirection)
{
var detailedPath = new List<NodeMotorInfo>();
var currentDirection = initialDirection;
for (int i = 0; i < simplePath.Count; i++)
{
string currentNodeId = simplePath[i];
string nextNodeId = (i + 1 < simplePath.Count) ? simplePath[i + 1] : null;
// 마그넷 방향 계산
MagnetDirection magnetDirection = MagnetDirection.Straight;
if (i > 0 && nextNodeId != null)
{
string prevNodeId = simplePath[i - 1];
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
}
// 노드 정보 생성
var nodeMotorInfo = new NodeMotorInfo(
currentNodeId,
currentDirection,
nextNodeId,
magnetDirection
);
// 회전 가능 노드 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
if (mapNode != null)
{
nodeMotorInfo.CanRotate = mapNode.CanRotate;
}
detailedPath.Add(nodeMotorInfo);
}
return detailedPath;
}
/// <summary>
/// 방향 전환 경로를 상세 경로로 변환
/// </summary>
private List<NodeMotorInfo> ConvertDirectionChangePath(DirectionChangePlanner.DirectionChangePlan plan, AgvDirection startDirection, AgvDirection endDirection)
{
var detailedPath = new List<NodeMotorInfo>();
var currentDirection = startDirection;
for (int i = 0; i < plan.DirectionChangePath.Count; i++)
{
string currentNodeId = plan.DirectionChangePath[i];
string nextNodeId = (i + 1 < plan.DirectionChangePath.Count) ? plan.DirectionChangePath[i + 1] : null;
// 방향 전환 노드에서 방향 변경
if (currentNodeId == plan.DirectionChangeNode && currentDirection != endDirection)
{
currentDirection = endDirection;
}
// 마그넷 방향 계산
MagnetDirection magnetDirection = MagnetDirection.Straight;
if (i > 0 && nextNodeId != null)
{
string prevNodeId = plan.DirectionChangePath[i - 1];
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
}
// 특수 동작 확인
bool requiresSpecialAction = false;
string specialActionDescription = "";
if (currentNodeId == plan.DirectionChangeNode)
{
requiresSpecialAction = true;
specialActionDescription = $"방향전환: {startDirection} → {endDirection}";
}
// 노드 정보 생성
var nodeMotorInfo = new NodeMotorInfo(
currentNodeId,
currentDirection,
nextNodeId,
true, // 방향 전환 경로의 경우 회전 가능으로 설정
currentNodeId == plan.DirectionChangeNode,
magnetDirection,
requiresSpecialAction,
specialActionDescription
);
detailedPath.Add(nodeMotorInfo);
}
return detailedPath;
}
/// <summary>
/// 경로 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<NodeMotorInfo> detailedPath)
{
float totalDistance = 0;
for (int i = 0; i < detailedPath.Count - 1; i++)
{
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i].NodeId);
var nextNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i + 1].NodeId);
if (currentNode != null && nextNode != null)
{
float dx = nextNode.Position.X - currentNode.Position.X;
float dy = nextNode.Position.Y - currentNode.Position.Y;
totalDistance += (float)Math.Sqrt(dx * dx + dy * dy);
}
}
return totalDistance;
}
/// <summary>
/// 경로 유효성 검증
/// </summary>
public bool ValidatePath(List<NodeMotorInfo> detailedPath)
{
if (detailedPath == null || detailedPath.Count == 0)
return false;
// 1. 모든 노드가 존재하는지 확인
foreach (var nodeInfo in detailedPath)
{
if (!_mapNodes.Any(n => n.NodeId == nodeInfo.NodeId))
return false;
}
// 2. 연결성 확인
for (int i = 0; i < detailedPath.Count - 1; i++)
{
string currentId = detailedPath[i].NodeId;
string nextId = detailedPath[i + 1].NodeId;
if (!_basicPathfinder.AreNodesConnected(currentId, nextId))
return false;
}
// 3. 물리적 제약사항 확인
return ValidatePhysicalConstraints(detailedPath);
}
/// <summary>
/// 물리적 제약사항 검증
/// </summary>
private bool ValidatePhysicalConstraints(List<NodeMotorInfo> detailedPath)
{
for (int i = 1; i < detailedPath.Count; i++)
{
var prevNode = detailedPath[i - 1];
var currentNode = detailedPath[i];
// 급작스러운 방향 전환 검증
if (prevNode.MotorDirection != currentNode.MotorDirection)
{
// 방향 전환은 반드시 회전 가능 노드에서만
if (!currentNode.CanRotate && !currentNode.IsDirectionChangePoint)
{
return false;
}
}
}
return true;
}
/// <summary>
/// 경로 최적화
/// </summary>
public AGVPathResult OptimizePath(AGVPathResult originalResult)
{
if (!originalResult.Success)
return originalResult;
// TODO: 경로 최적화 로직 구현
// - 불필요한 중간 노드 제거
// - 마그넷 방향 최적화
// - 방향 전환 최소화
return originalResult;
}
/// <summary>
/// 디버깅용 경로 정보
/// </summary>
public string GetPathSummary(AGVPathResult result)
{
if (!result.Success)
return $"경로 계산 실패: {result.ErrorMessage}";
var summary = new List<string>
{
$"=== AGV 고급 경로 계획 결과 ===",
$"총 노드 수: {result.DetailedPath.Count}",
$"총 거리: {result.TotalDistance:F1}px",
$"계산 시간: {result.CalculationTimeMs}ms",
$"방향 전환: {(result.RequiredDirectionChange ? $"필요 (노드: {result.DirectionChangeNode})" : "불필요")}",
$"설명: {result.PlanDescription}",
"",
"=== 상세 경로 ===",
};
foreach (var nodeInfo in result.DetailedPath)
{
summary.Add(nodeInfo.ToString());
}
return string.Join("\n", summary);
}
}
}

768
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/DirectionChangePlanner.cs Normal file
View File

@@ -0,0 +1,768 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 방향 전환 경로 계획 시스템
/// 물리적 제약사항을 고려한 방향 전환 경로 생성
/// </summary>
public class DirectionChangePlanner
{
/// <summary>
/// 방향 전환 계획 결과
/// </summary>
public class DirectionChangePlan
{
public bool Success { get; set; }
public List<string> DirectionChangePath { get; set; }
public string DirectionChangeNode { get; set; }
public string ErrorMessage { get; set; }
public string PlanDescription { get; set; }
public DirectionChangePlan()
{
DirectionChangePath = new List<string>();
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
}
public static DirectionChangePlan CreateSuccess(List<string> path, string changeNode, string description)
{
return new DirectionChangePlan
{
Success = true,
DirectionChangePath = path,
DirectionChangeNode = changeNode,
PlanDescription = description
};
}
public static DirectionChangePlan CreateFailure(string error)
{
return new DirectionChangePlan
{
Success = false,
ErrorMessage = error
};
}
}
private readonly List<MapNode> _mapNodes;
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly AStarPathfinder _pathfinder;
public DirectionChangePlanner(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
_pathfinder = new AStarPathfinder();
_pathfinder.SetMapNodes(_mapNodes);
}
/// <summary>
/// 방향 전환이 필요한 경로 계획
/// </summary>
public DirectionChangePlan PlanDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
// 방향이 같으면 직접 경로 계산
if (currentDirection == requiredDirection)
{
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath.Success)
{
return DirectionChangePlan.CreateSuccess(
directPath.Path,
null,
"방향 전환 불필요 - 직접 경로 사용"
);
}
}
// 방향 전환이 필요한 경우 - 먼저 간단한 직접 경로 확인
var directPath2 = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath2.Success)
{
// 직접 경로에 갈림길이 포함된 경우 그 갈림길에서 방향 전환
foreach (var nodeId in directPath2.Path.Skip(1).Take(directPath2.Path.Count - 2)) // 시작과 끝 제외
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 간단한 방향 전환: 직접 경로 사용하되 방향 전환 노드 표시
return DirectionChangePlan.CreateSuccess(
directPath2.Path,
nodeId,
$"갈림길 {nodeId}에서 방향 전환: {currentDirection} → {requiredDirection}"
);
}
}
}
// 복잡한 방향 전환이 필요한 경우
return PlanDirectionChangeRoute(startNodeId, targetNodeId, currentDirection, requiredDirection);
}
/// <summary>
/// 방향 전환 경로 계획
/// </summary>
private DirectionChangePlan PlanDirectionChangeRoute(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
// 1. 방향 전환 가능한 갈림길 찾기
var changeJunctions = FindSuitableChangeJunctions(startNodeId, targetNodeId, currentDirection, requiredDirection);
if (changeJunctions.Count == 0)
{
return DirectionChangePlan.CreateFailure("방향 전환 가능한 갈림길을 찾을 수 없습니다.");
}
// 2. 각 갈림길에 대해 경로 계획 시도
foreach (var junction in changeJunctions)
{
var plan = TryDirectionChangeAtJunction(startNodeId, targetNodeId, junction, currentDirection, requiredDirection);
if (plan.Success)
{
return plan;
}
}
return DirectionChangePlan.CreateFailure("모든 갈림길에서 방향 전환 경로 계획이 실패했습니다.");
}
/// <summary>
/// 방향 전환에 적합한 갈림길 검색 (인근 우회 경로 우선)
/// </summary>
private List<string> FindSuitableChangeJunctions(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var suitableJunctions = new List<string>();
// 1. 시작점 인근의 갈림길들을 우선 검색 (경로 진행 중 우회용)
var nearbyJunctions = FindNearbyJunctions(startNodeId, 2); // 2단계 내의 갈림길
foreach (var junction in nearbyJunctions)
{
if (junction == startNodeId) continue; // 시작점 제외
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junction);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 이 갈림길을 통해 목적지로 갈 수 있는지 확인
if (CanReachTargetViaJunction(junction, targetNodeId) &&
HasSuitableDetourOptions(junction, startNodeId))
{
suitableJunctions.Add(junction);
}
}
}
// 2. 직진 경로상의 갈림길들도 검색 (단, 되돌아가기 방지)
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath.Success)
{
foreach (var nodeId in directPath.Path.Skip(2)) // 시작점과 다음 노드는 제외
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 직진 경로상에서는 더 엄격한 조건 적용
if (!suitableJunctions.Contains(nodeId) &&
HasMultipleExitOptions(nodeId))
{
suitableJunctions.Add(nodeId);
}
}
}
}
// 거리순으로 정렬 (가까운 갈림길 우선 - 인근 우회용)
return SortJunctionsByDistance(startNodeId, suitableJunctions);
}
/// <summary>
/// 특정 노드 주변의 갈림길 검색
/// </summary>
private List<string> FindNearbyJunctions(string nodeId, int maxSteps)
{
var junctions = new List<string>();
var visited = new HashSet<string>();
var queue = new Queue<(string NodeId, int Steps)>();
queue.Enqueue((nodeId, 0));
visited.Add(nodeId);
while (queue.Count > 0)
{
var (currentNodeId, steps) = queue.Dequeue();
if (steps > maxSteps) continue;
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(currentNodeId);
if (junctionInfo != null && junctionInfo.IsJunction && currentNodeId != nodeId)
{
junctions.Add(currentNodeId);
}
// 연결된 노드들을 큐에 추가
var connectedNodes = GetAllConnectedNodes(currentNodeId);
foreach (var connectedId in connectedNodes)
{
if (!visited.Contains(connectedId))
{
visited.Add(connectedId);
queue.Enqueue((connectedId, steps + 1));
}
}
}
return junctions;
}
/// <summary>
/// 양방향 연결을 고려한 연결 노드 검색
/// </summary>
private List<string> GetAllConnectedNodes(string nodeId)
{
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (node == null) return new List<string>();
var connected = new HashSet<string>();
// 직접 연결
foreach (var connectedId in node.ConnectedNodes)
{
connected.Add(connectedId);
}
// 역방향 연결
foreach (var otherNode in _mapNodes)
{
if (otherNode.NodeId != nodeId && otherNode.ConnectedNodes.Contains(nodeId))
{
connected.Add(otherNode.NodeId);
}
}
return connected.ToList();
}
/// <summary>
/// 갈림길을 거리순으로 정렬
/// </summary>
private List<string> SortJunctionsByDistance(string startNodeId, List<string> junctions)
{
var distances = new List<(string NodeId, double Distance)>();
foreach (var junction in junctions)
{
var path = _pathfinder.FindPath(startNodeId, junction);
double distance = path.Success ? path.TotalDistance : double.MaxValue;
distances.Add((junction, distance));
}
return distances.OrderBy(d => d.Distance).Select(d => d.NodeId).ToList();
}
/// <summary>
/// 특정 갈림길에서 방향 전환 시도
/// </summary>
private DirectionChangePlan TryDirectionChangeAtJunction(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
try
{
// 방향 전환 경로 생성
var changePath = GenerateDirectionChangePath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
if (changePath.Count > 0)
{
// **VALIDATION**: 되돌아가기 패턴 검증
var validationResult = ValidateDirectionChangePath(changePath, startNodeId, junctionNodeId);
if (!validationResult.IsValid)
{
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ❌ 갈림길 {junctionNodeId} 경로 검증 실패: {validationResult.ValidationError}");
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId} 검증 실패: {validationResult.ValidationError}");
}
// 실제 방향 전환 노드 찾기 (우회 노드)
string actualDirectionChangeNode = FindActualDirectionChangeNode(changePath, junctionNodeId);
string description = $"갈림길 {GetDisplayName(junctionNodeId)}를 통해 {GetDisplayName(actualDirectionChangeNode)}에서 방향 전환: {currentDirection} → {requiredDirection}";
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ✅ 유효한 방향전환 경로: {string.Join(" → ", changePath)}");
return DirectionChangePlan.CreateSuccess(changePath, actualDirectionChangeNode, description);
}
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 방향 전환 경로 생성 실패");
}
catch (Exception ex)
{
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 오류: {ex.Message}");
}
}
/// <summary>
/// 방향 전환 경로 생성 (인근 갈림길 우회 방식)
/// </summary>
private List<string> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
// 2. 인근 갈림길을 통한 우회인지, 직진 경로상 갈림길인지 판단
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
bool isNearbyDetour = !directPath.Success || !directPath.Path.Contains(junctionNodeId);
if (isNearbyDetour)
{
// 인근 갈림길 우회: 직진하다가 마그넷으로 방향 전환
return GenerateNearbyDetourPath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
}
else
{
// 직진 경로상 갈림길: 기존 방식으로 처리 (단, 되돌아가기 방지)
return GenerateDirectPathChangeRoute(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
}
}
/// <summary>
/// 인근 갈림길을 통한 우회 경로 생성 (예: 012 → 013 → 마그넷으로 016 방향)
/// </summary>
private List<string> GenerateNearbyDetourPath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
// 1. 시작점에서 갈림길까지 직진 (현재 방향 유지)
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
fullPath.AddRange(toJunctionPath.Path);
// 2. 갈림길에서 방향 전환 후 목적지로
// 이때 마그넷 센서를 이용해 목적지 방향으로 진입
var fromJunctionPath = _pathfinder.FindPath(junctionNodeId, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
}
return fullPath;
}
/// <summary>
/// 직진 경로상 갈림길에서 방향 전환 경로 생성 (기존 방식 개선)
/// </summary>
private List<string> GenerateDirectPathChangeRoute(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
fullPath.AddRange(toJunctionPath.Path);
// 2. 갈림길에서 방향 전환 처리 (되돌아가기 방지)
if (currentDirection != requiredDirection)
{
string fromNodeId = toJunctionPath.Path.Count >= 2 ?
toJunctionPath.Path[toJunctionPath.Path.Count - 2] : startNodeId;
var changeSequence = GenerateDirectionChangeSequence(junctionNodeId, fromNodeId, currentDirection, requiredDirection);
if (changeSequence.Count > 1)
{
fullPath.AddRange(changeSequence.Skip(1));
}
}
// 3. 갈림길에서 목표점까지의 경로
string lastNode = fullPath.LastOrDefault() ?? junctionNodeId;
var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1));
}
return fullPath;
}
/// <summary>
/// 갈림길에서 방향 전환 시퀀스 생성
/// 물리적으로 실현 가능한 방향 전환 경로 생성
/// </summary>
private List<string> GenerateDirectionChangeSequence(string junctionNodeId, string fromNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var sequence = new List<string> { junctionNodeId };
// 방향이 같으면 변경 불필요
if (currentDirection == requiredDirection)
return sequence;
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return sequence;
// 물리적으로 실현 가능한 방향 전환 시퀀스 생성
// 핵심 원리: AGV는 RFID 태그를 읽자마자 바로 방향전환하면 안됨
// 왔던 길로 되돌아가지 않도록 다른 노드로 우회한 후 방향전환
var connectedNodes = junctionInfo.ConnectedNodes;
// 왔던 노드(fromNodeId)를 제외한 연결 노드들만 후보로 선택
// 이렇게 해야 AGV가 되돌아가는 것을 방지할 수 있음
var availableNodes = connectedNodes.Where(nodeId => nodeId != fromNodeId).ToList();
if (availableNodes.Count > 0)
{
// 방향 전환을 위한 우회 경로 생성
// 예시: 003→004(전진) 상태에서 후진 필요한 경우
// 잘못된 방법: 004→003 (왔던 길로 되돌아감)
// 올바른 방법: 004→005→004 (005로 우회하여 방향전환)
// 가장 적합한 우회 노드 선택 (직진 방향 우선, 각도 변화 최소)
string detourNode = FindBestDetourNode(junctionNodeId, availableNodes, fromNodeId);
if (!string.IsNullOrEmpty(detourNode))
{
// 1단계: 갈림길에서 우회 노드로 이동 (현재 방향 유지)
// AGV는 계속 전진하여 한 태그 더 지나감
sequence.Add(detourNode);
// 2단계: 우회 노드에서 갈림길로 다시 돌아옴 (요구 방향으로 변경)
// 이때 AGV는 안전한 위치에서 방향을 전환할 수 있음
sequence.Add(junctionNodeId);
}
}
else
{
// 사용 가능한 우회 노드가 없는 경우 (2갈래 길목)
// 이 경우 물리적으로 방향 전환이 불가능할 수 있음
// 별도의 처리 로직이 필요할 수 있음
return sequence;
}
return sequence;
}
/// <summary>
/// 방향 전환을 위한 최적의 우회 노드 선택
/// AGV의 물리적 특성을 고려한 각도 기반 선택
/// </summary>
private string FindBestDetourNode(string junctionNodeId, List<string> availableNodes, string excludeNodeId)
{
// 왔던 길(excludeNodeId)를 제외한 노드 중에서 최적의 우회 노드 선택
// 우선순위: 1) 막다른 길이 아닌 노드 (우회 후 복귀 가능) 2) 직진방향 3) 목적지 방향
var junctionNode = _mapNodes.FirstOrDefault(n => n.NodeId == junctionNodeId);
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == excludeNodeId);
if (junctionNode == null || fromNode == null)
return availableNodes.FirstOrDefault();
string bestNode = null;
double minAngleChange = double.MaxValue;
bool foundNonDeadEnd = false;
// AGV가 들어온 방향 벡터 계산 (fromNode → junctionNode)
double incomingAngle = CalculateAngle(fromNode.Position, junctionNode.Position);
foreach (var nodeId in availableNodes)
{
if (nodeId == excludeNodeId) continue; // 왔던 길 제외
var candidateNode = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (candidateNode == null) continue;
// 갈림길에서 후보 노드로의 방향 벡터 계산 (junctionNode → candidateNode)
double outgoingAngle = CalculateAngle(junctionNode.Position, candidateNode.Position);
// 방향 변화 각도 계산 (0도가 직진, 180도가 유턴)
double angleChange = CalculateAngleChange(incomingAngle, outgoingAngle);
// 막다른 길 여부 확인
var nodeConnections = GetAllConnectedNodes(nodeId);
bool isDeadEnd = nodeConnections.Count <= 1;
// 최적 노드 선택 로직
bool shouldUpdate = false;
if (!foundNonDeadEnd && !isDeadEnd)
{
// 첫 번째 막다른 길이 아닌 노드 발견
shouldUpdate = true;
foundNonDeadEnd = true;
}
else if (foundNonDeadEnd && isDeadEnd)
{
// 이미 막다른 길이 아닌 노드를 찾았으므로 막다른 길은 제외
continue;
}
else if (foundNonDeadEnd == isDeadEnd)
{
// 같은 조건(둘 다 막다른길 or 둘 다 아님)에서는 각도가 작은 것 선택
shouldUpdate = angleChange < minAngleChange;
}
if (shouldUpdate)
{
minAngleChange = angleChange;
bestNode = nodeId;
}
}
return bestNode ?? availableNodes.FirstOrDefault(n => n != excludeNodeId);
}
/// <summary>
/// 두 점 사이의 각도 계산 (라디안 단위)
/// </summary>
private double CalculateAngle(System.Drawing.Point from, System.Drawing.Point to)
{
double dx = to.X - from.X;
double dy = to.Y - from.Y;
return Math.Atan2(dy, dx);
}
/// <summary>
/// 두 방향 사이의 각도 변화량 계산 (0~180도 범위)
/// 0도에 가까울수록 직진, 180도에 가까울수록 유턴
/// </summary>
private double CalculateAngleChange(double fromAngle, double toAngle)
{
// 각도 차이 계산
double angleDiff = Math.Abs(toAngle - fromAngle);
// 0~π 범위로 정규화 (0~180도)
if (angleDiff > Math.PI)
{
angleDiff = 2 * Math.PI - angleDiff;
}
return angleDiff;
}
/// <summary>
/// 실제 방향 전환이 일어나는 노드 찾기
/// </summary>
private string FindActualDirectionChangeNode(List<string> changePath, string junctionNodeId)
{
// 방향전환 경로 구조: [start...junction, detourNode, junction...target]
// 실제 방향전환은 detourNode에서 일어남 (AGV가 한 태그 더 지나간 후)
if (changePath.Count < 3)
return junctionNodeId; // 기본값으로 갈림길 반환
// 갈림길이 두 번 나타나는 위치 찾기
int firstJunctionIndex = changePath.IndexOf(junctionNodeId);
int lastJunctionIndex = changePath.LastIndexOf(junctionNodeId);
// 갈림길이 두 번 나타나고, 그 사이에 노드가 있는 경우
if (firstJunctionIndex != lastJunctionIndex && lastJunctionIndex - firstJunctionIndex == 2)
{
// 첫 번째와 두 번째 갈림길 사이에 있는 노드가 실제 방향전환 노드
string detourNode = changePath[firstJunctionIndex + 1];
return detourNode;
}
// 방향전환 구조를 찾지 못한 경우 기본값 반환
return junctionNodeId;
}
/// <summary>
/// 갈림길에서 적절한 우회 옵션이 있는지 확인
/// </summary>
private bool HasSuitableDetourOptions(string junctionNodeId, string excludeNodeId)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return false;
// 제외할 노드(직전 노드)를 뺀 연결된 노드가 2개 이상이어야 적절한 우회 가능
var availableConnections = junctionInfo.ConnectedNodes
.Where(nodeId => nodeId != excludeNodeId)
.ToList();
// 최소 2개의 우회 옵션이 있어야 함 (갈림길에서 방향전환 후 다시 나갈 수 있어야 함)
return availableConnections.Count >= 2;
}
/// <summary>
/// 갈림길을 통해 목적지에 도달할 수 있는지 확인
/// </summary>
private bool CanReachTargetViaJunction(string junctionNodeId, string targetNodeId)
{
// 갈림길에서 목적지까지의 경로가 존재하는지 확인
var pathToTarget = _pathfinder.FindPath(junctionNodeId, targetNodeId);
return pathToTarget.Success;
}
/// <summary>
/// 갈림길에서 여러 출구 옵션이 있는지 확인 (직진 경로상 갈림길용)
/// </summary>
private bool HasMultipleExitOptions(string junctionNodeId)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return false;
// 최소 3개 이상의 연결 노드가 있어야 적절한 방향전환 가능
return junctionInfo.ConnectedNodes.Count >= 3;
}
/// <summary>
/// 방향전환 경로 검증 - 되돌아가기 패턴 및 물리적 실현성 검증
/// </summary>
private PathValidationResult ValidateDirectionChangePath(List<string> path, string startNodeId, string junctionNodeId)
{
if (path == null || path.Count == 0)
{
return PathValidationResult.CreateInvalid(startNodeId, "", "경로가 비어있습니다.");
}
// 1. 되돌아가기 패턴 검증 (A → B → A)
var backtrackingPatterns = DetectBacktrackingPatterns(path);
if (backtrackingPatterns.Count > 0)
{
var issues = new List<string>();
foreach (var pattern in backtrackingPatterns)
{
issues.Add($"되돌아가기 패턴 발견: {pattern}");
}
string errorMessage = $"되돌아가기 패턴 검출 ({backtrackingPatterns.Count}개): {string.Join(", ", issues)}";
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 경로: {string.Join(" → ", path)}");
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 되돌아가기 패턴: {errorMessage}");
return PathValidationResult.CreateInvalidWithBacktracking(
path, backtrackingPatterns, startNodeId, "", junctionNodeId, errorMessage);
}
// 2. 연속된 중복 노드 검증
var duplicates = DetectConsecutiveDuplicates(path);
if (duplicates.Count > 0)
{
string errorMessage = $"연속된 중복 노드 발견: {string.Join(", ", duplicates)}";
return PathValidationResult.CreateInvalid(startNodeId, "", errorMessage);
}
// 3. 경로 연결성 검증
var connectivity = ValidatePathConnectivity(path);
if (!connectivity.IsValid)
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"경로 연결성 오류: {connectivity.ValidationError}");
}
// 4. 갈림길 포함 여부 검증
if (!path.Contains(junctionNodeId))
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"갈림길 {junctionNodeId}이 경로에 포함되지 않음");
}
System.Diagnostics.Debug.WriteLine($"[PathValidation] ✅ 유효한 경로: {string.Join(" → ", path)}");
return PathValidationResult.CreateValid(path, startNodeId, "", junctionNodeId);
}
/// <summary>
/// 되돌아가기 패턴 검출 (A → B → A)
/// </summary>
private List<BacktrackingPattern> DetectBacktrackingPatterns(List<string> path)
{
var patterns = new List<BacktrackingPattern>();
for (int i = 0; i < path.Count - 2; i++)
{
string nodeA = path[i];
string nodeB = path[i + 1];
string nodeC = path[i + 2];
// A → B → A 패턴 검출
if (nodeA == nodeC && nodeA != nodeB)
{
var pattern = BacktrackingPattern.Create(nodeA, nodeB, nodeA, i, i + 2);
patterns.Add(pattern);
}
}
return patterns;
}
/// <summary>
/// 연속된 중복 노드 검출
/// </summary>
private List<string> DetectConsecutiveDuplicates(List<string> path)
{
var duplicates = new List<string>();
for (int i = 0; i < path.Count - 1; i++)
{
if (path[i] == path[i + 1])
{
duplicates.Add(path[i]);
}
}
return duplicates;
}
/// <summary>
/// 경로 연결성 검증
/// </summary>
private PathValidationResult ValidatePathConnectivity(List<string> path)
{
for (int i = 0; i < path.Count - 1; i++)
{
string currentNode = path[i];
string nextNode = path[i + 1];
// 두 노드간 직접 연결성 확인 (맵 노드의 ConnectedNodes 리스트 사용)
var currentMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNode);
if (currentMapNode == null || !currentMapNode.ConnectedNodes.Contains(nextNode))
{
return PathValidationResult.CreateInvalid(currentNode, nextNode, $"노드 {currentNode}와 {nextNode} 사이에 연결이 없음");
}
}
return PathValidationResult.CreateNotRequired();
}
/// <summary>
/// 두 점 사이의 거리 계산
/// </summary>
private float CalculateDistance(System.Drawing.Point p1, System.Drawing.Point p2)
{
float dx = p2.X - p1.X;
float dy = p2.Y - p1.Y;
return (float)Math.Sqrt(dx * dx + dy * dy);
}
/// <summary>
/// 경로 계획 요약 정보
/// </summary>
public string GetPlanSummary()
{
var junctions = _junctionAnalyzer.GetJunctionSummary();
return string.Join("\n", junctions);
}
/// <summary>
/// 노드의 표시명 가져오기 (RFID 우선, 없으면 (NodeID) 형태)
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <returns>표시할 이름</returns>
private string GetDisplayName(string nodeId)
{
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (node != null && !string.IsNullOrEmpty(node.RfidId))
{
return node.RfidId;
}
return $"({nodeId})";
}
}
}

121
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs Normal file
View File

@@ -0,0 +1,121 @@
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 마그넷 센서 방향 제어
/// </summary>
public enum MagnetDirection
{
/// <summary>
/// 직진 - 기본 마그넷 라인 추종
/// </summary>
Straight = 0,
/// <summary>
/// 좌측 - 마그넷 센서 가중치를 좌측으로 조정
/// </summary>
Left = 1,
/// <summary>
/// 우측 - 마그넷 센서 가중치를 우측으로 조정
/// </summary>
Right = 2
}
/// <summary>
/// 노드별 모터방향 정보 (방향 전환 지원 포함)
/// </summary>
public class NodeMotorInfo
{
/// <summary>
/// 노드 ID
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// 해당 노드에서의 모터방향
/// </summary>
public AgvDirection MotorDirection { get; set; }
/// <summary>
/// 마그넷 센서 방향 제어 (갈림길 처리용)
/// </summary>
public MagnetDirection MagnetDirection { get; set; }
/// <summary>
/// 다음 노드 ID (경로예측용)
/// </summary>
public string NextNodeId { get; set; }
/// <summary>
/// 회전 가능 노드 여부
/// </summary>
public bool CanRotate { get; set; }
/// <summary>
/// 방향 전환이 발생하는 노드 여부
/// </summary>
public bool IsDirectionChangePoint { get; set; }
/// <summary>
/// 특수 동작이 필요한 노드 여부 (갈림길 전진/후진 반복)
/// </summary>
public bool RequiresSpecialAction { get; set; }
/// <summary>
/// 특수 동작 설명
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
{
NodeId = nodeId;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNodeId = nextNodeId;
CanRotate = false;
IsDirectionChangePoint = false;
RequiresSpecialAction = false;
SpecialActionDescription = string.Empty;
}
/// <summary>
/// 방향 전환 정보를 포함한 생성자
/// </summary>
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId, bool canRotate, bool isDirectionChangePoint, MagnetDirection magnetDirection = MagnetDirection.Straight, bool requiresSpecialAction = false, string specialActionDescription = "")
{
NodeId = nodeId;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNodeId = nextNodeId;
CanRotate = canRotate;
IsDirectionChangePoint = isDirectionChangePoint;
RequiresSpecialAction = requiresSpecialAction;
SpecialActionDescription = specialActionDescription ?? string.Empty;
}
/// <summary>
/// 디버깅용 문자열 표현
/// </summary>
public override string ToString()
{
var result = $"{NodeId}:{MotorDirection}";
// 마그넷 방향이 직진이 아닌 경우 표시
if (MagnetDirection != MagnetDirection.Straight)
result += $"({MagnetDirection})";
if (IsDirectionChangePoint)
result += " [방향전환]";
if (CanRotate)
result += " [회전가능]";
if (RequiresSpecialAction)
result += $" [특수동작:{SpecialActionDescription}]";
return result;
}
}
}

103
Cs_HMI/AGVLogic/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
};
}
}
}

205
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Validation/PathValidationResult.cs Normal file
View File

@@ -0,0 +1,205 @@
using System.Collections.Generic;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Validation
{
/// <summary>
/// 경로 검증 결과 (되돌아가기 패턴 검증 포함)
/// </summary>
public class PathValidationResult
{
/// <summary>
/// 경로 검증이 필요한지 여부
/// </summary>
public bool IsValidationRequired { get; set; }
/// <summary>
/// 경로 검증 통과 여부
/// </summary>
public bool IsValid { get; set; }
/// <summary>
/// 검증된 경로
/// </summary>
public List<string> ValidatedPath { get; set; }
/// <summary>
/// 검출된 되돌아가기 패턴 목록 (A → B → A 형태)
/// </summary>
public List<BacktrackingPattern> BacktrackingPatterns { get; set; }
/// <summary>
/// 갈림길 노드 목록
/// </summary>
public List<string> JunctionNodes { get; set; }
/// <summary>
/// 시작 노드 ID
/// </summary>
public string StartNodeId { get; set; }
/// <summary>
/// 목표 노드 ID
/// </summary>
public string TargetNodeId { get; set; }
/// <summary>
/// 갈림길 노드 ID (방향 전환용)
/// </summary>
public string JunctionNodeId { get; set; }
/// <summary>
/// 검증 오류 메시지 (실패시)
/// </summary>
public string ValidationError { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public PathValidationResult()
{
IsValidationRequired = false;
IsValid = true;
ValidatedPath = new List<string>();
BacktrackingPatterns = new List<BacktrackingPattern>();
JunctionNodes = new List<string>();
StartNodeId = string.Empty;
TargetNodeId = string.Empty;
JunctionNodeId = string.Empty;
ValidationError = string.Empty;
}
/// <summary>
/// 검증 불필요한 경우 생성
/// </summary>
public static PathValidationResult CreateNotRequired()
{
return new PathValidationResult
{
IsValidationRequired = false,
IsValid = true
};
}
/// <summary>
/// 검증 성공 결과 생성
/// </summary>
public static PathValidationResult CreateValid(List<string> path, string startNodeId, string targetNodeId, string junctionNodeId = "")
{
return new PathValidationResult
{
IsValidationRequired = true,
IsValid = true,
ValidatedPath = new List<string>(path),
StartNodeId = startNodeId,
TargetNodeId = targetNodeId,
JunctionNodeId = junctionNodeId
};
}
/// <summary>
/// 검증 실패 결과 생성 (되돌아가기 패턴 검출)
/// </summary>
public static PathValidationResult CreateInvalidWithBacktracking(
List<string> path,
List<BacktrackingPattern> backtrackingPatterns,
string startNodeId,
string targetNodeId,
string junctionNodeId,
string error)
{
return new PathValidationResult
{
IsValidationRequired = true,
IsValid = false,
ValidatedPath = new List<string>(path),
BacktrackingPatterns = new List<BacktrackingPattern>(backtrackingPatterns),
StartNodeId = startNodeId,
TargetNodeId = targetNodeId,
JunctionNodeId = junctionNodeId,
ValidationError = error
};
}
/// <summary>
/// 일반 검증 실패 결과 생성
/// </summary>
public static PathValidationResult CreateInvalid(string startNodeId, string targetNodeId, string error)
{
return new PathValidationResult
{
IsValidationRequired = true,
IsValid = false,
StartNodeId = startNodeId,
TargetNodeId = targetNodeId,
ValidationError = error
};
}
}
/// <summary>
/// 되돌아가기 패턴 정보 (A → B → A)
/// </summary>
public class BacktrackingPattern
{
/// <summary>
/// 시작 노드 (A)
/// </summary>
public string StartNode { get; set; }
/// <summary>
/// 중간 노드 (B)
/// </summary>
public string MiddleNode { get; set; }
/// <summary>
/// 되돌아간 노드 (다시 A)
/// </summary>
public string ReturnNode { get; set; }
/// <summary>
/// 경로에서의 시작 인덱스
/// </summary>
public int StartIndex { get; set; }
/// <summary>
/// 경로에서의 종료 인덱스
/// </summary>
public int EndIndex { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public BacktrackingPattern()
{
StartNode = string.Empty;
MiddleNode = string.Empty;
ReturnNode = string.Empty;
StartIndex = -1;
EndIndex = -1;
}
/// <summary>
/// 되돌아가기 패턴 생성
/// </summary>
public static BacktrackingPattern Create(string startNode, string middleNode, string returnNode, int startIndex, int endIndex)
{
return new BacktrackingPattern
{
StartNode = startNode,
MiddleNode = middleNode,
ReturnNode = returnNode,
StartIndex = startIndex,
EndIndex = endIndex
};
}
/// <summary>
/// 패턴 설명 문자열
/// </summary>
public override string ToString()
{
return $"{StartNode} → {MiddleNode} → {ReturnNode} (인덱스: {StartIndex}-{EndIndex})";
}
}
}