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

파일정리

Browse Source
This commit is contained in:
ChiKyun Kim
2026-01-29 14:03:17 +09:00
parent 00cc0ef5b7
commit 58ca67150d
440 changed files with 47236 additions and 99165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,327 @@
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.Id] = junctionInfo;
}
}
}
/// <summary>
/// 개별 노드의 갈림길 정보 분석
/// </summary>
private JunctionInfo AnalyzeNode(MapNode node)
{
var junction = new JunctionInfo(node.Id);
// 양방향 연결을 고려하여 모든 연결된 노드 찾기
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 connectedNode in node.ConnectedMapNodes)
{
if (connectedNode != null)
{
connected.Add(connectedNode.Id);
}
}
// 역방향 연결된 노드들 (다른 노드에서 이 노드로 연결)
foreach (var otherNode in _mapNodes)
{
if (otherNode.Id != node.Id && otherNode.ConnectedMapNodes.Any(n => n.Id == node.Id))
{
connected.Add(otherNode.Id);
}
}
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.Id == 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>
/// <param name="fromNodeId">이전 노드 ID</param>
/// <param name="currentNodeId">현재 노드 ID</param>
/// <param name="toNodeId">목표 노드 ID</param>
/// <param name="motorDirection">AGV 모터 방향 (Forward/Backward)</param>
/// <returns>마그넷 방향 (모터 방향 고려)</returns>
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId, AgvDirection motorDirection )
{
if (!_junctions.ContainsKey(currentNodeId))
return MagnetDirection.Straight;
var junction = _junctions[currentNodeId];
if (!junction.IsJunction)
return MagnetDirection.Straight;
// 실제 각도 기반으로 마그넷 방향 계산
var fromNode = _mapNodes.FirstOrDefault(n => n.Id == fromNodeId);
var currentNode = _mapNodes.FirstOrDefault(n => n.Id == currentNodeId);
var toNode = _mapNodes.FirstOrDefault(n => n.Id == 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;
// 전진 방향 기준으로 마그넷 방향 결정
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
MagnetDirection baseMagnetDirection;
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
baseMagnetDirection = MagnetDirection.Straight;
else if (angleDiff < 0) // 음수면 왼쪽 회전
baseMagnetDirection = MagnetDirection.Left;
else // 양수면 오른쪽 회전
baseMagnetDirection = MagnetDirection.Right;
// 후진 모터 방향일 경우 마그넷 방향 반대로 설정
// Forward: Left/Right 그대로 사용
// Backward: Left ↔ Right 반대로 사용
if (motorDirection == AgvDirection.Backward)
{
if (baseMagnetDirection == MagnetDirection.Left)
return MagnetDirection.Right;
else if (baseMagnetDirection == MagnetDirection.Right)
return MagnetDirection.Left;
}
return baseMagnetDirection;
}
/// <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;
}
}
}

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

@@ -0,0 +1,314 @@
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>
/// 경로 노드 목록 (시작 → 목적지 순서)
/// </summary>
public List<MapNode> Path { get; set; }
/// <summary>
/// AGV 명령어 목록 (이동 방향 시퀀스)
/// </summary>
public List<AgvDirection> Commands { get; set; }
/// <summary>
/// 총 거리
/// </summary>
public float TotalDistance { get; set; }
/// <summary>
/// 계산 소요 시간 (밀리초)
/// </summary>
public long CalculationTimeMs { get; set; }
/// <summary>
/// 탐색된 노드 수
/// </summary>
public 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 Message { 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 MapNode PrevNode { get; set; }
/// <summary>
/// PrevNode 에서 현재위치까지 이동한 모터의 방향값
/// </summary>
public AgvDirection PrevDirection { get; set; }
public MapNode Gateway { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public AGVPathResult()
{
Success = false;
Path = new List<MapNode>();
Commands = new List<AgvDirection>();
DetailedPath = new List<NodeMotorInfo>();
TotalDistance = 0;
CalculationTimeMs = 0;
ExploredNodes = 0;
EstimatedTimeSeconds = 0;
RotationCount = 0;
Message = string.Empty;
PlanDescription = string.Empty;
RequiredDirectionChange = false;
DirectionChangeNode = string.Empty;
DockingValidation = DockingValidationResult.CreateNotRequired();
PrevNode = null;
PrevDirection = AgvDirection.Stop;
}
/// <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<MapNode> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<MapNode>(path),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 실패 결과 생성
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <returns>실패 결과</returns>
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs = 0, int exploredNodes = 0)
{
return new AGVPathResult
{
Success = false,
Message = errorMessage,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes
};
}
/// <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 GetDetailedPathInfo(bool shortmessage = false)
{
if (!Success)
{
return $"경로 계산 실패: {Message} (계산시간: {CalculationTimeMs}ms)";
}
var data = DetailedPath.Select(t =>
{
if (shortmessage)
return $"{t.RfidId:00}{t.MotorDirection.ToString().Substring(0, 1)}{t.MagnetDirection.ToString().Substring(0, 1)}";
else
return $"{t.RfidId}[{t.NodeId}] {t.MotorDirection.ToString().Substring(0, 1)}-{t.MagnetDirection.ToString().Substring(0, 1)}";
});
return string.Join(" → ", data);
}
/// <summary>
/// 단순 경로 목록 반환 (호환성용 - 노드 ID 문자열 목록)
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetSimplePath()
{
if (DetailedPath != null && DetailedPath.Count > 0)
{
return DetailedPath.Select(n => n.NodeId).ToList();
}
return Path?.Select(n => n.Id).ToList() ?? 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: {Message}";
}
}
}
}

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

