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

..

Browse Source
This commit is contained in:
backuppc
2025-10-28 17:33:49 +09:00
parent e8b9fceb88
commit 24a14fbd48
5 changed files with 92 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -128,8 +128,14 @@ namespace AGVNavigationCore.PathFinding.Planning
//정방향/역방향 이동 시 다음 노드 확인
// 경로 계획 단계에서는 마그넷 방향이 미리 알려지지 않으므로 Straight로 기본값 사용
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, MagnetDirection.Straight, _mapNodes);
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes);
// ✅ 현재 방향 유지: prevDirection = currentDirection (방향 일관성)
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(
startNode, prevNode, currentDirection, currentDirection, MagnetDirection.Straight, _mapNodes);
// ✅ 방향 전환: prevDirection = currentDirection, direction = ReverseDirection
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(
startNode, prevNode, currentDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes);
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
@@ -137,7 +143,7 @@ namespace AGVNavigationCore.PathFinding.Planning
(targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
(targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
{
if (nextNodeForward.NodeId == pathResult.Path[1].NodeId) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다
if ((nextNodeForward?.NodeId ?? string.Empty) == pathResult.Path[1].NodeId) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
@@ -160,17 +166,32 @@ namespace AGVNavigationCore.PathFinding.Planning
//뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다.
if (nextNodeBackward != null && pathResult.Path.Count > 1 && nextNodeBackward.NodeId == pathResult.Path[1].NodeId && targetNode.DockDirection == DockingDirection.Backward)
// ⚠️ 단, 현재 방향과 목적지 도킹 방향이 일치해야 함!
if (nextNodeBackward != null && pathResult.Path.Count > 1 &&
nextNodeBackward.NodeId == pathResult.Path[1].NodeId) // ✅ 추가: 현재도 Backward여야 함
{
MakeDetailData(pathResult, ReverseDirection);
MakeMagnetDirection(pathResult);
return pathResult;
if (targetNode.DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward ||
targetNode.DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward)
{
MakeDetailData(pathResult, ReverseDirection);
MakeMagnetDirection(pathResult);
return pathResult;
}
}
if (nextNodeForward != null && pathResult.Path.Count > 1 && nextNodeForward.NodeId == pathResult.Path[1].NodeId && targetNode.DockDirection == DockingDirection.Forward)
if (nextNodeForward != null && pathResult.Path.Count > 1 &&
nextNodeForward.NodeId == pathResult.Path[1].NodeId &&
targetNode.DockDirection == DockingDirection.Forward &&
currentDirection == AgvDirection.Forward) // ✅ 추가: 현재도 Forward여야 함
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
return pathResult;
if (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])
@@ -201,15 +222,16 @@ namespace AGVNavigationCore.PathFinding.Planning
path1.PrevNode = prevNode;
path1.PrevDirection = prevDirection;
//다음좌표를 보고 정방향인지 역방향인지 체크한다.
//다음좌표를 보고 교차로가 진행방향인지 반대방향인지 체크한다.(!모터의 정/역방향을 말하는것이 아님)
bool ReverseCheck = false;
if (path1.Path.Count > 1 && nextNodeForward != null && nextNodeForward.NodeId.Equals(path1.Path[1].NodeId))
{
ReverseCheck = false; //현재 진행 방향으로 이동해야한다
MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else if (path1.Path.Count > 1 && nextNodeBackward != null && nextNodeBackward.NodeId.Equals(path1.Path[1].NodeId))
{
ReverseCheck = true;
ReverseCheck = true; //현재 방향의 반대방향으로 이동해야한다
MakeDetailData(path1, ReverseDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else return AGVPathResult.CreateFailure("교차로까지 계산된 경로에 현재 위치정보로 추측을 할 수 없습니다", 0, 0);
@@ -224,8 +246,17 @@ namespace AGVNavigationCore.PathFinding.Planning
path2.PrevNode = prevNode;
path2.PrevDirection = prevDirection;
if (ReverseCheck) MakeDetailData(path2, currentDirection);
else MakeDetailData(path2, ReverseDirection);
//2번paths느최종 목적지 이므로 목적지와 도킹방향 확인해서 결정한다
if ((path2.Path.Last().DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward) ||
(path2.Path.Last().DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward))
{
MakeDetailData(path2, ReverseDirection);
}
else if ((path2.Path.Last().DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
(path2.Path.Last().DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
{
MakeDetailData(path2, currentDirection);
}
//3.방향전환을 위환 대체 노드찾기
var tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
@@ -236,25 +267,30 @@ namespace AGVNavigationCore.PathFinding.Planning
if (tempNode == null)
return AGVPathResult.CreateFailure("방향 전환을 위한 대체 노드를 찾을 수 없습니다.", 0, 0);
// path1 (시작 → 교차로)
var combinedResult = path1;
// 교차로 → 대체노드 경로 계산
var pathToTemp = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode.NodeId);
pathToTemp.PrevNode = JunctionInPath;
pathToTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
if (!pathToTemp.Success)
return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0);
if (ReverseCheck) MakeDetailData(pathToTemp, ReverseDirection);
else MakeDetailData(pathToTemp, currentDirection);
//교차로찍고 원래방향으로 돌어가야한다.
if (pathToTemp.DetailedPath.Count > 1)
pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = currentDirection;
// path1 + pathToTemp 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp);
// 대체노드 → 교차로 경로 계산 (역방향)
var pathFromTemp = _basicPathfinder.FindPath(tempNode.NodeId, JunctionInPath.NodeId);
pathFromTemp.PrevNode = JunctionInPath;
pathFromTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
if (!pathFromTemp.Success)
return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0);
@@ -264,6 +300,37 @@ namespace AGVNavigationCore.PathFinding.Planning
// (path1 + pathToTemp) + pathFromTemp 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp);
//대체노드에서 최종 목적지를 다시 확인한다.
if ((currentDirection == AgvDirection.Forward && targetNode.DockDirection != DockingDirection.Forward) ||
(currentDirection == AgvDirection.Backward && targetNode.DockDirection != DockingDirection.Backward))
{
//목적지와 방향이 맞지 않다. 그러므로 대체노드를 추가로 더 찾아야한다.
var tempNode2 = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
combinedResult.Path[combinedResult.Path.Count - 2].NodeId,
path2.Path[1].NodeId);
var pathToTemp2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode2.NodeId);
if (ReverseCheck) MakeDetailData(pathToTemp2, currentDirection);
else MakeDetailData(pathToTemp2, ReverseDirection);
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp2);
//교차로찍고 원래방향으로 돌어가야한다.
if (combinedResult.DetailedPath.Count > 1)
{
if (ReverseCheck)
combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
else
combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = currentDirection;
}
var pathToTemp3 = _basicPathfinder.FindPath(tempNode2.NodeId, JunctionInPath.NodeId);
if (ReverseCheck) MakeDetailData(pathToTemp3, ReverseDirection);
else MakeDetailData(pathToTemp3, currentDirection);
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp3);
}
// (path1 + pathToTemp + pathFromTemp) + path2 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, path2);