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

refactor: Convert AGVPathResult.Path from List<string> to List<MapNode>

Browse Source 
## Summary
- Changed Path property type from List<string> to List<MapNode> for better type safety
- Eliminated O(n) node lookups throughout pathfinding system
- Added DirectionalHelper with motor direction consistency bonus/penalty
- Updated all path processing to work with MapNode objects directly

## Files Modified
- AGVPathResult.cs: Path property and CreateSuccess() method signature
- AStarPathfinder.cs: Path generation and node lookup optimization
- AGVPathfinder.cs: Path processing with MapNode objects
- DirectionChangePlanner.cs: Direction change planning with MapNode paths
- DockingValidator.cs: Docking validation with direct node access
- UnifiedAGVCanvas.Events.cs: Path visualization with MapNode iteration
- UnifiedAGVCanvas.cs: Destination node access
- VirtualAGV.cs: Path conversion to string IDs for storage
- DirectionalHelper.cs: Enhanced with motor direction awareness
- NodeMotorInfo.cs: Updated for new path structure

## Benefits
- Performance: Eliminated O(n) lookup searches
- Type Safety: Compile-time checking for node properties
- Code Quality: Direct property access instead of repeated lookups
- Maintainability: Single source of truth for node data

## Build Status
✅ AGVNavigationCore: Build successful (0 errors, 2 warnings)

🤖 Generated with Claude Code
Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
2025-10-27 16:46:13 +09:00
parent dbf81bfc60
commit 735b7dccec
12 changed files with 520 additions and 239 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
View File