@@ -0,0 +1,622 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// A* 알고리즘 기반 경로 탐색기
/// </summary>
public class AStarPathfinder
{
private Dictionary<string, PathNode> _nodeMap;
private List<MapNode> _mapNodes;
private Dictionary<string, MapNode> _mapNodeLookup; // Quick lookup for node ID -> MapNode
/// <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>();
_mapNodeLookup = new Dictionary<string, MapNode>();
}
/// <summary>
/// 맵 노드 설정
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
public void SetMapNodes(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_nodeMap.Clear();
_mapNodeLookup.Clear();
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
foreach (var mapNode in _mapNodes)
{
_mapNodeLookup[mapNode.Id] = mapNode; // Add to lookup table
if (mapNode.IsNavigationNode())
{
var pathNode = new PathNode(mapNode.Id, mapNode.Position);
_nodeMap[mapNode.Id] = pathNode;
}
}
// 단일 연결을 양방향으로 확장
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.Id))
{
var pathNode = _nodeMap[mapNode.Id];
foreach (var connectedNode in mapNode.ConnectedMapNodes)
{
if (connectedNode != null && _nodeMap.ContainsKey(connectedNode.Id))
{
// 양방향 연결 생성 (단일 연결이 양방향을 의미)
if (!pathNode.ConnectedNodes.Contains(connectedNode.Id))
{
pathNode.ConnectedNodes.Add(connectedNode.Id);
}
var connectedPathNode = _nodeMap[connectedNode.Id];
if (!connectedPathNode.ConnectedNodes.Contains(mapNode.Id))
{
connectedPathNode.ConnectedNodes.Add(mapNode.Id);
}
}
}
}
}
}
/// <summary>
/// 노드 ID로 MapNode 가져오기 (헬퍼 메서드)
/// </summary>
private MapNode GetMapNode(string nodeId)
{
return _mapNodeLookup.ContainsKey(nodeId) ? _mapNodeLookup[nodeId] : null;
}
/// <summary>
/// 경로 찾기 (A* 알고리즘)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindPathAStar(MapNode start, MapNode end)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(start.Id))
{
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {start.Id}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(end.Id))
{
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {end.Id}", stopwatch.ElapsedMilliseconds, 0);
}
//출발지와 목적지가 동일한 경우
if (start.Id == end.Id)
{
//var startMapNode = GetMapNode(start);
var singlePath = new List<MapNode> { start };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
var startNode = _nodeMap[start.Id];
var endNode = _nodeMap[end.Id];
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 == end.Id)
{
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>
///// 경유지를 거쳐 경로 찾기 (오버로드)
///// 여러 경유지를 순차적으로 거쳐서 최종 목적지까지의 경로를 계산합니다.
///// 기존 FindPath를 여러 번 호출하여 각 구간의 경로를 합칩니다.
///// </summary>
///// <param name="startNodeId">시작 노드 ID</param>
///// <param name="endNodeId">최종 목적지 노드 ID</param>
///// <param name="waypointNodeIds">경유지 노드 ID 배열 (선택사항)</param>
///// <returns>경로 계산 결과 (모든 경유지를 거친 전체 경로)</returns>
//public AGVPathResult FindPath(string startNodeId, string endNodeId, params string[] waypointNodeIds)
//{
// var stopwatch = System.Diagnostics.Stopwatch.StartNew();
// try
// {
// // 경유지가 없으면 기본 FindPath 호출
// if (waypointNodeIds == null || waypointNodeIds.Length == 0)
// {
// return FindPathAStar(startNodeId, endNodeId);
// }
// // 경유지 유효성 검증
// var validWaypoints = new List<string>();
// foreach (var waypointId in waypointNodeIds)
// {
// if (string.IsNullOrEmpty(waypointId))
// continue;
// if (!_nodeMap.ContainsKey(waypointId))
// {
// return AGVPathResult.CreateFailure($"경유지 노드를 찾을 수 없습니다: {waypointId}", stopwatch.ElapsedMilliseconds, 0);
// }
// validWaypoints.Add(waypointId);
// }
// // 경유지가 없으면 기본 경로 계산
// if (validWaypoints.Count == 0)
// {
// return FindPathAStar(startNodeId, endNodeId);
// }
// // 첫 번째 경유지가 시작노드와 같은지 검사
// if (validWaypoints[0] == startNodeId)
// {
// return AGVPathResult.CreateFailure(
// $"첫 번째 경유지({validWaypoints[0]})가 시작 노드({startNodeId})와 동일합니다. 경유지는 시작노드와 달라야 합니다.",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 마지막 경유지가 목적지노드와 같은지 검사
// if (validWaypoints[validWaypoints.Count - 1] == endNodeId)
// {
// return AGVPathResult.CreateFailure(
// $"마지막 경유지({validWaypoints[validWaypoints.Count - 1]})가 목적지 노드({endNodeId})와 동일합니다. 경유지는 목적지노드와 달라야 합니다.",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 연속된 중복만 제거 (순서 유지)
// // 예: [1, 2, 2, 3, 2] -> [1, 2, 3, 2] (연속 중복만 제거)
// var deduplicatedWaypoints = new List<string>();
// string lastWaypoint = null;
// foreach (var waypoint in validWaypoints)
// {
// if (waypoint != lastWaypoint)
// {
// deduplicatedWaypoints.Add(waypoint);
// lastWaypoint = waypoint;
// }
// }
// validWaypoints = deduplicatedWaypoints;
// // 최종 경로 리스트와 누적 값
// var combinedPath = new List<MapNode>();
// float totalDistance = 0;
// long totalCalculationTime = 0;
// // 현재 시작점
// string currentStart = startNodeId;
// // 1단계: 각 경유지까지의 경로 계산
// for (int i = 0; i < validWaypoints.Count; i++)
// {
// string waypoint = validWaypoints[i];
// // 현재 위치에서 경유지까지의 경로 계산
// var segmentResult = FindPathAStar(currentStart, waypoint);
// if (!segmentResult.Success)
// {
// return AGVPathResult.CreateFailure(
// $"경유지 {i + 1}({waypoint})까지의 경로 계산 실패: {segmentResult.ErrorMessage}",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 경로 합치기 (첫 번째 구간이 아니면 시작점 제거하여 중복 방지)
// if (combinedPath.Count > 0 && segmentResult.Path.Count > 0)
// {
// // 시작 노드 제거 (이전 경로의 마지막 노드와 동일)
// combinedPath.AddRange(segmentResult.Path.Skip(1));
// }
// else
// {
// combinedPath.AddRange(segmentResult.Path);
// }
// totalDistance += segmentResult.TotalDistance;
// totalCalculationTime += segmentResult.CalculationTimeMs;
// // 다음 경유지의 시작점은 현재 경유지
// currentStart = waypoint;
// }
// // 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
// var finalSegmentResult = FindPathAStar(currentStart, endNodeId);
// if (!finalSegmentResult.Success)
// {
// return AGVPathResult.CreateFailure(
// $"최종 목적지까지의 경로 계산 실패: {finalSegmentResult.ErrorMessage}",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 최종 경로 합치기 (시작점 제거)
// if (combinedPath.Count > 0 && finalSegmentResult.Path.Count > 0)
// {
// combinedPath.AddRange(finalSegmentResult.Path.Skip(1));
// }
// else
// {
// combinedPath.AddRange(finalSegmentResult.Path);
// }
// totalDistance += finalSegmentResult.TotalDistance;
// totalCalculationTime += finalSegmentResult.CalculationTimeMs;
// stopwatch.Stop();
// // 결과 생성
// return AGVPathResult.CreateSuccess(
// combinedPath,
// new List<AgvDirection>(),
// totalDistance,
// totalCalculationTime
// );
// }
// catch (Exception ex)
// {
// return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
// }
//}
/// <summary>
/// 두 경로 결과를 합치기
/// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침
/// </summary>
/// <param name="previousResult">이전 경로 결과</param>
/// <param name="currentResult">현재 경로 결과</param>
/// <returns>합쳐진 경로 결과</returns>
public AGVPathResult CombineResults( AGVPathResult previousResult, AGVPathResult currentResult)
{
// 입력 검증
if (previousResult == null)
return currentResult;
if (currentResult == null)
return previousResult;
if (!previousResult.Success)
return AGVPathResult.CreateFailure(
$"이전 경로 결과 실패: {previousResult.Message}",
previousResult.CalculationTimeMs);
if (!currentResult.Success)
return AGVPathResult.CreateFailure(
$"현재 경로 결과 실패: {currentResult.Message}",
currentResult.CalculationTimeMs);
// 경로가 비어있는 경우 처리
if (previousResult.Path == null || previousResult.Path.Count == 0)
return currentResult;
if (currentResult.Path == null || currentResult.Path.Count == 0)
return previousResult;
// 합친 경로 생성
var combinedPath = new List<MapNode>(previousResult.Path);
var combinedCommands = new List<AgvDirection>(previousResult.Commands);
var combinedDetailedPath = new List<NodeMotorInfo>(previousResult.DetailedPath ?? new List<NodeMotorInfo>());
// 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교
string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].Id;
string firstNodeOfCurrent = currentResult.Path[0].Id;
if (lastNodeOfPrevious == firstNodeOfCurrent)
{
// 첫 번째 노드 제거 (중복 제거)
combinedPath.RemoveAt(combinedPath.Count - 1);
combinedPath.AddRange(currentResult.Path);
// DetailedPath도 첫 번째 노드 제거
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
{
combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1);
combinedDetailedPath.AddRange(currentResult.DetailedPath);
}
}
else
{
// 그대로 붙임
combinedPath.AddRange(currentResult.Path);
// DetailedPath도 그대로 붙임
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
{
combinedDetailedPath.AddRange(currentResult.DetailedPath);
}
}
// 명령어 합치기
combinedCommands.AddRange(currentResult.Commands);
// 총 거리 합산
float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance;
// 계산 시간 합산
long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs;
// 합쳐진 결과 생성
var result = AGVPathResult.CreateSuccess(
combinedPath,
combinedCommands,
combinedDistance,
combinedCalculationTime
);
// DetailedPath 설정
result.DetailedPath = combinedDetailedPath;
result.PrevNode = previousResult.PrevNode;
result.PrevDirection = previousResult.PrevDirection;
return result;
}
///// <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 = FindPathAStar(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<MapNode> ReconstructPath(PathNode endNode)
{
var path = new List<MapNode>();
var current = endNode;
while (current != null)
{
var mapNode = GetMapNode(current.NodeId);
if (mapNode != null)
{
path.Add(mapNode);
}
current = current.Parent;
}
path.Reverse();
return path;
}
/// <summary>
/// 경로의 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<MapNode> path)
{
if (path.Count < 2) return 0;
float totalDistance = 0;
for (int i = 0; i < path.Count - 1; i++)
{
var nodeId1 = path[i].Id;
var nodeId2 = path[i + 1].Id;
if (_nodeMap.ContainsKey(nodeId1) && _nodeMap.ContainsKey(nodeId2))
{
totalDistance += _nodeMap[nodeId1].DistanceTo(_nodeMap[nodeId2]);
}
}
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;
}
/// <summary>
/// 방향 전환을 위한 대체 노드 찾기
/// 교차로에 연결된 노드 중에서 왔던 길과 갈 길이 아닌 다른 노드를 찾음
/// 방향 전환 시 왕복 경로에 사용될 노드
/// </summary>
/// <param name="junctionNodeId">교차로 노드 ID (B)</param>
/// <param name="previousNodeId">이전 노드 ID (A - 왔던 길)</param>
/// <param name="targetNodeId">목표 노드 ID (C - 갈 길)</param>
/// <param name="mapNodes">전체 맵 노드 목록</param>
/// <returns>방향 전환에 사용할 대체 노드, 없으면 null</returns>
public MapNode FindAlternateNodeForDirectionChange(
string junctionNodeId,
string previousNodeId,
string targetNodeId)
{
// 입력 검증
if (string.IsNullOrEmpty(junctionNodeId) || string.IsNullOrEmpty(previousNodeId) || string.IsNullOrEmpty(targetNodeId))
return null;
if (_mapNodes == null || _mapNodes.Count == 0)
return null;
// 교차로 노드 찾기
var junctionNode = _mapNodes.FirstOrDefault(n => n.Id == junctionNodeId);
if (junctionNode == null || junctionNode.ConnectedNodes == null || junctionNode.ConnectedNodes.Count == 0)
return null;
// 교차로에 연결된 모든 노드 중에서 조건을 만족하는 노드 찾기
// 조건:
// 1. 이전 노드(왔던 길)가 아님
// 2. 목표 노드(갈 길)가 아님
// 3. 실제로 존재하는 노드
// 4. 활성 상태인 노드
// 5. 네비게이션 가능한 노드
var alternateNodes = new List<MapNode>();
foreach (var connectedNodeId in junctionNode.ConnectedNodes)
{
if (connectedNodeId == null) continue;
// 조건 1: 왔던 길이 아님
if (connectedNodeId == previousNodeId) continue;
// 조건 2: 갈 길이 아님
if (connectedNodeId == targetNodeId) continue;
// 조건 3, 4, 5: 존재하고, 활성 상태이고, 네비게이션 가능
var connectedNode = _mapNodes.FirstOrDefault(n => n.Id == connectedNodeId);
if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
{
alternateNodes.Add(connectedNode);
}
}
// 찾은 노드가 없으면 null 반환
if (alternateNodes.Count == 0)
return null;
// 여러 개 찾았으면 첫 번째 노드 반환
// (필요시 거리 기반으로 가장 가까운 노드를 선택할 수도 있음)
return alternateNodes[0];
}
}
}

