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

agv 노드 정보 정리 세분화

Browse Source
This commit is contained in:
backuppc
2025-12-09 13:18:22 +09:00
parent 455e18f427
commit 9db031c305
28 changed files with 992 additions and 1476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -101,7 +101,7 @@ namespace AGVNavigationCore.PathFinding.Core
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindPath(string startNodeId, string endNodeId)
public AGVPathResult FindPathAStar(string startNodeId, string endNodeId)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
@@ -126,7 +126,6 @@ namespace AGVNavigationCore.PathFinding.Core
var startNode = _nodeMap[startNodeId];
var endNode = _nodeMap[endNodeId];
var openSet = new List<PathNode>();
var closedSet = new HashSet<string>();
var exploredCount = 0;
@@ -199,7 +198,7 @@ namespace AGVNavigationCore.PathFinding.Core
// 경유지가 없으면 기본 FindPath 호출
if (waypointNodeIds == null || waypointNodeIds.Length == 0)
{
return FindPath(startNodeId, endNodeId);
return FindPathAStar(startNodeId, endNodeId);
}
// 경유지 유효성 검증
@@ -220,7 +219,7 @@ namespace AGVNavigationCore.PathFinding.Core
// 경유지가 없으면 기본 경로 계산
if (validWaypoints.Count == 0)
{
return FindPath(startNodeId, endNodeId);
return FindPathAStar(startNodeId, endNodeId);
}
// 첫 번째 경유지가 시작노드와 같은지 검사
@@ -267,7 +266,7 @@ namespace AGVNavigationCore.PathFinding.Core
string waypoint = validWaypoints[i];
// 현재 위치에서 경유지까지의 경로 계산
var segmentResult = FindPath(currentStart, waypoint);
var segmentResult = FindPathAStar(currentStart, waypoint);
if (!segmentResult.Success)
{
@@ -295,7 +294,7 @@ namespace AGVNavigationCore.PathFinding.Core
}
// 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
var finalSegmentResult = FindPath(currentStart, endNodeId);
var finalSegmentResult = FindPathAStar(currentStart, endNodeId);
if (!finalSegmentResult.Success)
{
@@ -442,7 +441,7 @@ namespace AGVNavigationCore.PathFinding.Core
AGVPathResult bestResult = null;
foreach (var targetId in targetNodeIds)
{
var result = FindPath(startNodeId, targetId);
var result = FindPathAStar(startNodeId, targetId);
if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
{
bestResult = result;

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

@@ -22,8 +22,6 @@ namespace AGVNavigationCore.PathFinding.Planning
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly DirectionChangePlanner _directionChangePlanner;
public AGVPathfinder(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
@@ -49,8 +47,9 @@ namespace AGVNavigationCore.PathFinding.Planning
n.IsActive &&
n.IsNavigationNode() &&
n.ConnectedNodes != null &&
n.DisableCross == false &&
n.ConnectedNodes.Count >= 3 &&
n.ConnectedMapNodes.Where(t => t.Type == NodeType.Docking || t.Type == NodeType.Charging).Any() == false &&
n.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false &&
n.NodeId != startNode.NodeId
).ToList();
@@ -99,9 +98,10 @@ namespace AGVNavigationCore.PathFinding.Planning
if (pathNode != null &&
pathNode.IsActive &&
pathNode.IsNavigationNode() &&
pathNode.DisableCross == false &&
pathNode.ConnectedNodes != null &&
pathNode.ConnectedNodes.Count >= 3 &&
pathNode.ConnectedMapNodes.Where(t => t.Type == NodeType.Docking || t.Type == NodeType.Charging).Any() == false)
pathNode.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false)
{
if (pathNode.NodeId.Equals(StartNode.NodeId) == false)
return pathNode;
@@ -122,12 +122,12 @@ namespace AGVNavigationCore.PathFinding.Planning
if (prevNode == null)
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
if (startNode.NodeId == targetNode.NodeId && targetNode.DockDirection.MatchAGVDirection(prevDirection))
return AGVPathResult.CreateSuccess(new List<MapNode> { startNode,startNode }, new List<AgvDirection>(), 0, 0);
return AGVPathResult.CreateSuccess(new List<MapNode> { startNode, startNode }, new List<AgvDirection>(), 0, 0);
var ReverseDirection = (currentDirection == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
//1.목적지까지의 최단거리 경로를 찾는다.
var pathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
var pathResult = _basicPathfinder.FindPathAStar(startNode.NodeId, targetNode.NodeId);
pathResult.PrevNode = prevNode;
pathResult.PrevDirection = prevDirection;
if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
@@ -221,7 +221,7 @@ namespace AGVNavigationCore.PathFinding.Planning
//진행방향으로 이동했을때 나오는 노드를 사용한다.
if (nextNodeForward != null)
{
var Path0 = _basicPathfinder.FindPath(startNode.NodeId, nextNodeForward.NodeId);
var Path0 = _basicPathfinder.FindPathAStar(startNode.NodeId, nextNodeForward.NodeId);
Path0.PrevNode = prevNode;
Path0.PrevDirection = prevDirection;
MakeDetailData(Path0, prevDirection);
@@ -267,7 +267,7 @@ namespace AGVNavigationCore.PathFinding.Planning
//경유지를 포함하여 경로를 다시 계산한다.
//1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
var path1 = _basicPathfinder.FindPath(startNode.NodeId, JunctionInPath.NodeId);
var path1 = _basicPathfinder.FindPathAStar(startNode.NodeId, JunctionInPath.NodeId);
path1.PrevNode = prevNode;
path1.PrevDirection = prevDirection;
@@ -295,7 +295,7 @@ namespace AGVNavigationCore.PathFinding.Planning
//2.교차로 - 종료위치
var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId);
var path2 = _basicPathfinder.FindPathAStar(JunctionInPath.NodeId, targetNode.NodeId);
path2.PrevNode = prevNode;
path2.PrevDirection = prevDirection;
@@ -332,7 +332,7 @@ namespace AGVNavigationCore.PathFinding.Planning
//if (tempNode != null)
{
// 교차로 → 대체노드 경로 계산
var pathToTemp = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode.NodeId);
var pathToTemp = _basicPathfinder.FindPathAStar(JunctionInPath.NodeId, tempNode.NodeId);
pathToTemp.PrevNode = JunctionInPath;
pathToTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
if (!pathToTemp.Success)
@@ -348,7 +348,7 @@ namespace AGVNavigationCore.PathFinding.Planning
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp);
// 대체노드 → 교차로 경로 계산 (역방향)
var pathFromTemp = _basicPathfinder.FindPath(tempNode.NodeId, JunctionInPath.NodeId);
var pathFromTemp = _basicPathfinder.FindPathAStar(tempNode.NodeId, JunctionInPath.NodeId);
pathFromTemp.PrevNode = JunctionInPath;
pathFromTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
if (!pathFromTemp.Success)
@@ -362,7 +362,7 @@ namespace AGVNavigationCore.PathFinding.Planning
//현재까지 노드에서 목적지까지의 방향이 일치하면 그대로 사용한다.
bool temp3ok = false;
var TempCheck3 = _basicPathfinder.FindPath(combinedResult.Path.Last().NodeId, targetNode.NodeId);
var TempCheck3 = _basicPathfinder.FindPathAStar(combinedResult.Path.Last().NodeId, targetNode.NodeId);
if (TempCheck3.Path.First().NodeId.Equals(combinedResult.Path.Last().NodeId))
{
if (targetNode.DockDirection == DockingDirection.Forward && combinedResult.DetailedPath.Last().MotorDirection == AgvDirection.Forward)
@@ -385,7 +385,7 @@ namespace AGVNavigationCore.PathFinding.Planning
combinedResult.Path[combinedResult.Path.Count - 2].NodeId,
path2.Path[1].NodeId);
var pathToTemp2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode2.NodeId);
var pathToTemp2 = _basicPathfinder.FindPathAStar(JunctionInPath.NodeId, tempNode2.NodeId);
if (ReverseCheck) MakeDetailData(pathToTemp2, currentDirection);
else MakeDetailData(pathToTemp2, ReverseDirection);
@@ -400,7 +400,7 @@ namespace AGVNavigationCore.PathFinding.Planning
combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = currentDirection;
}
var pathToTemp3 = _basicPathfinder.FindPath(tempNode2.NodeId, JunctionInPath.NodeId);
var pathToTemp3 = _basicPathfinder.FindPathAStar(tempNode2.NodeId, JunctionInPath.NodeId);
if (ReverseCheck) MakeDetailData(pathToTemp3, ReverseDirection);
else MakeDetailData(pathToTemp3, currentDirection);
@@ -438,7 +438,7 @@ namespace AGVNavigationCore.PathFinding.Planning
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].NodeId : null;
// 노드 정보 생성 (현재 방향 유지)
var nodeInfo = new NodeMotorInfo(i+1,
var nodeInfo = new NodeMotorInfo(i + 1,
nodeId, RfidId,
currentDirection,
nextNodeId,

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

@@ -73,7 +73,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 방향이 같으면 직접 경로 계산
if (currentDirection == requiredDirection)
{
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
var directPath = _pathfinder.FindPathAStar(startNodeId, targetNodeId);
if (directPath.Success)
{
return DirectionChangePlan.CreateSuccess(
@@ -85,7 +85,7 @@ namespace AGVNavigationCore.PathFinding.Planning
}
// 방향 전환이 필요한 경우 - 먼저 간단한 직접 경로 확인
var directPath2 = _pathfinder.FindPath(startNodeId, targetNodeId);
var directPath2 = _pathfinder.FindPathAStar(startNodeId, targetNodeId);
if (directPath2.Success)
{
// 직접 경로에 갈림길이 포함된 경우 그 갈림길에서 방향 전환
@@ -160,7 +160,7 @@ namespace AGVNavigationCore.PathFinding.Planning
}
// 2. 직진 경로상의 갈림길들도 검색 (단, 되돌아가기 방지)
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
var directPath = _pathfinder.FindPathAStar(startNodeId, targetNodeId);
if (directPath.Success)
{
foreach (var node in directPath.Path.Skip(2)) // 시작점과 다음 노드는 제외
@@ -261,7 +261,7 @@ namespace AGVNavigationCore.PathFinding.Planning
foreach (var junction in junctions)
{
var path = _pathfinder.FindPath(startNodeId, junction);
var path = _pathfinder.FindPathAStar(startNodeId, junction);
double distance = path.Success ? path.TotalDistance : double.MaxValue;
distances.Add((junction, distance));
}
@@ -313,12 +313,12 @@ namespace AGVNavigationCore.PathFinding.Planning
var fullPath = new List<MapNode>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
var toJunctionPath = _pathfinder.FindPathAStar(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
// 2. 인근 갈림길을 통한 우회인지, 직진 경로상 갈림길인지 판단
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
var directPath = _pathfinder.FindPathAStar(startNodeId, targetNodeId);
bool isNearbyDetour = !directPath.Success || !directPath.Path.Any(n => n.NodeId == junctionNodeId);
if (isNearbyDetour)
@@ -341,7 +341,7 @@ namespace AGVNavigationCore.PathFinding.Planning
var fullPath = new List<MapNode>();
// 1. 시작점에서 갈림길까지 직진 (현재 방향 유지)
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
var toJunctionPath = _pathfinder.FindPathAStar(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
@@ -349,7 +349,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 2. 갈림길에서 방향 전환 후 목적지로
// 이때 마그넷 센서를 이용해 목적지 방향으로 진입
var fromJunctionPath = _pathfinder.FindPath(junctionNodeId, targetNodeId);
var fromJunctionPath = _pathfinder.FindPathAStar(junctionNodeId, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
@@ -366,7 +366,7 @@ namespace AGVNavigationCore.PathFinding.Planning
var fullPath = new List<MapNode>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
var toJunctionPath = _pathfinder.FindPathAStar(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
@@ -387,7 +387,7 @@ namespace AGVNavigationCore.PathFinding.Planning
// 3. 갈림길에서 목표점까지의 경로
string lastNode = fullPath.LastOrDefault()?.NodeId ?? junctionNodeId;
var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
var fromJunctionPath = _pathfinder.FindPathAStar(lastNode, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1));
@@ -609,7 +609,7 @@ namespace AGVNavigationCore.PathFinding.Planning
private bool CanReachTargetViaJunction(string junctionNodeId, string targetNodeId)
{
// 갈림길에서 목적지까지의 경로가 존재하는지 확인
var pathToTarget = _pathfinder.FindPath(junctionNodeId, targetNodeId);
var pathToTarget = _pathfinder.FindPathAStar(junctionNodeId, targetNodeId);
return pathToTarget.Success;
}