@@ -18,9 +18,9 @@ namespace AGVNavigationCore.PathFinding.Core
public bool Success { get; set; }
/// <summary>
/// 경로 노드 ID 목록 (시작 → 목적지 순서)
/// 경로 노드 목록 (시작 → 목적지 순서)
/// </summary>
public List<string> Path { get; set; }
public List<MapNode> Path { get; set; }
/// <summary>
/// AGV 명령어 목록 (이동 방향 시퀀스)
@@ -96,13 +96,19 @@ namespace AGVNavigationCore.PathFinding.Core
/// </summary>
public MapNode PrevNode { get; set; }
/// <summary>
/// PrevNode 에서 현재위치까지 이동한 모터의 방향값
/// </summary>
public AgvDirection PrevDirection { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public AGVPathResult()
{
Success = false;
Path = new List<string>();
Path = new List<MapNode>();
Commands = new List<AgvDirection>();
DetailedPath = new List<NodeMotorInfo>();
TotalDistance = 0;
@@ -116,6 +122,7 @@ namespace AGVNavigationCore.PathFinding.Core
DirectionChangeNode = string.Empty;
DockingValidation = DockingValidationResult.CreateNotRequired();
PrevNode = null;
PrevDirection = AgvDirection.Stop;
}
/// <summary>
@@ -126,12 +133,12 @@ namespace AGVNavigationCore.PathFinding.Core
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<string> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
public static AGVPathResult CreateSuccess(List<MapNode> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<string>(path),
Path = new List<MapNode>(path),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs
@@ -287,7 +294,7 @@ namespace AGVNavigationCore.PathFinding.Core
/// <summary>
/// 단순 경로 목록 반환 (호환성용)
/// 단순 경로 목록 반환 (호환성용 - 노드 ID 문자열 목록)
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetSimplePath()
@@ -296,7 +303,7 @@ namespace AGVNavigationCore.PathFinding.Core
{
return DetailedPath.Select(n => n.NodeId).ToList();
}
return Path ?? new List<string>();
return Path?.Select(n => n.NodeId).ToList() ?? new List<string>();
}
/// <summary>

50
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
View File

@@ -14,6 +14,7 @@ namespace AGVNavigationCore.PathFinding.Core
{
private Dictionary<string, PathNode> _nodeMap;
private List<MapNode> _mapNodes;
private Dictionary<string, MapNode> _mapNodeLookup; // Quick lookup for node ID -> MapNode
/// <summary>
/// 휴리스틱 가중치 (기본값: 1.0)
@@ -33,6 +34,7 @@ namespace AGVNavigationCore.PathFinding.Core
{
_nodeMap = new Dictionary<string, PathNode>();
_mapNodes = new List<MapNode>();
_mapNodeLookup = new Dictionary<string, MapNode>();
}
/// <summary>
@@ -43,10 +45,13 @@ namespace AGVNavigationCore.PathFinding.Core
{
_mapNodes = mapNodes ?? new List<MapNode>();
_nodeMap.Clear();
_mapNodeLookup.Clear();
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
foreach (var mapNode in _mapNodes)
{
_mapNodeLookup[mapNode.NodeId] = mapNode; // Add to lookup table
if (mapNode.IsNavigationNode())
{
var pathNode = new PathNode(mapNode.NodeId, mapNode.Position);
@@ -82,6 +87,14 @@ namespace AGVNavigationCore.PathFinding.Core
}
}
/// <summary>
/// 노드 ID로 MapNode 가져오기 (헬퍼 메서드)
/// </summary>
private MapNode GetMapNode(string nodeId)
{
return _mapNodeLookup.ContainsKey(nodeId) ? _mapNodeLookup[nodeId] : null;
}
/// <summary>
/// 경로 찾기 (A* 알고리즘)
/// </summary>
@@ -106,7 +119,8 @@ namespace AGVNavigationCore.PathFinding.Core
if (startNodeId == endNodeId)
{
var singlePath = new List<string> { startNodeId };
var startMapNode = GetMapNode(startNodeId);
var singlePath = new List<MapNode> { startMapNode };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
@@ -240,7 +254,7 @@ namespace AGVNavigationCore.PathFinding.Core
validWaypoints = deduplicatedWaypoints;
// 최종 경로 리스트와 누적 값
var combinedPath = new List<string>();
var combinedPath = new List<MapNode>();
float totalDistance = 0;
long totalCalculationTime = 0;
@@ -353,23 +367,25 @@ namespace AGVNavigationCore.PathFinding.Core
return previousResult;
// 합친 경로 생성
var combinedPath = new List<string>(previousResult.Path);
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];
string firstNodeOfCurrent = currentResult.Path[0];
string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].NodeId;
string firstNodeOfCurrent = currentResult.Path[0].NodeId;
if (lastNodeOfPrevious == firstNodeOfCurrent)
{
// 첫 번째 노드 제거 (중복 제거)
combinedPath.AddRange(currentResult.Path.Skip(1));
combinedPath.RemoveAt(combinedPath.Count - 1);
combinedPath.AddRange(currentResult.Path);
// DetailedPath도 첫 번째 노드 제거
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
{
combinedDetailedPath.AddRange(currentResult.DetailedPath.Skip(1));
combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1);
combinedDetailedPath.AddRange(currentResult.DetailedPath);
}
}
else
@@ -404,6 +420,7 @@ namespace AGVNavigationCore.PathFinding.Core
// DetailedPath 설정
result.DetailedPath = combinedDetailedPath;
result.PrevNode = previousResult.PrevNode;
result.PrevDirection = previousResult.PrevDirection;
return result;
}
@@ -463,14 +480,18 @@ namespace AGVNavigationCore.PathFinding.Core
/// <summary>
/// 경로 재구성 (부모 노드를 따라 역추적)
/// </summary>
private List<string> ReconstructPath(PathNode endNode)
private List<MapNode> ReconstructPath(PathNode endNode)
{
var path = new List<string>();
var path = new List<MapNode>();
var current = endNode;
while (current != null)
{
path.Add(current.NodeId);
var mapNode = GetMapNode(current.NodeId);
if (mapNode != null)
{
path.Add(mapNode);
}
current = current.Parent;
}
@@ -481,16 +502,19 @@ namespace AGVNavigationCore.PathFinding.Core
/// <summary>
/// 경로의 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<string> path)
private float CalculatePathDistance(List<MapNode> 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]))
var nodeId1 = path[i].NodeId;
var nodeId2 = path[i + 1].NodeId;
if (_nodeMap.ContainsKey(nodeId1) && _nodeMap.ContainsKey(nodeId2))
{
totalDistance += _nodeMap[path[i]].DistanceTo(_nodeMap[path[i + 1]]);
totalDistance += _nodeMap[nodeId1].DistanceTo(_nodeMap[nodeId2]);
}
}