101
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;
}
}
}

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

@@ -0,0 +1,749 @@
using System;
using System.Collections.Generic;
using System.Drawing;
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 DirectionalPathfinder _directionPathfinder;
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);
_directionPathfinder = new DirectionalPathfinder();
}
/// <summary>
/// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
/// </summary>
/// <param name="startNode">기준이 되는 노드</param>
/// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
public MapNode FindNearestJunction(MapNode startNode)
{
if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
return null;
// 교차로: 3개 이상의 노드가 연결된 노드
var junctions = _mapNodes.Where(n =>
n.IsActive &&
n.IsNavigationNode() &&
n.ConnectedNodes != null &&
n.DisableCross == false &&
n.ConnectedNodes.Count >= 3 &&
n.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false &&
n.Id != startNode.Id
).ToList();
// docking 포인트가 연결된 노드는 제거한다.
if (junctions.Count == 0)
return null;
// 직선 거리 기반으로 가장 가까운 교차로 찾기
MapNode nearestJunction = null;
float minDistance = float.MaxValue;
foreach (var junction in junctions)
{
float dx = junction.Position.X - startNode.Position.X;
float dy = junction.Position.Y - startNode.Position.Y;
float distance = (float)Math.Sqrt(dx * dx + dy * dy);
if (distance < minDistance)
{
minDistance = distance;
nearestJunction = junction;
}
}
return nearestJunction;
}
/// <summary>
/// 지정한 노드에서 경로상 가장 가까운 교차로를 찾는다.
/// (최단 경로 내에서 3개 이상 연결된 교차로를 찾음)
/// </summary>
/// <param name="startNode">시작 노드</param>
/// <param name="targetNode">목적지 노드</param>
/// <returns>경로상의 가장 가까운 교차로 노드 (또는 null)</returns>
public MapNode FindNearestJunctionOnPath(AGVPathResult pathResult)
{
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
return null;
// 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
var StartNode = pathResult.Path.First();
foreach (var pathNode in pathResult.Path)
{
if (pathNode != null &&
pathNode.IsActive &&
pathNode.IsNavigationNode() &&
pathNode.DisableCross == false &&
pathNode.ConnectedNodes != null &&
pathNode.ConnectedNodes.Count >= 3 &&
pathNode.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false)
{
if (pathNode.Id.Equals(StartNode.Id) == false)
return pathNode;
}
}
return null;
}
public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode)
{
// 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
return _basicPathfinder.FindPathAStar(startNode, targetNode);
}
/// <summary>
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
/// </summary>
/// <summary>
/// 단순 경로 찾기 (복잡한 제약조건/방향전환 로직 없이 A* 결과만 반환)
/// </summary>
public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode _prevNode, AgvDirection prevDirection)
{
// 1. 입력 검증
if (startNode == null || targetNode == null)
return AGVPathResult.CreateFailure("노드 정보 오류", 0, 0);
// 2. A* 경로 탐색
var pathResult = _basicPathfinder.FindPathAStar(startNode, targetNode);
pathResult.PrevNode = _prevNode;
pathResult.PrevDirection = prevDirection;
if (!pathResult.Success)
return AGVPathResult.CreateFailure(pathResult.Message ?? "경로 없음", 0, 0);
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
if (pathResult.Path != null && pathResult.Path.Count > 0)
{
var detailedPath = new List<NodeMotorInfo>();
for (int i = 0; i < pathResult.Path.Count; i++)
{
var node = pathResult.Path[i];
var nextNode = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1] : null;
// 마그넷 방향 계산 (갈림길인 경우)
// 마그넷 방향 계산 (갈림길인 경우)
MagnetDirection magnetDirection = MagnetDirection.Straight;
//갈림길에 있다면 미리 방향을 저장해준다.
if ((node.ConnectedNodes?.Count ?? 0) > 2 && nextNode != null)
{
//다음 노드ID를 확인해서 마그넷 방향 데이터를 찾는다.
if (node.MagnetDirections.ContainsKey(nextNode.Id) == false)
{
return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
}
else
{
var magdir = node.MagnetDirections[nextNode.Id].ToString();
if (magdir == "L") magnetDirection = MagnetDirection.Left;
else if (magdir == "R") magnetDirection = MagnetDirection.Right;
}
}
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode, magnetDirection);
// 속도 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);
if (mapNode != null)
{
nodeInfo.Speed = mapNode.SpeedLimit;
detailedPath.Add(nodeInfo);
}
}
pathResult.DetailedPath = detailedPath;
}
return pathResult;
}
/// <summary>
/// 길목(Gateway) 기반 고급 경로 계산 (기존 SimulatorForm.CalcPath 이관)
/// </summary>
public AGVPathResult CalculatePath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
{
AGVPathResult Retval;
// var o_StartNode = startNode;
// startNode, targetNode는 이미 인자로 받음
if (startNode == null || targetNode == null) return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음");
try
{
// 종료노드라면 이전위치로 이동시켜야한다. (Simulator Logic)
// 만약 시작노드가 끝단(ConnectedMapNodes.Count == 1)이라면,
// AGV가 해당 노드에 '도착'한 상태가 아니라 '작업' 중일 수 있으므로
// 이전 노드(진입점)로 위치를 보정하여 경로를 계산한다.
AGVPathResult LimitPath = null;
if (startNode.ConnectedMapNodes.Count == 1)
{
// 시작점 -> 이전점 경로 (보통 후진이나 전진 1칸)
LimitPath = this.FindPathAStar(startNode, prevNode);
if (LimitPath.Success)
{
for (int i = 0; i < LimitPath.Path.Count; i++)
{
var nodeinfo = LimitPath.Path[i];
var dir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
LimitPath.DetailedPath.Add(new NodeMotorInfo(i + 1, nodeinfo.Id, nodeinfo.RfidId, dir));
}
// 시작 위치 및 방향 변경
// var org_start = startNode; // Unused
startNode = prevNode;
prevNode = LimitPath.Path.First(); // startNode (original)
prevDir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
}
else
{
// 경로 생성 실패 시 보정 없이 진행하거나 에러 처리
// 여기서는 일단 기존 로직대로 진행
}
}
// 2. Buffer-to-Buffer 예외 처리
// 05~31 구간 체크
var node05 = _mapNodes.FirstOrDefault(n => n.RfidId == 5);
var node31 = _mapNodes.FirstOrDefault(n => n.RfidId == 31);
bool fixpath = false;
Retval = null;
MapNode gatewayNode = null;
if (node05 != null && node31 != null)
{
// 버퍼 구간 경로 테스트
var rlt = this.FindPathAStar(node05, node31);
if (rlt.Success)
{
// 버퍼구간내에 시작과 종료가 모두 포함되어있다
if (rlt.Path.Find(n => n.Id == startNode.Id) != null &&
rlt.Path.Find(n => n.Id == targetNode.Id) != null)
{
Retval = CalcPathBufferToBuffer(startNode, targetNode, prevNode, prevDir);
fixpath = true;
}
}
}
if (!fixpath)
{
// 3. 목적지별 Gateway 및 진입 조건 확인
gatewayNode = GetGatewayNode(targetNode);
if (gatewayNode == null)
{
// 게이트웨이가 없는 경우라면(일반 노드 등), Gateway 로직 없이 기본 경로 탐색
Retval = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
}
else
{
// Gateway Node 찾음
// 4. Start -> Gateway 경로 계산 (A*)
var pathToGateway = this.FindBasicPath(startNode, gatewayNode, prevNode, prevDir);
if (pathToGateway.Success == false)
return AGVPathResult.CreateFailure($"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.Message}");
// 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
if (pathToGateway.Path.Count > 1)
{
var predictNext = pathToGateway.Path[1];
if (predictNext.Id == prevNode.Id)
{
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
foreach (var item in pathToGateway.DetailedPath)
item.MotorDirection = reverseDir;
}
}
// 마지막 경로는 게이트웨이이므로 제거 (Gateway 진입 후 처리는 GetPathFromGateway에서 담당)
if (pathToGateway.Path.Count > 0 && pathToGateway.Path.Last().Id == gatewayNode.Id)
{
var idx = pathToGateway.Path.Count - 1;
pathToGateway.Path.RemoveAt(idx);
pathToGateway.DetailedPath.RemoveAt(idx);
}
// 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
MapNode GateprevNode = pathToGateway.Path.LastOrDefault() ?? prevNode;
NodeMotorInfo GatePrevDetail = pathToGateway.DetailedPath.LastOrDefault();
var arrivalOrientation = GatePrevDetail?.MotorDirection ?? prevDir;
var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, GateprevNode, arrivalOrientation);
if (!gatewayPathResult.Success)
return AGVPathResult.CreateFailure($"{gatewayPathResult.Message}");
Retval = CombinePaths(pathToGateway, gatewayPathResult);
}
}
//게이트웨이
Retval.Gateway = gatewayNode;
// 경로 오류 검사
if (Retval == null || Retval.Success == false) return Retval ?? AGVPathResult.CreateFailure("경로 계산 결과 없음");
if (LimitPath != null)
{
Retval = CombinePaths(LimitPath, Retval);
}
// 해당 경로와 대상의 도킹포인트 방향 검사
if (targetNode.DockDirection != DockingDirection.DontCare)
{
var lastPath = Retval.DetailedPath.LastOrDefault();
if (lastPath != null)
{
if (targetNode.DockDirection == DockingDirection.Forward && lastPath.MotorDirection != AgvDirection.Forward)
{
return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(FWD) Target:{targetNode.DockDirection}");
}
if (targetNode.DockDirection == DockingDirection.Backward && lastPath.MotorDirection != AgvDirection.Backward)
{
return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(BWD) Target:{targetNode.DockDirection}");
}
}
}
// 경로 최적화: A -> B -> A 패턴 제거
// 6[F][R] → 13[B][L] → 6[F][L] 같은 경우 제거
while (fixpath == false)
{
var updatecount = 0;
for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
{
var n1 = Retval.DetailedPath[i];
var n2 = Retval.DetailedPath[i + 1];
var n3 = Retval.DetailedPath[i + 2];
if (n1.NodeId == n3.NodeId)
{
bool isInverse = false;
// 1. 모터 방향이 반대인가? (F <-> B)
bool isMotorInverse = (n1.MotorDirection != n2.MotorDirection) &&
(n1.MotorDirection == AgvDirection.Forward || n1.MotorDirection == AgvDirection.Backward) &&
(n2.MotorDirection == AgvDirection.Forward || n2.MotorDirection == AgvDirection.Backward);
if (isMotorInverse)
{
// 2. 마그넷 방향이 반대인가? (L <-> R, S <-> S)
bool isMagnetInverse = false;
if (n1.MagnetDirection == MagnetDirection.Straight && n2.MagnetDirection == MagnetDirection.Straight) isMagnetInverse = true;
else if (n1.MagnetDirection == MagnetDirection.Left && n2.MagnetDirection == MagnetDirection.Right) isMagnetInverse = true;
else if (n1.MagnetDirection == MagnetDirection.Right && n2.MagnetDirection == MagnetDirection.Left) isMagnetInverse = true;
if (isMagnetInverse) isInverse = true;
}
if (isInverse)
{
// 제자리 회귀 경로 발견 -> 앞의 두 노드(n1, n2)를 제거하여 n3만 남김
Retval.DetailedPath.RemoveAt(i);
Retval.DetailedPath.RemoveAt(i);
if (Retval.Path.Count > i + 1)
{
Retval.Path.RemoveAt(i);
Retval.Path.RemoveAt(i);
}
i--; // 인덱스 재조정
updatecount += 1;
}
}
}
if (updatecount == 0) break;
}
// 불가능한 회전 경로 검사 (사용자 요청 로직 반영)
for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
{
var n1 = Retval.DetailedPath[i];
var n2 = Retval.DetailedPath[i + 1];
var n3 = Retval.DetailedPath[i + 2];
if (n1.NodeId == n3.NodeId &&
n1.MotorDirection == n3.MotorDirection &&
n1.MotorDirection == n2.MotorDirection) // Fix: 중간 노드 방향도 같을 때만 에러
{
return AGVPathResult.CreateFailure($"불가능한 회전 경로가 포함되어있습니다. {n1.RfidId}->{n2.RfidId}->{n3.RfidId}");
}
}
// 기타 검증 로직 (마지막 노드 도킹, 시작노드 일치 등)
var lastnode = Retval.Path.Last();
if (lastnode.StationType != StationType.Normal)
{
var lastnodePath = Retval.DetailedPath.Last();
if (lastnode.DockDirection == DockingDirection.Forward && lastnodePath.MotorDirection != AgvDirection.Forward)
return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
if (lastnode.DockDirection == DockingDirection.Backward && lastnodePath.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
}
// 첫번째 노드 일치 검사 - 필요시 수행 (startNode가 변경될 수 있어서 o_StartNode 등 필요할 수도 있음)
// 여기서는 생략 혹은 간단히 체크
// 되돌아가는 길 방향 일치 검사
if (Retval.DetailedPath.Count > 1)
{
var FirstDetailPath = Retval.DetailedPath[0];
var NextDetailPath = Retval.DetailedPath[1];
AgvDirection? PredictNextDir = null;
if (NextDetailPath.NodeId == prevNode.Id)
{
if (NextDetailPath.MagnetDirection == MagnetDirection.Straight)
PredictNextDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
}
if (PredictNextDir != null && (FirstDetailPath.MotorDirection != (AgvDirection)PredictNextDir))
{
// return AGVPathResult.CreateFailure($"되돌아가는 길인데 방향이 일치하지않음");
// 경고 수준이나 무시 가능한 경우도 있음
}
}
// 연결성 검사
for (int i = 0; i < Retval.DetailedPath.Count - 1; i++)
{
var cnode = Retval.Path[i];
var nnode = Retval.Path[i + 1];
if (cnode.ConnectedNodes.Contains(nnode.Id) == false && cnode.Id != nnode.Id)
{
return AGVPathResult.CreateFailure($"[{cnode.RfidId}] 노드에 연결되지 않은 [{nnode.RfidId}]노드가 지정됨");
}
}
//각 도킹포인트별로 절대 움직이면 안되는 조건확인
var firstnode = Retval.Path.FirstOrDefault();
var firstDet = Retval.DetailedPath.First();
var failmessage = $"[{firstnode.ID2}] 노드의 시작모터 방향({firstDet.MotorDirection})이 올바르지 않습니다";
if (firstnode.StationType == StationType.Charger1 && firstDet.MotorDirection != AgvDirection.Forward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Loader && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.UnLoader && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Clearner && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Buffer)
{
//버퍼는 도킹이되어잇느닞 확인하고. 그때 방향을 체크해야한다.
}
return Retval;
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"[계산오류] {ex.Message}");
}
}
private AGVPathResult CalcPathBufferToBuffer(MapNode start, MapNode target, MapNode prev, AgvDirection prevDir)
{
// Monitor Side 판단 및 Buffer 간 이동 로직
int deltaX = 0;
int deltaY = 0;
if (prev == null) return AGVPathResult.CreateFailure("이전 노드 정보가 없습니다");
else
{
deltaX = start.Position.X - prev.Position.X;
deltaY = -(start.Position.Y - prev.Position.Y);
}
if (Math.Abs(deltaY) > Math.Abs(deltaX))
deltaX = deltaY;
bool isMonitorLeft = false;
if (deltaX > 0) // 오른쪽(Forward)으로 이동해 옴
isMonitorLeft = (prevDir == AgvDirection.Backward);
else if (deltaX < 0) // 왼쪽(Reverse)으로 이동해 옴
isMonitorLeft = (prevDir == AgvDirection.Forward);
else
return AGVPathResult.CreateFailure("이전 노드와의 방향을 알 수 없습니다");
if (isMonitorLeft)
{
// Monitor Left -> Gateway 탈출
var GateWayNode = _mapNodes.FirstOrDefault(n => n.RfidId == 6);
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
AGVPathResult escPath = null;
if (start.Position.X > prev.Position.X)
escPath = this.FindBasicPath(start, GateWayNode, prev, prevDir);
else
escPath = this.FindBasicPath(start, GateWayNode, prev, reverseDir);
if (!escPath.Success) return AGVPathResult.CreateFailure("버퍼 탈출 경로 실패");
var lastNode = escPath.Path.Last();
var lastPrev = escPath.Path[escPath.Path.Count - 2];
var lastDir = escPath.DetailedPath.Last().MotorDirection;
var gateToTarget = GetPathFromGateway(GateWayNode, target, lastPrev, lastDir);
escPath.Path.RemoveAt(escPath.Path.Count - 1);
escPath.DetailedPath.RemoveAt(escPath.DetailedPath.Count - 1);
return CombinePaths(escPath, gateToTarget);
}
else
{
// Monitor Right -> 직접 진입 또는 Overshoot
bool isTargetLeft = target.Position.X < start.Position.X;
if (target == start)
{
// 제자리 재정렬 (Same as Simulator logic)
var list = new List<MapNode>();
var retval = AGVPathResult.CreateSuccess(list, new List<AgvDirection>(), 0, 0);
var resversedir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
retval.Path.Add(target);
if (deltaX < 0)
{
var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
if (nextNode != null)
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
retval.Path.Add(nextNode);
var lastDefailt = retval.DetailedPath.Last();
retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Forward)
{
Speed = SpeedLevel.M,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo((retval.DetailedPath.Max(t => t.seq) + 1), target.Id, target.RfidId, AgvDirection.Forward));
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
}
else
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, resversedir));
retval.Path.Add(prev);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Last().seq + 1, prev.Id, prev.RfidId, prevDir)
{
Speed = SpeedLevel.M,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, prevDir));
}
}
else
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
retval.Path.Add(nextNode);
var lastDefailt = retval.DetailedPath.Last();
retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Backward)
{
Speed = SpeedLevel.L,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
}
return retval;
}
else if (isTargetLeft)
{
return this.FindBasicPath(start, target, prev, AgvDirection.Backward);
}
else
{
// Overshoot
var path1 = this.FindBasicPath(start, target, prev, AgvDirection.Forward);
if (path1.Path.Count < 2) return AGVPathResult.CreateFailure("Overshoot 경로 생성 실패");
var last = path1.Path.Last();
var lastD = path1.DetailedPath.Last();
path1.Path.RemoveAt(path1.Path.Count - 1);
path1.DetailedPath.RemoveAt(path1.DetailedPath.Count - 1);
path1.Path.Add(last);
path1.DetailedPath.Add(new NodeMotorInfo(lastD.seq + 1, lastD.NodeId, lastD.RfidId, AgvDirection.Backward)
{
Speed = SpeedLevel.L,
});
return path1;
}
}
}
private AGVPathResult GetPathFromGateway(MapNode GTNode, MapNode targetNode, MapNode PrevNode, AgvDirection PrevDirection)
{
AGVPathResult resultPath = null;
var deltaX = GTNode.Position.X - PrevNode.Position.X;
var isMonitorLeft = false;
if (deltaX > 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
if (targetNode.StationType == StationType.Loader || targetNode.StationType == StationType.Charger2)
{
deltaX = GTNode.Position.Y - PrevNode.Position.Y;
if (deltaX < 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
}
switch (targetNode.StationType)
{
case StationType.Loader:
case StationType.Charger2:
case StationType.Charger1:
case StationType.UnLoader:
case StationType.Clearner:
case StationType.Buffer:
var rlt1 = new AGVPathResult();
rlt1.Success = true;
var motdir = targetNode.DockDirection == DockingDirection.Backward ? AgvDirection.Backward : AgvDirection.Forward;
var pathtarget = this.FindBasicPath(GTNode, targetNode, PrevNode, motdir);
if ((targetNode.DockDirection == DockingDirection.Backward && isMonitorLeft) ||
(targetNode.DockDirection == DockingDirection.Forward && !isMonitorLeft))
{
var turnPatterns = GetTurnaroundPattern(GTNode, targetNode);
if (turnPatterns == null || !turnPatterns.Any()) return new AGVPathResult { Success = false, Message = $"회차 패턴 없음: Dir {PrevDirection}" };
foreach (var item in turnPatterns)
{
var rfidvalue = ushort.Parse(item.Substring(0, 4));
var node = _mapNodes.FirstOrDefault(t => t.RfidId == rfidvalue);
rlt1.Path.Add(node);
AgvDirection nodedir = item.Substring(4, 1) == "F" ? AgvDirection.Forward : AgvDirection.Backward;
MagnetDirection magnet = MagnetDirection.Straight;
var magchar = item.Substring(5, 1);
if (magchar == "L") magnet = MagnetDirection.Left;
else if (magchar == "R") magnet = MagnetDirection.Right;
rlt1.DetailedPath.Add(new NodeMotorInfo(rlt1.DetailedPath.Count, node.Id, node.RfidId, nodedir, null, magnet)
{
Speed = SpeedLevel.L,
});
}
if (pathtarget.DetailedPath.First().NodeId != rlt1.DetailedPath.Last().NodeId ||
pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection)
{
// Gateway 턴 마지막 주소 불일치 경고 (로깅 등)
}
pathtarget.Path.RemoveAt(0);
pathtarget.DetailedPath.RemoveAt(0);
}
return CombinePaths(rlt1, pathtarget);
default:
return AGVPathResult.CreateFailure($"지원되지 않는 StationType: {targetNode.StationType}");
}
}
private MapNode GetGatewayNode(MapNode node)
{
var rfid = 0;
if (node.StationType == StationType.UnLoader) rfid = 10;
else if (node.StationType == StationType.Charger1) rfid = 9;
else if (node.StationType == StationType.Clearner) rfid = 6;
else if (node.StationType == StationType.Charger2) rfid = 13;
else if (node.StationType == StationType.Loader) rfid = 13;
else if (node.StationType == StationType.Buffer) rfid = 6;
if (rfid == 0) return null;
return _mapNodes.FirstOrDefault(t => t.RfidId == rfid);
}
private List<string> GetTurnaroundPattern(MapNode gatewayNode, MapNode targetNode)
{
switch (gatewayNode.RfidId)
{
case 6:
if (targetNode.StationType == StationType.Buffer)
return new List<string> { "0006BL", "0007FS", "0013BL", "0006BL" };
else
return new List<string> { "0006BL", "0007FS", "0013BL", "0006BS" };
case 9: return new List<string> { "0009FL", "0010BS", "0007FL", "0009FS" };
case 10: return new List<string> { "0010BR", "0009FR", "0007BS", "0010BS" };
case 13: return new List<string> { "0013BL", "0006FL", "0007BS", "0013BS" };
default: return null;
}
}
private AGVPathResult CombinePaths(AGVPathResult p1, AGVPathResult p2)
{
var res = new AGVPathResult();
res.Success = true;
var p1last = p1.DetailedPath.LastOrDefault();
var p2fist = p2.DetailedPath.FirstOrDefault();
if (p1last != null && p2fist != null &&
(p1last.NodeId == p2fist.NodeId && p1last.MotorDirection == p2fist.MotorDirection && p1last.MagnetDirection == p2fist.MagnetDirection))
{
p1.Path.RemoveAt(p1.Path.Count - 1);
p1.DetailedPath.RemoveAt(p1.DetailedPath.Count - 1);
}
foreach (var item in p1.Path) res.Path.Add(item);
foreach (var item in p2.Path) res.Path.Add(item);
foreach (var item in p1.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
foreach (var item in p2.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
return res;
}
}
}

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

@@ -0,0 +1,71 @@
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<MapNode> DirectionChangePath { get; set; }
public string DirectionChangeNode { get; set; }
public string ErrorMessage { get; set; }
public string PlanDescription { get; set; }
public DirectionChangePlan()
{
DirectionChangePath = new List<MapNode>();
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
}
public static DirectionChangePlan CreateSuccess(List<MapNode> 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);
}
}
}

