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

add files

Browse Source
This commit is contained in:
ChiKyun Kim
2025-09-15 17:36:46 +09:00
parent 25f28b26b0
commit debbf712d4
7 changed files with 194 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

122
Cs_HMI/AGVNavigationCore/PathFinding/AGVPathfinder.cs
View File

@@ -717,73 +717,107 @@ namespace AGVNavigationCore.PathFinding
var startNode = _nodeMap[startNodeId];
// 시작 노드에서 회전이 불가능하면 회전 가능한 가까운 노드 찾기
if (!startNode.CanRotate)
// 방향 전환을 위한 가장 가까운 교차로 찾기
var targetNodeId = originalResult.Path.LastOrDefault();
var junctionNode = FindNearestJunctionForDirectionChange(startNodeId, targetNodeId);
if (junctionNode == null)
{
var rotationNode = FindNearestRotationNode(startNodeId);
if (rotationNode == null)
{
return AGVPathResult.CreateFailure("방향 전환을 위한 회전 가능 노드를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 회전 노드로의 경로 추가
var pathToRotationNode = _pathfinder.FindPath(startNodeId, rotationNode.NodeId);
if (!pathToRotationNode.Success)
{
return AGVPathResult.CreateFailure("방향 전환 노드로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 회전 노드에서 원래 목적지로의 경로 계산
var pathFromRotationNode = _pathfinder.FindPath(rotationNode.NodeId, originalResult.Path.Last());
if (!pathFromRotationNode.Success)
{
return AGVPathResult.CreateFailure("방향 전환 후 목적지로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 전체 경로 조합
var combinedPath = new List<string>();
combinedPath.AddRange(pathToRotationNode.Path);
combinedPath.AddRange(pathFromRotationNode.Path.Skip(1)); // 중복 노드 제거
var combinedDistance = pathToRotationNode.TotalDistance + pathFromRotationNode.TotalDistance;
var combinedCommands = GenerateAGVCommandsWithDirectionChange(combinedPath, currentDirection, targetDirection, rotationNode.NodeId);
var nodeMotorInfos = GenerateNodeMotorInfos(combinedPath);
return AGVPathResult.CreateSuccess(combinedPath, combinedCommands, nodeMotorInfos, combinedDistance, originalResult.CalculationTimeMs);
return AGVPathResult.CreateFailure("방향 전환을 위한 교차로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
else
// 교차로로의 경로 추가
var pathToJunction = _pathfinder.FindPath(startNodeId, junctionNode.NodeId);
if (!pathToJunction.Success)
{
// 시작 노드에서 바로 방향 전환
var commandsWithRotation = GenerateAGVCommandsWithDirectionChange(originalResult.Path, currentDirection, targetDirection, startNodeId);
return AGVPathResult.CreateSuccess(originalResult.Path, commandsWithRotation, originalResult.NodeMotorInfos, originalResult.TotalDistance, originalResult.CalculationTimeMs);
return AGVPathResult.CreateFailure("방향 전환 교차로로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 교차로에서 원래 목적지로의 경로 계산
var pathFromJunction = _pathfinder.FindPath(junctionNode.NodeId, originalResult.Path.Last());
if (!pathFromJunction.Success)
{
return AGVPathResult.CreateFailure("방향 전환 후 목적지로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
}
// 전체 경로 조합
var combinedPath = new List<string>();
combinedPath.AddRange(pathToJunction.Path);
combinedPath.AddRange(pathFromJunction.Path.Skip(1)); // 중복 노드 제거
var combinedDistance = pathToJunction.TotalDistance + pathFromJunction.TotalDistance;
var combinedCommands = GenerateAGVCommandsWithDirectionChange(combinedPath, currentDirection, targetDirection, junctionNode.NodeId);
var nodeMotorInfos = GenerateNodeMotorInfos(combinedPath);
return AGVPathResult.CreateSuccess(combinedPath, combinedCommands, nodeMotorInfos, combinedDistance, originalResult.CalculationTimeMs);
}
/// <summary>
/// 가장 가까운 회전 가능 노드 찾기
/// 방향 전환을 위한 가장 가까운 교차로 찾기
/// </summary>
/// <param name="fromNodeId">시작 노드 ID</param>
/// <returns>가장 가까운 회전 가능 노드</returns>
private MapNode FindNearestRotationNode(string fromNodeId)
/// <param name="targetNodeId">목적지 노드 ID (경로상 교차로 우선 검색용)</param>
/// <returns>가장 가까운 교차로 노드</returns>
private MapNode FindNearestJunctionForDirectionChange(string fromNodeId, string targetNodeId = null)
{
var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate && n.IsActive).ToList();
// 1단계: 시작점에서 목적지까지 직접 경로상의 교차로 우선 검색
if (!string.IsNullOrEmpty(targetNodeId))
{
var directPath = _pathfinder.FindPath(fromNodeId, targetNodeId);
if (directPath.Success)
{
foreach (var nodeId in directPath.Path)
{
var node = _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
if (node != null && IsJunction(node))
{
return node;
}
}
}
}
// 2단계: 경로상에 교차로가 없으면 거리가 가장 가까운 교차로 찾기
var junctionNodes = _nodeMap.Values.Where(n => n.IsActive && IsJunction(n)).ToList();
MapNode nearestNode = null;
var shortestDistance = float.MaxValue;
foreach (var rotationNode in rotationNodes)
foreach (var junctionNode in junctionNodes)
{
var pathResult = _pathfinder.FindPath(fromNodeId, rotationNode.NodeId);
var pathResult = _pathfinder.FindPath(fromNodeId, junctionNode.NodeId);
if (pathResult.Success && pathResult.TotalDistance < shortestDistance)
{
shortestDistance = pathResult.TotalDistance;
nearestNode = rotationNode;
nearestNode = junctionNode;
}
}
return nearestNode;
}
/// <summary>
/// 노드가 교차로인지 판단 (3개 이상 연결된 노드)
/// </summary>
/// <param name="node">검사할 노드</param>
/// <returns>교차로이면 true</returns>
private bool IsJunction(MapNode node)
{
// 연결된 노드 수 계산 (단방향 + 양방향)
var connectedCount = node.ConnectedNodes.Count;
// 역방향 연결도 고려
foreach (var otherNode in _nodeMap.Values)
{
if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
{
connectedCount++;
}
}
// 3개 이상 연결되어 있으면 교차로
return connectedCount >= 3;
}
/// <summary>
/// 방향 전환을 포함한 AGV 명령어 생성
/// </summary>