74
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -88,17 +88,16 @@ namespace AGVNavigationCore.PathFinding.Planning
// 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
var StartNode = pathResult.Path.First();
foreach (var nodeId in pathResult.Path)
foreach (var pathNode in pathResult.Path)
{
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (node != null &&
node.IsActive &&
node.IsNavigationNode() &&
node.ConnectedNodes != null &&
node.ConnectedNodes.Count >= 3)
if (pathNode != null &&
pathNode.IsActive &&
pathNode.IsNavigationNode() &&
pathNode.ConnectedNodes != null &&
pathNode.ConnectedNodes.Count >= 3)
{
if (node.NodeId.Equals(StartNode) == false)
return node;
if (pathNode.NodeId.Equals(StartNode.NodeId) == false)
return pathNode;
}
}
@@ -106,7 +105,7 @@ namespace AGVNavigationCore.PathFinding.Planning
}
public AGVPathResult FindPath_test(MapNode startNode, MapNode targetNode,
MapNode prevNode, AgvDirection currentDirection)
MapNode prevNode, AgvDirection prevDirection, AgvDirection currentDirection)
{
// 입력 검증
if (startNode == null)
@@ -127,8 +126,8 @@ namespace AGVNavigationCore.PathFinding.Planning
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
//정방향/역방향 이동 시 다음 노드 확인
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, currentDirection, _mapNodes);
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, ReverseDirection, _mapNodes);
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, _mapNodes);
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, _mapNodes);
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
@@ -136,9 +135,12 @@ namespace AGVNavigationCore.PathFinding.Planning
(targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
(targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
return pathResult;
if (nextNodeForward.NodeId == pathResult.Path[1].NodeId) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
return pathResult;
}
}
@@ -154,15 +156,15 @@ namespace AGVNavigationCore.PathFinding.Planning
// return pathResult;
//}
//뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다.
if (nextNodeBackward.NodeId == pathResult.Path[1] && targetNode.DockDirection == DockingDirection.Backward)
if (nextNodeBackward != null && pathResult.Path.Count > 1 && nextNodeBackward.NodeId == pathResult.Path[1].NodeId && targetNode.DockDirection == DockingDirection.Backward)
{
MakeDetailData(pathResult, ReverseDirection);
MakeMagnetDirection(pathResult);
return pathResult;
}
if(nextNodeForward.NodeId == pathResult.Path[1] && targetNode.DockDirection == DockingDirection.Forward)
if (nextNodeForward != null && pathResult.Path.Count > 1 && nextNodeForward.NodeId == pathResult.Path[1].NodeId && targetNode.DockDirection == DockingDirection.Forward)
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
@@ -197,12 +199,14 @@ namespace AGVNavigationCore.PathFinding.Planning
path1.PrevNode = prevNode;
//다음좌표를 보고 정방향인지 역방향인지 체크한다.
if( nextNodeForward.NodeId.Equals( path1.Path[1]))
bool ReverseCheck = false;
if (path1.Path.Count > 1 && nextNodeForward != null && nextNodeForward.NodeId.Equals(path1.Path[1].NodeId))
{
MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else if(nextNodeBackward.NodeId.Equals(path1.Path[1]))
else if (path1.Path.Count > 1 && nextNodeBackward != null && nextNodeBackward.NodeId.Equals(path1.Path[1].NodeId))
{
ReverseCheck = true;
MakeDetailData(path1, ReverseDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else return AGVPathResult.CreateFailure("교차로까지 계산된 경로에 현재 위치정보로 추측을 할 수 없습니다", 0, 0);
@@ -215,12 +219,13 @@ namespace AGVNavigationCore.PathFinding.Planning
//2.교차로 - 종료위치
var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId);
path2.PrevNode = prevNode;
MakeDetailData(path2, ReverseDirection);
if (ReverseCheck) MakeDetailData(path2, currentDirection);
else MakeDetailData(path2, ReverseDirection);
//3.방향전환을 위환 대체 노드찾기
var tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
path1.Path[path1.Path.Count - 2],
path2.Path[1]);
path1.Path[path1.Path.Count - 2].NodeId,
path2.Path[1].NodeId);
//4. path1 + tempnode + path2 가 최종 위치가 된다.
if (tempNode == null)
@@ -233,7 +238,10 @@ namespace AGVNavigationCore.PathFinding.Planning
var pathToTemp = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode.NodeId);
if (!pathToTemp.Success)
return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0);
MakeDetailData(pathToTemp, currentDirection);
if (ReverseCheck) MakeDetailData(pathToTemp, ReverseDirection);
else MakeDetailData(pathToTemp, currentDirection);
if (pathToTemp.DetailedPath.Count > 1)
pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
@@ -244,7 +252,9 @@ namespace AGVNavigationCore.PathFinding.Planning
var pathFromTemp = _basicPathfinder.FindPath(tempNode.NodeId, JunctionInPath.NodeId);
if (!pathFromTemp.Success)
return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0);
MakeDetailData(pathFromTemp, ReverseDirection);
if (ReverseCheck) MakeDetailData(pathFromTemp, currentDirection);
else MakeDetailData(pathFromTemp, ReverseDirection);
// (path1 + pathToTemp) + pathFromTemp 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp);
@@ -271,12 +281,14 @@ namespace AGVNavigationCore.PathFinding.Planning
var detailedPath1 = new List<NodeMotorInfo>();
for (int i = 0; i < path1.Path.Count; i++)
{
string nodeId = path1.Path[i];
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1] : null;
var node = path1.Path[i];
string nodeId = node.NodeId;
string RfidId = node.RfidId;
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].NodeId : null;
// 노드 정보 생성 (현재 방향 유지)
var nodeInfo = new NodeMotorInfo(
nodeId,
nodeId, RfidId,
currentDirection,
nextNodeId,
MagnetDirection.Straight
@@ -302,13 +314,13 @@ namespace AGVNavigationCore.PathFinding.Planning
for (int i = 0; i < path1.DetailedPath.Count; i++)
{
var detailPath = path1.DetailedPath[i];
string nodeId = path1.Path[i];
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1] : null;
string nodeId = path1.Path[i].NodeId;
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].NodeId : null;
// 마그넷 방향 계산 (3개 이상 연결된 교차로에서만 좌/우 가중치 적용)
if (i > 0 && nextNodeId != null)
{
string prevNodeId = path1.Path[i - 1];
string prevNodeId = path1.Path[i - 1].NodeId;
if (path1.DetailedPath[i - 1].MotorDirection != detailPath.MotorDirection)
detailPath.MagnetDirection = MagnetDirection.Straight;
else

97
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/DirectionChangePlanner.cs
View File

@@ -20,19 +20,19 @@ namespace AGVNavigationCore.PathFinding.Planning
public class DirectionChangePlan
{
public bool Success { get; set; }
public List<string> DirectionChangePath { 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<string>();
DirectionChangePath = new List<MapNode>();
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
}
public static DirectionChangePlan CreateSuccess(List<string> path, string changeNode, string description)
public static DirectionChangePlan CreateSuccess(List<MapNode> path, string changeNode, string description)
{
return new DirectionChangePlan
{
@@ -89,16 +89,16 @@ namespace AGVNavigationCore.PathFinding.Planning
if (directPath2.Success)
{
// 직접 경로에 갈림길이 포함된 경우 그 갈림길에서 방향 전환
foreach (var nodeId in directPath2.Path.Skip(1).Take(directPath2.Path.Count - 2)) // 시작과 끝 제외
foreach (var node in directPath2.Path.Skip(1).Take(directPath2.Path.Count - 2)) // 시작과 끝 제외
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(node.NodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 간단한 방향 전환: 직접 경로 사용하되 방향 전환 노드 표시
return DirectionChangePlan.CreateSuccess(
directPath2.Path,
nodeId,
$"갈림길 {nodeId}에서 방향 전환: {currentDirection} → {requiredDirection}"
node.NodeId,
$"갈림길 {node.NodeId}에서 방향 전환: {currentDirection} → {requiredDirection}"
);
}
}
@@ -163,16 +163,16 @@ namespace AGVNavigationCore.PathFinding.Planning
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath.Success)
{
foreach (var nodeId in directPath.Path.Skip(2)) // 시작점과 다음 노드는 제외
foreach (var node in directPath.Path.Skip(2)) // 시작점과 다음 노드는 제외
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(node.NodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 직진 경로상에서는 더 엄격한 조건 적용
if (!suitableJunctions.Contains(nodeId) &&
HasMultipleExitOptions(nodeId))
if (!suitableJunctions.Contains(node.NodeId) &&
HasMultipleExitOptions(node.NodeId))
{
suitableJunctions.Add(nodeId);
suitableJunctions.Add(node.NodeId);
}
}
}
@@ -290,7 +290,7 @@ namespace AGVNavigationCore.PathFinding.Planning
string actualDirectionChangeNode = FindActualDirectionChangeNode(changePath, junctionNodeId);
string description = $"갈림길 {GetDisplayName(junctionNodeId)}를 통해 {GetDisplayName(actualDirectionChangeNode)}에서 방향 전환: {currentDirection} → {requiredDirection}";
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ✅ 유효한 방향전환 경로: {string.Join(" → ", changePath)}");
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ✅ 유효한 방향전환 경로: {string.Join(" → ", changePath.Select(n => n.NodeId))}");
return DirectionChangePlan.CreateSuccess(changePath, actualDirectionChangeNode, description);
}
@@ -305,9 +305,9 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 방향 전환 경로 생성 (인근 갈림길 우회 방식)
/// </summary>
private List<string> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
private List<MapNode> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
var fullPath = new List<MapNode>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
@@ -316,7 +316,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 2. 인근 갈림길을 통한 우회인지, 직진 경로상 갈림길인지 판단
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
bool isNearbyDetour = !directPath.Success || !directPath.Path.Contains(junctionNodeId);
bool isNearbyDetour = !directPath.Success || !directPath.Path.Any(n => n.NodeId == junctionNodeId);
if (isNearbyDetour)
{
@@ -333,9 +333,9 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 인근 갈림길을 통한 우회 경로 생성 (예: 012 → 013 → 마그넷으로 016 방향)
/// </summary>
private List<string> GenerateNearbyDetourPath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
private List<MapNode> GenerateNearbyDetourPath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
var fullPath = new List<MapNode>();
// 1. 시작점에서 갈림길까지 직진 (현재 방향 유지)
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
@@ -358,9 +358,9 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 직진 경로상 갈림길에서 방향 전환 경로 생성 (기존 방식 개선)
/// </summary>
private List<string> GenerateDirectPathChangeRoute(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
private List<MapNode> GenerateDirectPathChangeRoute(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
var fullPath = new List<MapNode>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
@@ -373,17 +373,17 @@ namespace AGVNavigationCore.PathFinding.Planning
if (currentDirection != requiredDirection)
{
string fromNodeId = toJunctionPath.Path.Count >= 2 ?
toJunctionPath.Path[toJunctionPath.Path.Count - 2] : startNodeId;
toJunctionPath.Path[toJunctionPath.Path.Count - 2].NodeId : startNodeId;
var changeSequence = GenerateDirectionChangeSequence(junctionNodeId, fromNodeId, currentDirection, requiredDirection);
if (changeSequence.Count > 1)
{
fullPath.AddRange(changeSequence.Skip(1));
fullPath.AddRange(changeSequence.Skip(1).Select(nodeId => _mapNodes.FirstOrDefault(n => n.NodeId == nodeId)).Where(n => n != null));
}
}
// 3. 갈림길에서 목표점까지의 경로
string lastNode = fullPath.LastOrDefault() ?? junctionNodeId;
string lastNode = fullPath.LastOrDefault()?.NodeId ?? junctionNodeId;
var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
@@ -549,7 +549,7 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 실제 방향 전환이 일어나는 노드 찾기
/// </summary>
private string FindActualDirectionChangeNode(List<string> changePath, string junctionNodeId)
private string FindActualDirectionChangeNode(List<MapNode> changePath, string junctionNodeId)
{
// 방향전환 경로 구조: [start...junction, detourNode, junction...target]
// 실제 방향전환은 detourNode에서 일어남 (AGV가 한 태그 더 지나간 후)
@@ -558,14 +558,23 @@ namespace AGVNavigationCore.PathFinding.Planning
return junctionNodeId; // 기본값으로 갈림길 반환
// 갈림길이 두 번 나타나는 위치 찾기
int firstJunctionIndex = changePath.IndexOf(junctionNodeId);
int lastJunctionIndex = changePath.LastIndexOf(junctionNodeId);
int firstJunctionIndex = changePath.FindIndex(n => n.NodeId == junctionNodeId);
int lastJunctionIndex = -1;
for (int i = changePath.Count - 1; i >= 0; i--)
{
if (changePath[i].NodeId == junctionNodeId)
{
lastJunctionIndex = i;
break;
}
}
// 갈림길이 두 번 나타나고, 그 사이에 노드가 있는 경우
if (firstJunctionIndex != lastJunctionIndex && lastJunctionIndex - firstJunctionIndex == 2)
if (firstJunctionIndex != -1 && lastJunctionIndex != -1 &&
firstJunctionIndex != lastJunctionIndex && lastJunctionIndex - firstJunctionIndex == 2)
{
// 첫 번째와 두 번째 갈림길 사이에 있는 노드가 실제 방향전환 노드
string detourNode = changePath[firstJunctionIndex + 1];
string detourNode = changePath[firstJunctionIndex + 1].NodeId;
return detourNode;
}
@@ -617,7 +626,7 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 방향전환 경로 검증 - 되돌아가기 패턴 및 물리적 실현성 검증
/// </summary>
private PathValidationResult ValidateDirectionChangePath(List<string> path, string startNodeId, string junctionNodeId)
private PathValidationResult ValidateDirectionChangePath(List<MapNode> path, string startNodeId, string junctionNodeId)
{
if (path == null || path.Count == 0)
{
@@ -635,11 +644,11 @@ namespace AGVNavigationCore.PathFinding.Planning
}
string errorMessage = $"되돌아가기 패턴 검출 ({backtrackingPatterns.Count}개): {string.Join(", ", issues)}";
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 경로: {string.Join(" → ", path)}");
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 경로: {string.Join(" → ", path.Select(n => n.NodeId))}");
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 되돌아가기 패턴: {errorMessage}");
return PathValidationResult.CreateInvalidWithBacktracking(
path, backtrackingPatterns, startNodeId, "", junctionNodeId, errorMessage);
path.Select(n => n.NodeId).ToList(), backtrackingPatterns, startNodeId, "", junctionNodeId, errorMessage);
}
// 2. 연속된 중복 노드 검증
@@ -658,27 +667,27 @@ namespace AGVNavigationCore.PathFinding.Planning
}
// 4. 갈림길 포함 여부 검증
if (!path.Contains(junctionNodeId))
if (!path.Any(n => n.NodeId == junctionNodeId))
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"갈림길 {junctionNodeId}이 경로에 포함되지 않음");
}
System.Diagnostics.Debug.WriteLine($"[PathValidation] ✅ 유효한 경로: {string.Join(" → ", path)}");
return PathValidationResult.CreateValid(path, startNodeId, "", junctionNodeId);
System.Diagnostics.Debug.WriteLine($"[PathValidation] ✅ 유효한 경로: {string.Join(" → ", path.Select(n => n.NodeId))}");
return PathValidationResult.CreateValid(path.Select(n => n.NodeId).ToList(), startNodeId, "", junctionNodeId);
}
/// <summary>
/// 되돌아가기 패턴 검출 (A → B → A)
/// </summary>
private List<BacktrackingPattern> DetectBacktrackingPatterns(List<string> path)
private List<BacktrackingPattern> DetectBacktrackingPatterns(List<MapNode> 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];
string nodeA = path[i].NodeId;
string nodeB = path[i + 1].NodeId;
string nodeC = path[i + 2].NodeId;
// A → B → A 패턴 검출
if (nodeA == nodeC && nodeA != nodeB)
@@ -694,15 +703,15 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 연속된 중복 노드 검출
/// </summary>
private List<string> DetectConsecutiveDuplicates(List<string> path)
private List<string> DetectConsecutiveDuplicates(List<MapNode> path)
{
var duplicates = new List<string>();
for (int i = 0; i < path.Count - 1; i++)
{
if (path[i] == path[i + 1])
if (path[i].NodeId == path[i + 1].NodeId)
{
duplicates.Add(path[i]);
duplicates.Add(path[i].NodeId);
}
}
@@ -712,12 +721,12 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 경로 연결성 검증
/// </summary>
private PathValidationResult ValidatePathConnectivity(List<string> path)
private PathValidationResult ValidatePathConnectivity(List<MapNode> path)
{
for (int i = 0; i < path.Count - 1; i++)
{
string currentNode = path[i];
string nextNode = path[i + 1];
string currentNode = path[i].NodeId;
string nextNode = path[i + 1].NodeId;
// 두 노드간 직접 연결성 확인 (맵 노드의 ConnectedNodes 리스트 사용)
var currentMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNode);

26
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs
View File

@@ -33,6 +33,11 @@ namespace AGVNavigationCore.PathFinding.Planning
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// RFID Value
/// </summary>
public string RfidId { get; set; }
/// <summary>
/// 해당 노드에서의 모터방향
/// </summary>
@@ -68,9 +73,10 @@ namespace AGVNavigationCore.PathFinding.Planning
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
public NodeMotorInfo(string nodeId,string rfid, AgvDirection motorDirection, string nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
{
NodeId = nodeId;
RfidId = rfid;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNodeId = nextNodeId;
@@ -80,27 +86,13 @@ namespace AGVNavigationCore.PathFinding.Planning
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}";
var result = $"{RfidId}[{NodeId}]:{MotorDirection}";
// 마그넷 방향이 직진이 아닌 경우 표시
if (MagnetDirection != MagnetDirection.Straight)