329
AGVLogic/AGVNavigationCore/PathFinding/Planning/DirectionalPathfinder.cs Normal file
View File

@@ -0,0 +1,329 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// 방향 기반 경로 탐색기
/// 이전 위치 + 현재 위치 + 이동 방향을 기반으로 다음 노드를 결정
/// </summary>
public class DirectionalPathfinder
{
/// <summary>
/// 이동 방향별 가중치
/// </summary>
public class DirectionWeights
{
public float ForwardWeight { get; set; } = 1.0f; // 직진
public float LeftWeight { get; set; } = 1.5f; // 좌측
public float RightWeight { get; set; } = 1.5f; // 우측
public float BackwardWeight { get; set; } = 2.0f; // 후진
}
private readonly DirectionWeights _weights;
public DirectionalPathfinder(DirectionWeights weights = null)
{
_weights = weights ?? new DirectionWeights();
}
/// <summary>
/// 이전 위치와 현재 위치, 그리고 이동 방향을 기반으로 다음 노드 ID를 반환
/// </summary>
/// <param name="previousPos">이전 위치 (이전 RFID 감지 위치)</param>
/// <param name="currentNode">현재 노드 (현재 RFID 노드)</param>
/// <param name="currentPos">현재 위치</param>
/// <param name="direction">이동 방향 (Forward/Backward/Left/Right)</param>
/// <param name="allNodes">맵의 모든 노드</param>
/// <returns>다음 노드 ID (또는 null)</returns>
public string GetNextNodeId(
Point previousPos,
MapNode currentNode,
Point currentPos,
AgvDirection direction,
List<MapNode> allNodes)
{
// 전제조건: 최소 2개 위치 히스토리 필요
if (previousPos == Point.Empty || currentPos == Point.Empty)
{
return null;
}
if (currentNode == null || allNodes == null || allNodes.Count == 0)
{
return null;
}
// 현재 노드에 연결된 노드들 가져오기
var connectedNodeIds = currentNode.ConnectedNodes;
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
{
return null;
}
// 연결된 노드 중 현재 노드가 아닌 것들만 필터링
var candidateNodes = allNodes.Where(n =>
connectedNodeIds.Contains(n.Id) && n.Id != currentNode.Id
).ToList();
if (candidateNodes.Count == 0)
{
return null;
}
// 이전→현재 벡터 계산 (진행 방향 벡터)
var movementVector = new PointF(
currentPos.X - previousPos.X,
currentPos.Y - previousPos.Y
);
// 벡터 정규화
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
if (movementLength < 0.001f) // 거의 이동하지 않음
{
return candidateNodes[0].Id; // 첫 번째 연결 노드 반환
}
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 각 후보 노드에 대해 방향 점수 계산
var scoredCandidates = new List<(MapNode node, float score)>();
foreach (var candidate in candidateNodes)
{
var toNextVector = new PointF(
candidate.Position.X - currentPos.X,
candidate.Position.Y - currentPos.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
if (toNextLength < 0.001f)
{
continue;
}
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 진행 방향 기반 점수 계산
float score = CalculateDirectionalScore(
normalizedMovement,
normalizedToNext,
direction
);
scoredCandidates.Add((candidate, score));
}
if (scoredCandidates.Count == 0)
{
return null;
}
// 가장 높은 점수를 가진 노드 반환
var bestCandidate = scoredCandidates.OrderByDescending(x => x.score).First();
return bestCandidate.node.Id;
}
/// <summary>
/// 이동 방향을 기반으로 방향 점수를 계산
/// 높은 점수 = 더 나은 선택지
/// </summary>
private float CalculateDirectionalScore(
PointF movementDirection, // 정규화된 이전→현재 벡터
PointF nextDirection, // 정규화된 현재→다음 벡터
AgvDirection requestedDir) // 요청된 이동 방향
{
float baseScore = 0;
// 벡터 간 각도 계산 (내적)
float dotProduct = (movementDirection.X * nextDirection.X) +
(movementDirection.Y * nextDirection.Y);
// 외적으로 좌우 판별 (Z 성분)
float crossProduct = (movementDirection.X * nextDirection.Y) -
(movementDirection.Y * nextDirection.X);
switch (requestedDir)
{
case AgvDirection.Forward:
// Forward: 직진 방향 선호 (dotProduct ≈ 1)
if (dotProduct > 0.9f) // 거의 같은 방향
{
baseScore = 100.0f * _weights.ForwardWeight;
}
else if (dotProduct > 0.5f) // 비슷한 방향
{
baseScore = 80.0f * _weights.ForwardWeight;
}
else if (dotProduct > 0.0f) // 약간 다른 방향
{
baseScore = 50.0f * _weights.ForwardWeight;
}
else if (dotProduct > -0.5f) // 거의 반대 방향 아님
{
baseScore = 20.0f * _weights.BackwardWeight;
}
else
{
baseScore = 0.0f; // 완전 반대
}
break;
case AgvDirection.Backward:
// Backward: 역진 방향 선호 (dotProduct ≈ -1)
if (dotProduct < -0.9f) // 거의 반대 방향
{
baseScore = 100.0f * _weights.BackwardWeight;
}
else if (dotProduct < -0.5f) // 비슷하게 반대
{
baseScore = 80.0f * _weights.BackwardWeight;
}
else if (dotProduct < 0.0f) // 약간 다른 방향
{
baseScore = 50.0f * _weights.BackwardWeight;
}
else if (dotProduct < 0.5f) // 거의 같은 방향 아님
{
baseScore = 20.0f * _weights.ForwardWeight;
}
else
{
baseScore = 0.0f; // 완전 같은 방향
}
break;
case AgvDirection.Left:
// Left: 좌측 방향 선호
// Forward 상태에서: crossProduct > 0 = 좌측
// Backward 상태에서: crossProduct < 0 = 좌측 (반대)
if (dotProduct > 0.0f) // Forward 상태
{
// crossProduct > 0이면 좌측
if (crossProduct > 0.5f)
{
baseScore = 100.0f * _weights.LeftWeight;
}
else if (crossProduct > 0.0f)
{
baseScore = 70.0f * _weights.LeftWeight;
}
else if (crossProduct > -0.5f)
{
baseScore = 50.0f * _weights.ForwardWeight;
}
else
{
baseScore = 30.0f * _weights.RightWeight;
}
}
else // Backward 상태 - 좌우 반전
{
// Backward에서 좌측 = crossProduct < 0
if (crossProduct < -0.5f)
{
baseScore = 100.0f * _weights.LeftWeight;
}
else if (crossProduct < 0.0f)
{
baseScore = 70.0f * _weights.LeftWeight;
}
else if (crossProduct < 0.5f)
{
baseScore = 50.0f * _weights.BackwardWeight;
}
else
{
baseScore = 30.0f * _weights.RightWeight;
}
}
break;
case AgvDirection.Right:
// Right: 우측 방향 선호
// Forward 상태에서: crossProduct < 0 = 우측
// Backward 상태에서: crossProduct > 0 = 우측 (반대)
if (dotProduct > 0.0f) // Forward 상태
{
// crossProduct < 0이면 우측
if (crossProduct < -0.5f)
{
baseScore = 100.0f * _weights.RightWeight;
}
else if (crossProduct < 0.0f)
{
baseScore = 70.0f * _weights.RightWeight;
}
else if (crossProduct < 0.5f)
{
baseScore = 50.0f * _weights.ForwardWeight;
}
else
{
baseScore = 30.0f * _weights.LeftWeight;
}
}
else // Backward 상태 - 좌우 반전
{
// Backward에서 우측 = crossProduct > 0
if (crossProduct > 0.5f)
{
baseScore = 100.0f * _weights.RightWeight;
}
else if (crossProduct > 0.0f)
{
baseScore = 70.0f * _weights.RightWeight;
}
else if (crossProduct > -0.5f)
{
baseScore = 50.0f * _weights.BackwardWeight;
}
else
{
baseScore = 30.0f * _weights.LeftWeight;
}
}
break;
}
return baseScore;
}
/// <summary>
/// 벡터 간 각도를 도 단위로 계산
/// </summary>
private float CalculateAngle(PointF vector1, PointF vector2)
{
float dotProduct = (vector1.X * vector2.X) + (vector1.Y * vector2.Y);
float magnitude1 = (float)Math.Sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y);
float magnitude2 = (float)Math.Sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y);
if (magnitude1 < 0.001f || magnitude2 < 0.001f)
{
return 0;
}
float cosAngle = dotProduct / (magnitude1 * magnitude2);
cosAngle = Math.Max(-1.0f, Math.Min(1.0f, cosAngle)); // 범위 제한
return (float)(Math.Acos(cosAngle) * 180.0 / Math.PI);
}
}
}

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

