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

..

Browse Source
This commit is contained in:
backuppc
2026-01-14 15:29:38 +09:00
parent 5801137d63
commit d5516f9708
17 changed files with 605 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -126,7 +126,7 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// 단순 경로 찾기 (복잡한 제약조건/방향전환 로직 없이 A* 결과만 반환)
/// </summary>
public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDirection)
public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode _prevNode, AgvDirection prevDirection)
{
// 1. 입력 검증
if (startNode == null || targetNode == null)
@@ -134,13 +134,13 @@ namespace AGVNavigationCore.PathFinding.Planning
// 2. A* 경로 탐색
var pathResult = _basicPathfinder.FindPathAStar(startNode, targetNode);
pathResult.PrevNode = prevNode;
pathResult.PrevNode = _prevNode;
pathResult.PrevDirection = prevDirection;
if (!pathResult.Success)
return AGVPathResult.CreateFailure(pathResult.ErrorMessage ?? "경로 없음", 0, 0);
// 3. 상세 데이터 생성 (단순화: 방향 전환 없이 현재 방향 유지)
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
if (pathResult.Path != null && pathResult.Path.Count > 0)
{
var detailedPath = new List<NodeMotorInfo>();
@@ -149,8 +149,47 @@ namespace AGVNavigationCore.PathFinding.Planning
var node = pathResult.Path[i];
string nextNodeId = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1].Id : null;
// 단순화: 입력된 현재 방향을 그대로 사용
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNodeId, MagnetDirection.Straight);
// 마그넷 방향 계산 (갈림길인 경우)
MagnetDirection magnetDirection = MagnetDirection.Straight;
if (node.ConnectedNodes != null && node.ConnectedNodes.Count >= 3 && i > 0 && nextNodeId != null)
{
// 갈림길인 경우: 진입 방향과 진출 방향의 각도 계산
var prevNode = pathResult.Path[i - 1];
var nextNode = pathResult.Path[i + 1];
// 진입 각도 계산 (이전 노드 → 현재 노드)
double entryAngle = Math.Atan2(
node.Position.Y - prevNode.Position.Y,
node.Position.X - prevNode.Position.X
) * 180.0 / Math.PI;
// 진출 각도 계산 (현재 노드 → 다음 노드)
double exitAngle = Math.Atan2(
nextNode.Position.Y - node.Position.Y,
nextNode.Position.X - node.Position.X
) * 180.0 / Math.PI;
// 각도 차이 계산 (-180~180 범위로 정규화)
double angleDiff = exitAngle - entryAngle;
while (angleDiff > 180) angleDiff -= 360;
while (angleDiff < -180) angleDiff += 360;
// 10도 이상 차이나면 좌/우회전 처리
if (Math.Abs(angleDiff) >= 10)
{
if (angleDiff > 0)
{
magnetDirection = MagnetDirection.Right;
}
else
{
magnetDirection = MagnetDirection.Left;
}
}
}
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNodeId, magnetDirection);
// 속도 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);
@@ -164,75 +203,7 @@ namespace AGVNavigationCore.PathFinding.Planning
return pathResult;
}
/// <summary>
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
/// </summary>
/// <param name="path1"></param>
/// <param name="currentDirection"></param>
public void MakeDetailData(AGVPathResult path1, AgvDirection currentDirection)
{
if (path1.Success && path1.Path != null && path1.Path.Count > 0)
{
var detailedPath1 = new List<NodeMotorInfo>();
for (int i = 0; i < path1.Path.Count; i++)
{
var node = path1.Path[i];
string nodeId = node.Id;
var RfidId = node.RfidId;
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].Id : null;
// 노드 정보 생성 (현재 방향 유지)
var nodeInfo = new NodeMotorInfo(i + 1,
nodeId, RfidId,
currentDirection,
nextNodeId,
MagnetDirection.Straight
);
// [Speed Control] MapNode의 속도 설정 적용
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == nodeId);
if (mapNode != null)
{
nodeInfo.Speed = mapNode.SpeedLimit;
}
detailedPath1.Add(nodeInfo);
}
// path1에 상세 경로 정보 설정
path1.DetailedPath = detailedPath1;
}
}
/// <summary>
/// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다
/// </summary>
/// <param name="path1"></param>
private void MakeMagnetDirection(AGVPathResult path1)
{
if (path1.Success && path1.DetailedPath != null && path1.DetailedPath.Count > 0)
{
for (int i = 0; i < path1.DetailedPath.Count; i++)
{
var detailPath = path1.DetailedPath[i];
string nodeId = path1.Path[i].Id;
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1].Id : null;
// 마그넷 방향 계산 (3개 이상 연결된 교차로에서만 좌/우 가중치 적용)
if (i > 0 && nextNodeId != null)
{
string prevNodeId = path1.Path[i - 1].Id;
if (path1.DetailedPath[i - 1].MotorDirection != detailPath.MotorDirection)
detailPath.MagnetDirection = MagnetDirection.Straight;
else
detailPath.MagnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, nodeId, nextNodeId, detailPath.MotorDirection);
}
else detailPath.MagnetDirection = MagnetDirection.Straight;
}
}
}
}