@@ -0,0 +1,131 @@
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>
/// 일련번호
/// </summary>
public int seq { get; set; }
/// <summary>
/// 노드 ID
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// RFID Value
/// </summary>
public ushort RfidId { get; set; }
/// <summary>
/// 해당 노드에서의 모터방향
/// </summary>
public AgvDirection MotorDirection { get; set; }
/// <summary>
/// 해당 노드에서의 제한 속도
/// </summary>
public SpeedLevel Speed { get; set; } = SpeedLevel.M;
/// <summary>
/// 마그넷 센서 방향 제어 (갈림길 처리용)
/// </summary>
public MagnetDirection MagnetDirection { get; set; }
/// <summary>
/// 다음 노드 ID (경로예측용)
/// </summary>
public MapNode NextNode { 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 bool IsPass { get; set; }
/// <summary>
/// 특수 동작 설명
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(int seqno,string nodeId,ushort rfid, AgvDirection motorDirection, MapNode nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
{
seq = seqno;
NodeId = nodeId;
RfidId = rfid;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNode = nextNodeId;
CanRotate = false;
IsDirectionChangePoint = false;
RequiresSpecialAction = false;
SpecialActionDescription = string.Empty;
IsPass = false;
}
/// <summary>
/// 디버깅용 문자열 표현
/// </summary>
public override string ToString()
{
var result = $"R{RfidId}[*{NodeId}]:{MotorDirection}";
// 마그넷 방향이 직진이 아닌 경우 표시
if (MagnetDirection != MagnetDirection.Straight)
result += $"({MagnetDirection})";
if (IsDirectionChangePoint)
result += " [방향전환]";
if (CanRotate)
result += " [회전가능]";
if (RequiresSpecialAction)
result += $" [특수동작:{SpecialActionDescription}]";
if (IsPass) result += "(O)";
return result;
}
}
}

103
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
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})";
}
}
}