fix: RFID duplicate validation and correct magnet direction calculation
- Add real-time RFID duplicate validation in map editor with automatic rollback - Remove RFID auto-assignment to maintain data consistency between editor and simulator - Fix magnet direction calculation to use actual forward direction angles instead of arbitrary assignment - Add node names to simulator combo boxes for better identification - Improve UI layout by drawing connection lines before text for better visibility 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
ChiKyun Kim
@@ -40,6 +40,11 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// </summary>
|
||||
public long CalculationTimeMs { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 탐색된 노드 수
|
||||
/// </summary>
|
||||
public int ExploredNodes { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 예상 소요 시간 (초)
|
||||
/// </summary>
|
||||
@@ -66,6 +71,7 @@ namespace AGVNavigationCore.PathFinding
|
||||
NodeMotorInfos = new List<NodeMotorInfo>();
|
||||
TotalDistance = 0;
|
||||
CalculationTimeMs = 0;
|
||||
ExploredNodes = 0;
|
||||
EstimatedTimeSeconds = 0;
|
||||
RotationCount = 0;
|
||||
ErrorMessage = string.Empty;
|
||||
|
||||
@@ -28,6 +28,11 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// </summary>
|
||||
public float DockingApproachDistance { get; set; } = 100.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 경로 탐색 옵션
|
||||
/// </summary>
|
||||
public PathfindingOptions Options { get; set; } = PathfindingOptions.Default;
|
||||
|
||||
/// <summary>
|
||||
/// 생성자
|
||||
/// </summary>
|
||||
@@ -60,6 +65,46 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
|
||||
/// <returns>AGV 경로 계산 결과</returns>
|
||||
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection = null)
|
||||
{
|
||||
return FindAGVPath(startNodeId, endNodeId, targetDirection, Options);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 계산 (현재 방향 및 옵션 지정 가능)
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="endNodeId">목적지 노드 ID</param>
|
||||
/// <param name="currentDirection">현재 AGV 방향</param>
|
||||
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
|
||||
/// <param name="options">경로 탐색 옵션</param>
|
||||
/// <returns>AGV 경로 계산 결과</returns>
|
||||
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? currentDirection, AgvDirection? targetDirection, PathfindingOptions options)
|
||||
{
|
||||
var result = FindAGVPath(startNodeId, endNodeId, targetDirection, options);
|
||||
|
||||
if (!result.Success || currentDirection == null || result.Commands.Count == 0)
|
||||
return result;
|
||||
|
||||
// 경로의 첫 번째 방향과 현재 방향 비교
|
||||
var firstDirection = result.Commands[0];
|
||||
|
||||
if (RequiresDirectionChange(currentDirection.Value, firstDirection))
|
||||
{
|
||||
return InsertDirectionChangeCommands(result, currentDirection.Value, firstDirection);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 계산 (옵션 지정 가능)
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="endNodeId">목적지 노드 ID</param>
|
||||
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
|
||||
/// <param name="options">경로 탐색 옵션</param>
|
||||
/// <returns>AGV 경로 계산 결과</returns>
|
||||
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options)
|
||||
{
|
||||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
|
||||
@@ -79,11 +124,11 @@ namespace AGVNavigationCore.PathFinding
|
||||
|
||||
if (IsSpecialNode(endNode))
|
||||
{
|
||||
return FindPathToSpecialNode(startNodeId, endNode, targetDirection, stopwatch);
|
||||
return FindPathToSpecialNode(startNodeId, endNode, targetDirection, options, stopwatch);
|
||||
}
|
||||
else
|
||||
{
|
||||
return FindNormalPath(startNodeId, endNodeId, targetDirection, stopwatch);
|
||||
return FindNormalPath(startNodeId, endNodeId, targetDirection, options, stopwatch);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
@@ -150,9 +195,26 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// <summary>
|
||||
/// 일반 노드로의 경로 계산
|
||||
/// </summary>
|
||||
private AGVPathResult FindNormalPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, System.Diagnostics.Stopwatch stopwatch)
|
||||
private AGVPathResult FindNormalPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
|
||||
{
|
||||
var result = _pathfinder.FindPath(startNodeId, endNodeId);
|
||||
PathResult result;
|
||||
|
||||
// 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
|
||||
if (options.AvoidRotationNodes)
|
||||
{
|
||||
result = FindPathAvoidingRotation(startNodeId, endNodeId, options);
|
||||
|
||||
// 회전 회피 경로를 찾지 못하면 일반 경로 계산
|
||||
if (!result.Success)
|
||||
{
|
||||
result = _pathfinder.FindPath(startNodeId, endNodeId);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
result = _pathfinder.FindPath(startNodeId, endNodeId);
|
||||
}
|
||||
|
||||
if (!result.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
|
||||
@@ -166,12 +228,29 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// <summary>
|
||||
/// 특수 노드(도킹/충전)로의 경로 계산
|
||||
/// </summary>
|
||||
private AGVPathResult FindPathToSpecialNode(string startNodeId, MapNode endNode, AgvDirection? targetDirection, System.Diagnostics.Stopwatch stopwatch)
|
||||
private AGVPathResult FindPathToSpecialNode(string startNodeId, MapNode endNode, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
|
||||
{
|
||||
var requiredDirection = GetRequiredDirectionForNode(endNode);
|
||||
var actualTargetDirection = targetDirection ?? requiredDirection;
|
||||
|
||||
var result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
|
||||
PathResult result;
|
||||
|
||||
// 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
|
||||
if (options.AvoidRotationNodes)
|
||||
{
|
||||
result = FindPathAvoidingRotation(startNodeId, endNode.NodeId, options);
|
||||
|
||||
// 회전 회피 경로를 찾지 못하면 일반 경로 계산
|
||||
if (!result.Success)
|
||||
{
|
||||
result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
|
||||
}
|
||||
|
||||
if (!result.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
|
||||
@@ -267,7 +346,7 @@ namespace AGVNavigationCore.PathFinding
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보 생성
|
||||
/// 노드별 모터방향 정보 생성 (방향 전환 로직 개선)
|
||||
/// </summary>
|
||||
/// <param name="path">경로 노드 목록</param>
|
||||
/// <returns>노드별 모터방향 정보 목록</returns>
|
||||
@@ -276,40 +355,236 @@ namespace AGVNavigationCore.PathFinding
|
||||
var nodeMotorInfos = new List<NodeMotorInfo>();
|
||||
if (path.Count < 2) return nodeMotorInfos;
|
||||
|
||||
// 전체 경로에 대한 방향 전환 계획 수립
|
||||
var directionPlan = PlanDirectionChanges(path);
|
||||
|
||||
for (int i = 0; i < path.Count; i++)
|
||||
{
|
||||
var currentNodeId = path[i];
|
||||
string nextNodeId = i < path.Count - 1 ? path[i + 1] : null;
|
||||
|
||||
AgvDirection motorDirection;
|
||||
// 계획된 방향 사용
|
||||
var motorDirection = directionPlan.ContainsKey(currentNodeId)
|
||||
? directionPlan[currentNodeId]
|
||||
: AgvDirection.Forward;
|
||||
|
||||
if (i == path.Count - 1)
|
||||
// 노드 특성 정보 수집
|
||||
bool canRotate = false;
|
||||
bool isDirectionChangePoint = false;
|
||||
bool requiresSpecialAction = false;
|
||||
string specialActionDescription = "";
|
||||
|
||||
if (_nodeMap.ContainsKey(currentNodeId))
|
||||
{
|
||||
// 마지막 노드: 도킹/충전 노드 타입에 따라 결정
|
||||
if (_nodeMap.ContainsKey(currentNodeId))
|
||||
var currentNode = _nodeMap[currentNodeId];
|
||||
canRotate = currentNode.CanRotate;
|
||||
|
||||
// 방향 전환 감지
|
||||
if (i > 0 && directionPlan.ContainsKey(path[i - 1]))
|
||||
{
|
||||
var currentNode = _nodeMap[currentNodeId];
|
||||
motorDirection = GetRequiredDirectionForNode(currentNode);
|
||||
var prevDirection = directionPlan[path[i - 1]];
|
||||
isDirectionChangePoint = prevDirection != motorDirection;
|
||||
}
|
||||
else
|
||||
|
||||
// 특수 동작 필요 여부 감지
|
||||
if (!canRotate && isDirectionChangePoint)
|
||||
{
|
||||
motorDirection = AgvDirection.Forward;
|
||||
requiresSpecialAction = true;
|
||||
specialActionDescription = "갈림길 전진/후진 반복";
|
||||
}
|
||||
else if (canRotate && isDirectionChangePoint)
|
||||
{
|
||||
specialActionDescription = "회전 노드 방향전환";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 중간 노드: 다음 노드와의 관계를 고려한 모터방향 결정
|
||||
motorDirection = CalculateMotorDirection(currentNodeId, nextNodeId);
|
||||
}
|
||||
|
||||
nodeMotorInfos.Add(new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId));
|
||||
var nodeMotorInfo = new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId,
|
||||
canRotate, isDirectionChangePoint, MagnetDirection.Straight, requiresSpecialAction, specialActionDescription);
|
||||
nodeMotorInfos.Add(nodeMotorInfo);
|
||||
}
|
||||
|
||||
return nodeMotorInfos;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산
|
||||
/// 경로 전체에 대한 방향 전환 계획 수립
|
||||
/// </summary>
|
||||
/// <param name="path">경로 노드 목록</param>
|
||||
/// <returns>노드별 모터 방향 계획</returns>
|
||||
private Dictionary<string, AgvDirection> PlanDirectionChanges(List<string> path)
|
||||
{
|
||||
var directionPlan = new Dictionary<string, AgvDirection>();
|
||||
if (path.Count < 2) return directionPlan;
|
||||
|
||||
// 1단계: 목적지 노드의 요구사항 분석
|
||||
var targetNodeId = path[path.Count - 1];
|
||||
var targetDirection = AgvDirection.Forward;
|
||||
|
||||
if (_nodeMap.ContainsKey(targetNodeId))
|
||||
{
|
||||
var targetNode = _nodeMap[targetNodeId];
|
||||
targetDirection = GetRequiredDirectionForNode(targetNode);
|
||||
}
|
||||
|
||||
// 2단계: 역방향으로 방향 전환점 찾기
|
||||
var currentDirection = targetDirection;
|
||||
directionPlan[targetNodeId] = currentDirection;
|
||||
|
||||
// 마지막에서 두 번째부터 역순으로 처리
|
||||
for (int i = path.Count - 2; i >= 0; i--)
|
||||
{
|
||||
var currentNodeId = path[i];
|
||||
var nextNodeId = path[i + 1];
|
||||
|
||||
if (_nodeMap.ContainsKey(currentNodeId))
|
||||
{
|
||||
var currentNode = _nodeMap[currentNodeId];
|
||||
|
||||
// 방법1: 회전 가능 노드에서 방향 전환
|
||||
if (currentNode.CanRotate && NeedDirectionChange(currentNodeId, nextNodeId, currentDirection))
|
||||
{
|
||||
// 회전 가능한 노드에서 방향 전환 수행
|
||||
var optimalDirection = CalculateOptimalDirection(currentNodeId, nextNodeId, path, i);
|
||||
directionPlan[currentNodeId] = optimalDirection;
|
||||
currentDirection = optimalDirection;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 일반 노드: 연속성 유지
|
||||
directionPlan[currentNodeId] = currentDirection;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
directionPlan[currentNodeId] = currentDirection;
|
||||
}
|
||||
}
|
||||
|
||||
// 3단계: 방법2 적용 - 불가능한 방향 전환 감지 및 수정
|
||||
ApplyDirectionChangeCorrection(path, directionPlan);
|
||||
|
||||
return directionPlan;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환이 필요한지 판단
|
||||
/// </summary>
|
||||
private bool NeedDirectionChange(string currentNodeId, string nextNodeId, AgvDirection currentDirection)
|
||||
{
|
||||
if (!_nodeMap.ContainsKey(nextNodeId)) return false;
|
||||
|
||||
var nextNode = _nodeMap[nextNodeId];
|
||||
var requiredDirection = GetRequiredDirectionForNode(nextNode);
|
||||
|
||||
return currentDirection != requiredDirection;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 회전 가능 노드에서 최적 방향 계산
|
||||
/// </summary>
|
||||
private AgvDirection CalculateOptimalDirection(string nodeId, string nextNodeId, List<string> path, int nodeIndex)
|
||||
{
|
||||
if (!_nodeMap.ContainsKey(nextNodeId)) return AgvDirection.Forward;
|
||||
|
||||
var nextNode = _nodeMap[nextNodeId];
|
||||
|
||||
// 목적지까지의 경로를 고려한 최적 방향 결정
|
||||
if (nextNode.Type == NodeType.Charging)
|
||||
{
|
||||
return AgvDirection.Forward; // 충전기는 전진 접근
|
||||
}
|
||||
else if (nextNode.Type == NodeType.Docking)
|
||||
{
|
||||
return AgvDirection.Backward; // 도킹은 후진 접근
|
||||
}
|
||||
else
|
||||
{
|
||||
// 경로 패턴 분석을 통한 방향 결정
|
||||
return AnalyzePathPattern(path, nodeIndex);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 패턴 분석을 통한 방향 결정
|
||||
/// </summary>
|
||||
private AgvDirection AnalyzePathPattern(List<string> path, int startIndex)
|
||||
{
|
||||
// 남은 경로에서 도킹/충전 스테이션이 있는지 확인
|
||||
for (int i = startIndex + 1; i < path.Count; i++)
|
||||
{
|
||||
if (_nodeMap.ContainsKey(path[i]))
|
||||
{
|
||||
var node = _nodeMap[path[i]];
|
||||
if (node.Type == NodeType.Docking)
|
||||
{
|
||||
return AgvDirection.Backward; // 도킹 준비를 위해 후진 방향
|
||||
}
|
||||
else if (node.Type == NodeType.Charging)
|
||||
{
|
||||
return AgvDirection.Forward; // 충전 준비를 위해 전진 방향
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return AgvDirection.Forward; // 기본값
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방법2: 불가능한 방향 전환 감지 및 보정 (갈림길 전진/후진 반복)
|
||||
/// </summary>
|
||||
private void ApplyDirectionChangeCorrection(List<string> path, Dictionary<string, AgvDirection> directionPlan)
|
||||
{
|
||||
for (int i = 0; i < path.Count - 1; i++)
|
||||
{
|
||||
var currentNodeId = path[i];
|
||||
var nextNodeId = path[i + 1];
|
||||
|
||||
if (directionPlan.ContainsKey(currentNodeId) && directionPlan.ContainsKey(nextNodeId))
|
||||
{
|
||||
var currentDir = directionPlan[currentNodeId];
|
||||
var nextDir = directionPlan[nextNodeId];
|
||||
|
||||
// 급격한 방향 전환 감지 (전진→후진, 후진→전진)
|
||||
if (IsImpossibleDirectionChange(currentDir, nextDir))
|
||||
{
|
||||
var currentNode = _nodeMap.ContainsKey(currentNodeId) ? _nodeMap[currentNodeId] : null;
|
||||
|
||||
if (currentNode != null && !currentNode.CanRotate)
|
||||
{
|
||||
// 회전 불가능한 노드에서 방향 전환 시도 → 특수 동작 추가
|
||||
AddTurnAroundSequence(currentNodeId, directionPlan);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 불가능한 방향 전환인지 확인
|
||||
/// </summary>
|
||||
private bool IsImpossibleDirectionChange(AgvDirection current, AgvDirection next)
|
||||
{
|
||||
return (current == AgvDirection.Forward && next == AgvDirection.Backward) ||
|
||||
(current == AgvDirection.Backward && next == AgvDirection.Forward);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 회전 불가능한 노드에서 방향 전환을 위한 특수 동작 시퀀스 추가
|
||||
/// </summary>
|
||||
private void AddTurnAroundSequence(string nodeId, Dictionary<string, AgvDirection> directionPlan)
|
||||
{
|
||||
// 갈림길에서 전진/후진 반복 작업으로 방향 변경
|
||||
// 실제 구현시에는 NodeMotorInfo에 특수 플래그나 추가 동작 정보 포함 필요
|
||||
// 현재는 안전한 방향으로 보정
|
||||
if (directionPlan.ContainsKey(nodeId))
|
||||
{
|
||||
// 일단 연속성을 유지하도록 보정 (실제로는 특수 회전 동작 필요)
|
||||
directionPlan[nodeId] = AgvDirection.Forward;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산 (레거시 - 새로운 PlanDirectionChanges 사용)
|
||||
/// </summary>
|
||||
/// <param name="currentNodeId">현재 노드 ID</param>
|
||||
/// <param name="nextNodeId">다음 노드 ID</param>
|
||||
@@ -321,18 +596,8 @@ namespace AGVNavigationCore.PathFinding
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
|
||||
var currentNode = _nodeMap[currentNodeId];
|
||||
var nextNode = _nodeMap[nextNodeId];
|
||||
|
||||
// 현재 노드와 다음 노드의 위치를 기반으로 이동 방향 계산
|
||||
var dx = nextNode.Position.X - currentNode.Position.X;
|
||||
var dy = nextNode.Position.Y - currentNode.Position.Y;
|
||||
var moveAngle = Math.Atan2(dy, dx);
|
||||
|
||||
// AGV의 구조: 리프트 ↔ AGV 몸체 ↔ 모니터
|
||||
// 전진: 모니터 방향으로 이동 (리프트에서 멀어짐)
|
||||
// 후진: 리프트 방향으로 이동 (리프트에 가까워짐)
|
||||
|
||||
// 다음 노드가 특수 노드인지 확인
|
||||
if (nextNode.Type == NodeType.Charging)
|
||||
{
|
||||
@@ -346,12 +611,56 @@ namespace AGVNavigationCore.PathFinding
|
||||
}
|
||||
else
|
||||
{
|
||||
// 일반 이동: 기본적으로 전진
|
||||
// 향후 경로 패턴 분석을 통해 더 정확한 방향 결정 가능
|
||||
// 일반 이동: 기본적으로 전진 (실제로는 PlanDirectionChanges에서 결정됨)
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 회전 노드를 회피하는 경로 탐색
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="endNodeId">목적지 노드 ID</param>
|
||||
/// <param name="options">경로 탐색 옵션</param>
|
||||
/// <returns>회전 회피 경로 결과</returns>
|
||||
private PathResult FindPathAvoidingRotation(string startNodeId, string endNodeId, PathfindingOptions options)
|
||||
{
|
||||
try
|
||||
{
|
||||
// 회전 가능한 노드들을 필터링하여 임시로 비활성화
|
||||
var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate).Select(n => n.NodeId).ToList();
|
||||
var originalConnections = new Dictionary<string, List<string>>();
|
||||
|
||||
// 시작과 끝 노드가 회전 노드인 경우는 제외
|
||||
var nodesToDisable = rotationNodes.Where(nodeId => nodeId != startNodeId && nodeId != endNodeId).ToList();
|
||||
|
||||
foreach (var nodeId in nodesToDisable)
|
||||
{
|
||||
// 임시로 연결 해제 (실제로는 pathfinder에서 해당 노드를 높은 비용으로 설정해야 함)
|
||||
originalConnections[nodeId] = _nodeMap[nodeId].ConnectedNodes.ToList();
|
||||
}
|
||||
|
||||
// 회전 노드 회피 경로 계산 시도
|
||||
var result = _pathfinder.FindPath(startNodeId, endNodeId);
|
||||
|
||||
// 결과에서 회전 노드가 포함되어 있으면 실패로 간주
|
||||
if (result.Success && result.Path != null)
|
||||
{
|
||||
var pathContainsRotation = result.Path.Any(nodeId => nodesToDisable.Contains(nodeId));
|
||||
if (pathContainsRotation)
|
||||
{
|
||||
return PathResult.CreateFailure("회전 노드를 회피하는 경로를 찾을 수 없습니다.", 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return PathResult.CreateFailure($"회전 회피 경로 계산 중 오류: {ex.Message}", 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 유효성 검증
|
||||
/// </summary>
|
||||
@@ -371,5 +680,149 @@ namespace AGVNavigationCore.PathFinding
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 방향에서 목표 방향으로 변경하기 위해 방향 전환이 필요한지 판단
|
||||
/// </summary>
|
||||
/// <param name="currentDirection">현재 방향</param>
|
||||
/// <param name="targetDirection">목표 방향</param>
|
||||
/// <returns>방향 전환 필요 여부</returns>
|
||||
private bool RequiresDirectionChange(AgvDirection currentDirection, AgvDirection targetDirection)
|
||||
{
|
||||
// 같은 방향이면 변경 불필요
|
||||
if (currentDirection == targetDirection)
|
||||
return false;
|
||||
|
||||
// 전진 <-> 후진 전환은 방향 전환 필요
|
||||
return (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward) ||
|
||||
(currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환을 위한 명령어를 기존 경로에 삽입
|
||||
/// </summary>
|
||||
/// <param name="originalResult">원래 경로 결과</param>
|
||||
/// <param name="currentDirection">현재 AGV 방향</param>
|
||||
/// <param name="targetDirection">목표 방향</param>
|
||||
/// <returns>방향 전환 명령이 포함된 경로 결과</returns>
|
||||
private AGVPathResult InsertDirectionChangeCommands(AGVPathResult originalResult, AgvDirection currentDirection, AgvDirection targetDirection)
|
||||
{
|
||||
if (originalResult.Path.Count < 1)
|
||||
return originalResult;
|
||||
|
||||
// 시작 노드를 찾아서 회전 가능한지 확인
|
||||
var startNodeId = originalResult.Path[0];
|
||||
if (!_nodeMap.ContainsKey(startNodeId))
|
||||
return originalResult;
|
||||
|
||||
var startNode = _nodeMap[startNodeId];
|
||||
|
||||
// 시작 노드에서 회전이 불가능하면 회전 가능한 가까운 노드 찾기
|
||||
if (!startNode.CanRotate)
|
||||
{
|
||||
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);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 시작 노드에서 바로 방향 전환
|
||||
var commandsWithRotation = GenerateAGVCommandsWithDirectionChange(originalResult.Path, currentDirection, targetDirection, startNodeId);
|
||||
return AGVPathResult.CreateSuccess(originalResult.Path, commandsWithRotation, originalResult.NodeMotorInfos, originalResult.TotalDistance, originalResult.CalculationTimeMs);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 가장 가까운 회전 가능 노드 찾기
|
||||
/// </summary>
|
||||
/// <param name="fromNodeId">시작 노드 ID</param>
|
||||
/// <returns>가장 가까운 회전 가능 노드</returns>
|
||||
private MapNode FindNearestRotationNode(string fromNodeId)
|
||||
{
|
||||
var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate && n.IsActive).ToList();
|
||||
|
||||
MapNode nearestNode = null;
|
||||
var shortestDistance = float.MaxValue;
|
||||
|
||||
foreach (var rotationNode in rotationNodes)
|
||||
{
|
||||
var pathResult = _pathfinder.FindPath(fromNodeId, rotationNode.NodeId);
|
||||
if (pathResult.Success && pathResult.TotalDistance < shortestDistance)
|
||||
{
|
||||
shortestDistance = pathResult.TotalDistance;
|
||||
nearestNode = rotationNode;
|
||||
}
|
||||
}
|
||||
|
||||
return nearestNode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환을 포함한 AGV 명령어 생성
|
||||
/// </summary>
|
||||
/// <param name="path">경로</param>
|
||||
/// <param name="currentDirection">현재 방향</param>
|
||||
/// <param name="targetDirection">목표 방향</param>
|
||||
/// <param name="rotationNodeId">방향 전환 노드 ID</param>
|
||||
/// <returns>AGV 명령어 목록</returns>
|
||||
private List<AgvDirection> GenerateAGVCommandsWithDirectionChange(List<string> path, AgvDirection currentDirection, AgvDirection targetDirection, string rotationNodeId)
|
||||
{
|
||||
var commands = new List<AgvDirection>();
|
||||
|
||||
int rotationIndex = path.IndexOf(rotationNodeId);
|
||||
|
||||
// 회전 노드까지는 현재 방향으로 이동
|
||||
for (int i = 0; i < rotationIndex; i++)
|
||||
{
|
||||
commands.Add(currentDirection);
|
||||
}
|
||||
|
||||
// 회전 명령 추가 (전진->후진 또는 후진->전진)
|
||||
if (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward)
|
||||
{
|
||||
commands.Add(AgvDirection.Left);
|
||||
commands.Add(AgvDirection.Left); // 180도 회전
|
||||
}
|
||||
else if (currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward)
|
||||
{
|
||||
commands.Add(AgvDirection.Right);
|
||||
commands.Add(AgvDirection.Right); // 180도 회전
|
||||
}
|
||||
|
||||
// 회전 노드부터 목적지까지는 목표 방향으로 이동
|
||||
for (int i = rotationIndex; i < path.Count - 1; i++)
|
||||
{
|
||||
commands.Add(targetDirection);
|
||||
}
|
||||
|
||||
return commands;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -43,40 +43,38 @@ namespace AGVNavigationCore.PathFinding
|
||||
_mapNodes = mapNodes ?? new List<MapNode>();
|
||||
_nodeMap.Clear();
|
||||
|
||||
// 1단계: 모든 네비게이션 노드를 PathNode로 변환
|
||||
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
|
||||
foreach (var mapNode in _mapNodes)
|
||||
{
|
||||
if (mapNode.IsNavigationNode())
|
||||
{
|
||||
var pathNode = new PathNode(mapNode.NodeId, mapNode.Position);
|
||||
pathNode.ConnectedNodes = new List<string>(mapNode.ConnectedNodes);
|
||||
_nodeMap[mapNode.NodeId] = pathNode;
|
||||
}
|
||||
}
|
||||
|
||||
// 2단계: 양방향 연결 자동 생성 (A→B 연결이 있으면 B→A도 추가)
|
||||
EnsureBidirectionalConnections();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 단방향 연결을 양방향으로 자동 변환
|
||||
/// A→B 연결이 있으면 B→A 연결도 자동 생성
|
||||
/// </summary>
|
||||
private void EnsureBidirectionalConnections()
|
||||
{
|
||||
foreach (var nodeId in _nodeMap.Keys.ToList())
|
||||
// 단일 연결을 양방향으로 확장
|
||||
foreach (var mapNode in _mapNodes)
|
||||
{
|
||||
var node = _nodeMap[nodeId];
|
||||
foreach (var connectedNodeId in node.ConnectedNodes.ToList())
|
||||
if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.NodeId))
|
||||
{
|
||||
// 연결된 노드가 존재하고 네비게이션 가능한 노드인지 확인
|
||||
if (_nodeMap.ContainsKey(connectedNodeId))
|
||||
var pathNode = _nodeMap[mapNode.NodeId];
|
||||
|
||||
foreach (var connectedNodeId in mapNode.ConnectedNodes)
|
||||
{
|
||||
var connectedNode = _nodeMap[connectedNodeId];
|
||||
// 역방향 연결이 없으면 추가
|
||||
if (!connectedNode.ConnectedNodes.Contains(nodeId))
|
||||
if (_nodeMap.ContainsKey(connectedNodeId))
|
||||
{
|
||||
connectedNode.ConnectedNodes.Add(nodeId);
|
||||
// 양방향 연결 생성 (단일 연결이 양방향을 의미)
|
||||
if (!pathNode.ConnectedNodes.Contains(connectedNodeId))
|
||||
{
|
||||
pathNode.ConnectedNodes.Add(connectedNodeId);
|
||||
}
|
||||
|
||||
var connectedPathNode = _nodeMap[connectedNodeId];
|
||||
if (!connectedPathNode.ConnectedNodes.Contains(mapNode.NodeId))
|
||||
{
|
||||
connectedPathNode.ConnectedNodes.Add(mapNode.NodeId);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
391
Cs_HMI/AGVNavigationCore/PathFinding/AdvancedAGVPathfinder.cs
Normal file
391
Cs_HMI/AGVNavigationCore/PathFinding/AdvancedAGVPathfinder.cs
Normal file
@@ -0,0 +1,391 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 고급 AGV 경로 계획기
|
||||
/// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
|
||||
/// </summary>
|
||||
public class AdvancedAGVPathfinder
|
||||
{
|
||||
/// <summary>
|
||||
/// 고급 AGV 경로 계산 결과
|
||||
/// </summary>
|
||||
public class AdvancedPathResult
|
||||
{
|
||||
public bool Success { get; set; }
|
||||
public List<NodeMotorInfo> DetailedPath { get; set; }
|
||||
public float TotalDistance { get; set; }
|
||||
public long CalculationTimeMs { get; set; }
|
||||
public int ExploredNodeCount { get; set; }
|
||||
public string ErrorMessage { get; set; }
|
||||
public string PlanDescription { get; set; }
|
||||
public bool RequiredDirectionChange { get; set; }
|
||||
public string DirectionChangeNode { get; set; }
|
||||
|
||||
public AdvancedPathResult()
|
||||
{
|
||||
DetailedPath = new List<NodeMotorInfo>();
|
||||
ErrorMessage = string.Empty;
|
||||
PlanDescription = string.Empty;
|
||||
}
|
||||
|
||||
public static AdvancedPathResult CreateSuccess(List<NodeMotorInfo> path, float distance, long time, int explored, string description, bool directionChange = false, string changeNode = null)
|
||||
{
|
||||
return new AdvancedPathResult
|
||||
{
|
||||
Success = true,
|
||||
DetailedPath = path,
|
||||
TotalDistance = distance,
|
||||
CalculationTimeMs = time,
|
||||
ExploredNodeCount = explored,
|
||||
PlanDescription = description,
|
||||
RequiredDirectionChange = directionChange,
|
||||
DirectionChangeNode = changeNode
|
||||
};
|
||||
}
|
||||
|
||||
public static AdvancedPathResult CreateFailure(string error, long time, int explored)
|
||||
{
|
||||
return new AdvancedPathResult
|
||||
{
|
||||
Success = false,
|
||||
ErrorMessage = error,
|
||||
CalculationTimeMs = time,
|
||||
ExploredNodeCount = explored
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 단순 경로 목록 반환 (호환성용)
|
||||
/// </summary>
|
||||
public List<string> GetSimplePath()
|
||||
{
|
||||
return DetailedPath.Select(n => n.NodeId).ToList();
|
||||
}
|
||||
}
|
||||
|
||||
private readonly List<MapNode> _mapNodes;
|
||||
private readonly AStarPathfinder _basicPathfinder;
|
||||
private readonly JunctionAnalyzer _junctionAnalyzer;
|
||||
private readonly DirectionChangePlanner _directionChangePlanner;
|
||||
|
||||
public AdvancedAGVPathfinder(List<MapNode> mapNodes)
|
||||
{
|
||||
_mapNodes = mapNodes ?? new List<MapNode>();
|
||||
_basicPathfinder = new AStarPathfinder();
|
||||
_basicPathfinder.SetMapNodes(_mapNodes);
|
||||
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
|
||||
_directionChangePlanner = new DirectionChangePlanner(_mapNodes);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 고급 AGV 경로 계산
|
||||
/// </summary>
|
||||
public AdvancedPathResult FindAdvancedPath(string startNodeId, string targetNodeId, AgvDirection currentDirection = AgvDirection.Forward)
|
||||
{
|
||||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
|
||||
try
|
||||
{
|
||||
// 1. 목적지 도킹 방향 요구사항 확인
|
||||
var requiredDirection = _directionChangePlanner.GetRequiredDockingDirection(targetNodeId);
|
||||
|
||||
// 2. 방향 전환이 필요한지 확인
|
||||
bool needDirectionChange = (currentDirection != requiredDirection);
|
||||
|
||||
AdvancedPathResult result;
|
||||
if (needDirectionChange)
|
||||
{
|
||||
// 방향 전환이 필요한 경우
|
||||
result = PlanPathWithDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 직접 경로 계산
|
||||
result = PlanDirectPath(startNodeId, targetNodeId, currentDirection);
|
||||
}
|
||||
|
||||
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
|
||||
return result;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return AdvancedPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 직접 경로 계획
|
||||
/// </summary>
|
||||
private AdvancedPathResult PlanDirectPath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
|
||||
{
|
||||
var basicResult = _basicPathfinder.FindPath(startNodeId, targetNodeId);
|
||||
|
||||
if (!basicResult.Success)
|
||||
{
|
||||
return AdvancedPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
|
||||
}
|
||||
|
||||
// 기본 경로를 상세 경로로 변환
|
||||
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
|
||||
|
||||
return AdvancedPathResult.CreateSuccess(
|
||||
detailedPath,
|
||||
basicResult.TotalDistance,
|
||||
basicResult.CalculationTimeMs,
|
||||
basicResult.ExploredNodeCount,
|
||||
"직접 경로 - 방향 전환 불필요"
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환을 포함한 경로 계획
|
||||
/// </summary>
|
||||
private AdvancedPathResult PlanPathWithDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);
|
||||
|
||||
if (!directionChangePlan.Success)
|
||||
{
|
||||
return AdvancedPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
|
||||
}
|
||||
|
||||
// 방향 전환 경로를 상세 경로로 변환
|
||||
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection);
|
||||
|
||||
// 거리 계산
|
||||
float totalDistance = CalculatePathDistance(detailedPath);
|
||||
|
||||
return AdvancedPathResult.CreateSuccess(
|
||||
detailedPath,
|
||||
totalDistance,
|
||||
0,
|
||||
0,
|
||||
directionChangePlan.PlanDescription,
|
||||
true,
|
||||
directionChangePlan.DirectionChangeNode
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 기본 경로를 상세 경로로 변환
|
||||
/// </summary>
|
||||
private List<NodeMotorInfo> ConvertToDetailedPath(List<string> simplePath, AgvDirection initialDirection)
|
||||
{
|
||||
var detailedPath = new List<NodeMotorInfo>();
|
||||
var currentDirection = initialDirection;
|
||||
|
||||
for (int i = 0; i < simplePath.Count; i++)
|
||||
{
|
||||
string currentNodeId = simplePath[i];
|
||||
string nextNodeId = (i + 1 < simplePath.Count) ? simplePath[i + 1] : null;
|
||||
|
||||
// 마그넷 방향 계산
|
||||
MagnetDirection magnetDirection = MagnetDirection.Straight;
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = simplePath[i - 1];
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
|
||||
}
|
||||
|
||||
// 노드 정보 생성
|
||||
var nodeMotorInfo = new NodeMotorInfo(
|
||||
currentNodeId,
|
||||
currentDirection,
|
||||
nextNodeId,
|
||||
magnetDirection
|
||||
);
|
||||
|
||||
// 회전 가능 노드 설정
|
||||
var mapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
|
||||
if (mapNode != null)
|
||||
{
|
||||
nodeMotorInfo.CanRotate = mapNode.CanRotate;
|
||||
}
|
||||
|
||||
detailedPath.Add(nodeMotorInfo);
|
||||
}
|
||||
|
||||
return detailedPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 경로를 상세 경로로 변환
|
||||
/// </summary>
|
||||
private List<NodeMotorInfo> ConvertDirectionChangePath(DirectionChangePlanner.DirectionChangePlan plan, AgvDirection startDirection, AgvDirection endDirection)
|
||||
{
|
||||
var detailedPath = new List<NodeMotorInfo>();
|
||||
var currentDirection = startDirection;
|
||||
|
||||
for (int i = 0; i < plan.DirectionChangePath.Count; i++)
|
||||
{
|
||||
string currentNodeId = plan.DirectionChangePath[i];
|
||||
string nextNodeId = (i + 1 < plan.DirectionChangePath.Count) ? plan.DirectionChangePath[i + 1] : null;
|
||||
|
||||
// 방향 전환 노드에서 방향 변경
|
||||
if (currentNodeId == plan.DirectionChangeNode && currentDirection != endDirection)
|
||||
{
|
||||
currentDirection = endDirection;
|
||||
}
|
||||
|
||||
// 마그넷 방향 계산
|
||||
MagnetDirection magnetDirection = MagnetDirection.Straight;
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = plan.DirectionChangePath[i - 1];
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
|
||||
}
|
||||
|
||||
// 특수 동작 확인
|
||||
bool requiresSpecialAction = false;
|
||||
string specialActionDescription = "";
|
||||
|
||||
if (currentNodeId == plan.DirectionChangeNode)
|
||||
{
|
||||
requiresSpecialAction = true;
|
||||
specialActionDescription = $"방향전환: {startDirection} → {endDirection}";
|
||||
}
|
||||
|
||||
// 노드 정보 생성
|
||||
var nodeMotorInfo = new NodeMotorInfo(
|
||||
currentNodeId,
|
||||
currentDirection,
|
||||
nextNodeId,
|
||||
true, // 방향 전환 경로의 경우 회전 가능으로 설정
|
||||
currentNodeId == plan.DirectionChangeNode,
|
||||
magnetDirection,
|
||||
requiresSpecialAction,
|
||||
specialActionDescription
|
||||
);
|
||||
|
||||
detailedPath.Add(nodeMotorInfo);
|
||||
}
|
||||
|
||||
return detailedPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 총 거리 계산
|
||||
/// </summary>
|
||||
private float CalculatePathDistance(List<NodeMotorInfo> detailedPath)
|
||||
{
|
||||
float totalDistance = 0;
|
||||
|
||||
for (int i = 0; i < detailedPath.Count - 1; i++)
|
||||
{
|
||||
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i].NodeId);
|
||||
var nextNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i + 1].NodeId);
|
||||
|
||||
if (currentNode != null && nextNode != null)
|
||||
{
|
||||
float dx = nextNode.Position.X - currentNode.Position.X;
|
||||
float dy = nextNode.Position.Y - currentNode.Position.Y;
|
||||
totalDistance += (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
|
||||
return totalDistance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 유효성 검증
|
||||
/// </summary>
|
||||
public bool ValidatePath(List<NodeMotorInfo> detailedPath)
|
||||
{
|
||||
if (detailedPath == null || detailedPath.Count == 0)
|
||||
return false;
|
||||
|
||||
// 1. 모든 노드가 존재하는지 확인
|
||||
foreach (var nodeInfo in detailedPath)
|
||||
{
|
||||
if (!_mapNodes.Any(n => n.NodeId == nodeInfo.NodeId))
|
||||
return false;
|
||||
}
|
||||
|
||||
// 2. 연결성 확인
|
||||
for (int i = 0; i < detailedPath.Count - 1; i++)
|
||||
{
|
||||
string currentId = detailedPath[i].NodeId;
|
||||
string nextId = detailedPath[i + 1].NodeId;
|
||||
|
||||
if (!_basicPathfinder.AreNodesConnected(currentId, nextId))
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3. 물리적 제약사항 확인
|
||||
return ValidatePhysicalConstraints(detailedPath);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 물리적 제약사항 검증
|
||||
/// </summary>
|
||||
private bool ValidatePhysicalConstraints(List<NodeMotorInfo> detailedPath)
|
||||
{
|
||||
for (int i = 1; i < detailedPath.Count; i++)
|
||||
{
|
||||
var prevNode = detailedPath[i - 1];
|
||||
var currentNode = detailedPath[i];
|
||||
|
||||
// 급작스러운 방향 전환 검증
|
||||
if (prevNode.MotorDirection != currentNode.MotorDirection)
|
||||
{
|
||||
// 방향 전환은 반드시 회전 가능 노드에서만
|
||||
if (!currentNode.CanRotate && !currentNode.IsDirectionChangePoint)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 최적화
|
||||
/// </summary>
|
||||
public AdvancedPathResult OptimizePath(AdvancedPathResult originalResult)
|
||||
{
|
||||
if (!originalResult.Success)
|
||||
return originalResult;
|
||||
|
||||
// TODO: 경로 최적화 로직 구현
|
||||
// - 불필요한 중간 노드 제거
|
||||
// - 마그넷 방향 최적화
|
||||
// - 방향 전환 최소화
|
||||
|
||||
return originalResult;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 디버깅용 경로 정보
|
||||
/// </summary>
|
||||
public string GetPathSummary(AdvancedPathResult result)
|
||||
{
|
||||
if (!result.Success)
|
||||
return $"경로 계산 실패: {result.ErrorMessage}";
|
||||
|
||||
var summary = new List<string>
|
||||
{
|
||||
$"=== AGV 고급 경로 계획 결과 ===",
|
||||
$"총 노드 수: {result.DetailedPath.Count}",
|
||||
$"총 거리: {result.TotalDistance:F1}px",
|
||||
$"계산 시간: {result.CalculationTimeMs}ms",
|
||||
$"방향 전환: {(result.RequiredDirectionChange ? $"필요 (노드: {result.DirectionChangeNode})" : "불필요")}",
|
||||
$"설명: {result.PlanDescription}",
|
||||
"",
|
||||
"=== 상세 경로 ===",
|
||||
};
|
||||
|
||||
foreach (var nodeInfo in result.DetailedPath)
|
||||
{
|
||||
summary.Add(nodeInfo.ToString());
|
||||
}
|
||||
|
||||
return string.Join("\n", summary);
|
||||
}
|
||||
}
|
||||
}
|
||||
518
Cs_HMI/AGVNavigationCore/PathFinding/DirectionChangePlanner.cs
Normal file
518
Cs_HMI/AGVNavigationCore/PathFinding/DirectionChangePlanner.cs
Normal file
@@ -0,0 +1,518 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 방향 전환 경로 계획 시스템
|
||||
/// 물리적 제약사항을 고려한 방향 전환 경로 생성
|
||||
/// </summary>
|
||||
public class DirectionChangePlanner
|
||||
{
|
||||
/// <summary>
|
||||
/// 방향 전환 계획 결과
|
||||
/// </summary>
|
||||
public class DirectionChangePlan
|
||||
{
|
||||
public bool Success { get; set; }
|
||||
public List<string> DirectionChangePath { get; set; }
|
||||
public string DirectionChangeNode { get; set; }
|
||||
public string ErrorMessage { get; set; }
|
||||
public string PlanDescription { get; set; }
|
||||
|
||||
public DirectionChangePlan()
|
||||
{
|
||||
DirectionChangePath = new List<string>();
|
||||
ErrorMessage = string.Empty;
|
||||
PlanDescription = string.Empty;
|
||||
}
|
||||
|
||||
public static DirectionChangePlan CreateSuccess(List<string> path, string changeNode, string description)
|
||||
{
|
||||
return new DirectionChangePlan
|
||||
{
|
||||
Success = true,
|
||||
DirectionChangePath = path,
|
||||
DirectionChangeNode = changeNode,
|
||||
PlanDescription = description
|
||||
};
|
||||
}
|
||||
|
||||
public static DirectionChangePlan CreateFailure(string error)
|
||||
{
|
||||
return new DirectionChangePlan
|
||||
{
|
||||
Success = false,
|
||||
ErrorMessage = error
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
private readonly List<MapNode> _mapNodes;
|
||||
private readonly JunctionAnalyzer _junctionAnalyzer;
|
||||
private readonly AStarPathfinder _pathfinder;
|
||||
|
||||
public DirectionChangePlanner(List<MapNode> mapNodes)
|
||||
{
|
||||
_mapNodes = mapNodes ?? new List<MapNode>();
|
||||
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
|
||||
_pathfinder = new AStarPathfinder();
|
||||
_pathfinder.SetMapNodes(_mapNodes);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환이 필요한 경로 계획
|
||||
/// </summary>
|
||||
public DirectionChangePlan PlanDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
// 방향이 같으면 직접 경로 계산
|
||||
if (currentDirection == requiredDirection)
|
||||
{
|
||||
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
|
||||
if (directPath.Success)
|
||||
{
|
||||
return DirectionChangePlan.CreateSuccess(
|
||||
directPath.Path,
|
||||
null,
|
||||
"방향 전환 불필요 - 직접 경로 사용"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
// 방향 전환이 필요한 경우
|
||||
return PlanDirectionChangeRoute(startNodeId, targetNodeId, currentDirection, requiredDirection);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 경로 계획
|
||||
/// </summary>
|
||||
private DirectionChangePlan PlanDirectionChangeRoute(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
// 1. 방향 전환 가능한 갈림길 찾기
|
||||
var changeJunctions = FindSuitableChangeJunctions(startNodeId, targetNodeId, currentDirection, requiredDirection);
|
||||
|
||||
if (changeJunctions.Count == 0)
|
||||
{
|
||||
return DirectionChangePlan.CreateFailure("방향 전환 가능한 갈림길을 찾을 수 없습니다.");
|
||||
}
|
||||
|
||||
// 2. 각 갈림길에 대해 경로 계획 시도
|
||||
foreach (var junction in changeJunctions)
|
||||
{
|
||||
var plan = TryDirectionChangeAtJunction(startNodeId, targetNodeId, junction, currentDirection, requiredDirection);
|
||||
if (plan.Success)
|
||||
{
|
||||
return plan;
|
||||
}
|
||||
}
|
||||
|
||||
return DirectionChangePlan.CreateFailure("모든 갈림길에서 방향 전환 경로 계획이 실패했습니다.");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환에 적합한 갈림길 검색
|
||||
/// </summary>
|
||||
private List<string> FindSuitableChangeJunctions(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
var suitableJunctions = new List<string>();
|
||||
|
||||
// 시작점과 목표점 사이의 경로에 있는 갈림길들 우선 검색
|
||||
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
|
||||
if (directPath.Success)
|
||||
{
|
||||
foreach (var nodeId in directPath.Path)
|
||||
{
|
||||
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
|
||||
if (junctionInfo != null && junctionInfo.IsJunction)
|
||||
{
|
||||
suitableJunctions.Add(nodeId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 추가로 시작점 주변의 갈림길들도 검색
|
||||
var nearbyJunctions = FindNearbyJunctions(startNodeId, 3); // 3단계 내의 갈림길
|
||||
foreach (var junction in nearbyJunctions)
|
||||
{
|
||||
if (!suitableJunctions.Contains(junction))
|
||||
{
|
||||
suitableJunctions.Add(junction);
|
||||
}
|
||||
}
|
||||
|
||||
// 거리순으로 정렬 (시작점에서 가까운 순)
|
||||
return SortJunctionsByDistance(startNodeId, suitableJunctions);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 노드 주변의 갈림길 검색
|
||||
/// </summary>
|
||||
private List<string> FindNearbyJunctions(string nodeId, int maxSteps)
|
||||
{
|
||||
var junctions = new List<string>();
|
||||
var visited = new HashSet<string>();
|
||||
var queue = new Queue<(string NodeId, int Steps)>();
|
||||
|
||||
queue.Enqueue((nodeId, 0));
|
||||
visited.Add(nodeId);
|
||||
|
||||
while (queue.Count > 0)
|
||||
{
|
||||
var (currentNodeId, steps) = queue.Dequeue();
|
||||
|
||||
if (steps > maxSteps) continue;
|
||||
|
||||
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(currentNodeId);
|
||||
if (junctionInfo != null && junctionInfo.IsJunction && currentNodeId != nodeId)
|
||||
{
|
||||
junctions.Add(currentNodeId);
|
||||
}
|
||||
|
||||
// 연결된 노드들을 큐에 추가
|
||||
var connectedNodes = GetAllConnectedNodes(currentNodeId);
|
||||
foreach (var connectedId in connectedNodes)
|
||||
{
|
||||
if (!visited.Contains(connectedId))
|
||||
{
|
||||
visited.Add(connectedId);
|
||||
queue.Enqueue((connectedId, steps + 1));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return junctions;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 양방향 연결을 고려한 연결 노드 검색
|
||||
/// </summary>
|
||||
private List<string> GetAllConnectedNodes(string nodeId)
|
||||
{
|
||||
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
|
||||
if (node == null) return new List<string>();
|
||||
|
||||
var connected = new HashSet<string>();
|
||||
|
||||
// 직접 연결
|
||||
foreach (var connectedId in node.ConnectedNodes)
|
||||
{
|
||||
connected.Add(connectedId);
|
||||
}
|
||||
|
||||
// 역방향 연결
|
||||
foreach (var otherNode in _mapNodes)
|
||||
{
|
||||
if (otherNode.NodeId != nodeId && otherNode.ConnectedNodes.Contains(nodeId))
|
||||
{
|
||||
connected.Add(otherNode.NodeId);
|
||||
}
|
||||
}
|
||||
|
||||
return connected.ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길을 거리순으로 정렬
|
||||
/// </summary>
|
||||
private List<string> SortJunctionsByDistance(string startNodeId, List<string> junctions)
|
||||
{
|
||||
var distances = new List<(string NodeId, double Distance)>();
|
||||
|
||||
foreach (var junction in junctions)
|
||||
{
|
||||
var path = _pathfinder.FindPath(startNodeId, junction);
|
||||
double distance = path.Success ? path.TotalDistance : double.MaxValue;
|
||||
distances.Add((junction, distance));
|
||||
}
|
||||
|
||||
return distances.OrderBy(d => d.Distance).Select(d => d.NodeId).ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 갈림길에서 방향 전환 시도
|
||||
/// </summary>
|
||||
private DirectionChangePlan TryDirectionChangeAtJunction(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
try
|
||||
{
|
||||
// 방향 전환 경로 생성
|
||||
var changePath = GenerateDirectionChangePath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
|
||||
|
||||
if (changePath.Count > 0)
|
||||
{
|
||||
// 실제 방향 전환 노드 찾기 (우회 노드)
|
||||
string actualDirectionChangeNode = FindActualDirectionChangeNode(changePath, junctionNodeId);
|
||||
|
||||
string description = $"갈림길 {junctionNodeId}를 통해 {actualDirectionChangeNode}에서 방향 전환: {currentDirection} → {requiredDirection}";
|
||||
return DirectionChangePlan.CreateSuccess(changePath, actualDirectionChangeNode, description);
|
||||
}
|
||||
|
||||
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 방향 전환 경로 생성 실패");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 오류: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 경로 생성
|
||||
/// </summary>
|
||||
private List<string> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
var fullPath = new List<string>();
|
||||
|
||||
// 1. 시작점에서 갈림길까지의 경로
|
||||
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
|
||||
if (!toJunctionPath.Success)
|
||||
return fullPath;
|
||||
|
||||
fullPath.AddRange(toJunctionPath.Path);
|
||||
|
||||
// 2. 갈림길에서 방향 전환 처리
|
||||
if (currentDirection != requiredDirection)
|
||||
{
|
||||
// AGV가 어느 노드에서 갈림길로 왔는지 파악
|
||||
string fromNodeId = toJunctionPath.Path.Count >= 2 ?
|
||||
toJunctionPath.Path[toJunctionPath.Path.Count - 2] : startNodeId;
|
||||
|
||||
var changeSequence = GenerateDirectionChangeSequence(junctionNodeId, fromNodeId, currentDirection, requiredDirection);
|
||||
if (changeSequence.Count > 1) // 첫 번째는 갈림길 자체이므로 제외
|
||||
{
|
||||
fullPath.AddRange(changeSequence.Skip(1));
|
||||
}
|
||||
}
|
||||
|
||||
// 3. 갈림길에서 목표점까지의 경로
|
||||
string lastNode = fullPath.LastOrDefault() ?? junctionNodeId;
|
||||
var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
|
||||
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
|
||||
{
|
||||
fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
|
||||
}
|
||||
|
||||
return fullPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길에서 방향 전환 시퀀스 생성
|
||||
/// 물리적으로 실현 가능한 방향 전환 경로 생성
|
||||
/// </summary>
|
||||
private List<string> GenerateDirectionChangeSequence(string junctionNodeId, string fromNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
|
||||
{
|
||||
var sequence = new List<string> { junctionNodeId };
|
||||
|
||||
// 방향이 같으면 변경 불필요
|
||||
if (currentDirection == requiredDirection)
|
||||
return sequence;
|
||||
|
||||
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
|
||||
if (junctionInfo == null || !junctionInfo.IsJunction)
|
||||
return sequence;
|
||||
|
||||
// 물리적으로 실현 가능한 방향 전환 시퀀스 생성
|
||||
// 핵심 원리: AGV는 RFID 태그를 읽자마자 바로 방향전환하면 안됨
|
||||
// 왔던 길로 되돌아가지 않도록 다른 노드로 우회한 후 방향전환
|
||||
var connectedNodes = junctionInfo.ConnectedNodes;
|
||||
|
||||
// 왔던 노드(fromNodeId)를 제외한 연결 노드들만 후보로 선택
|
||||
// 이렇게 해야 AGV가 되돌아가는 것을 방지할 수 있음
|
||||
var availableNodes = connectedNodes.Where(nodeId => nodeId != fromNodeId).ToList();
|
||||
|
||||
if (availableNodes.Count > 0)
|
||||
{
|
||||
// 방향 전환을 위한 우회 경로 생성
|
||||
// 예시: 003→004(전진) 상태에서 후진 필요한 경우
|
||||
// 잘못된 방법: 004→003 (왔던 길로 되돌아감)
|
||||
// 올바른 방법: 004→005→004 (005로 우회하여 방향전환)
|
||||
|
||||
// 가장 적합한 우회 노드 선택 (직진 방향 우선, 각도 변화 최소)
|
||||
string detourNode = FindBestDetourNode(junctionNodeId, availableNodes, fromNodeId);
|
||||
if (!string.IsNullOrEmpty(detourNode))
|
||||
{
|
||||
// 1단계: 갈림길에서 우회 노드로 이동 (현재 방향 유지)
|
||||
// AGV는 계속 전진하여 한 태그 더 지나감
|
||||
sequence.Add(detourNode);
|
||||
|
||||
// 2단계: 우회 노드에서 갈림길로 다시 돌아옴 (요구 방향으로 변경)
|
||||
// 이때 AGV는 안전한 위치에서 방향을 전환할 수 있음
|
||||
sequence.Add(junctionNodeId);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 사용 가능한 우회 노드가 없는 경우 (2갈래 길목)
|
||||
// 이 경우 물리적으로 방향 전환이 불가능할 수 있음
|
||||
// 별도의 처리 로직이 필요할 수 있음
|
||||
return sequence;
|
||||
}
|
||||
|
||||
return sequence;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환을 위한 최적의 우회 노드 선택
|
||||
/// AGV의 물리적 특성을 고려한 각도 기반 선택
|
||||
/// </summary>
|
||||
private string FindBestDetourNode(string junctionNodeId, List<string> availableNodes, string excludeNodeId)
|
||||
{
|
||||
// 왔던 길(excludeNodeId)를 제외한 노드 중에서 최적의 우회 노드 선택
|
||||
// 우선순위: 1) 직진방향 2) 가장 작은 각도 변화 3) 막다른 길이 아닌 노드
|
||||
|
||||
var junctionNode = _mapNodes.FirstOrDefault(n => n.NodeId == junctionNodeId);
|
||||
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == excludeNodeId);
|
||||
|
||||
if (junctionNode == null || fromNode == null)
|
||||
return availableNodes.FirstOrDefault();
|
||||
|
||||
string bestNode = null;
|
||||
double minAngleChange = double.MaxValue;
|
||||
bool foundNonDeadEnd = false;
|
||||
|
||||
// AGV가 들어온 방향 벡터 계산 (fromNode → junctionNode)
|
||||
double incomingAngle = CalculateAngle(fromNode.Position, junctionNode.Position);
|
||||
|
||||
foreach (var nodeId in availableNodes)
|
||||
{
|
||||
if (nodeId == excludeNodeId) continue; // 왔던 길 제외
|
||||
|
||||
var candidateNode = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
|
||||
if (candidateNode == null) continue;
|
||||
|
||||
// 갈림길에서 후보 노드로의 방향 벡터 계산 (junctionNode → candidateNode)
|
||||
double outgoingAngle = CalculateAngle(junctionNode.Position, candidateNode.Position);
|
||||
|
||||
// 방향 변화 각도 계산 (0도가 직진, 180도가 유턴)
|
||||
double angleChange = CalculateAngleChange(incomingAngle, outgoingAngle);
|
||||
|
||||
// 막다른 길 여부 확인
|
||||
var nodeConnections = GetAllConnectedNodes(nodeId);
|
||||
bool isDeadEnd = nodeConnections.Count <= 1;
|
||||
|
||||
// 최적 노드 선택 로직
|
||||
bool shouldUpdate = false;
|
||||
|
||||
if (!foundNonDeadEnd && !isDeadEnd)
|
||||
{
|
||||
// 첫 번째 막다른 길이 아닌 노드 발견
|
||||
shouldUpdate = true;
|
||||
foundNonDeadEnd = true;
|
||||
}
|
||||
else if (foundNonDeadEnd && isDeadEnd)
|
||||
{
|
||||
// 이미 막다른 길이 아닌 노드를 찾았으므로 막다른 길은 제외
|
||||
continue;
|
||||
}
|
||||
else if (foundNonDeadEnd == isDeadEnd)
|
||||
{
|
||||
// 같은 조건(둘 다 막다른길 or 둘 다 아님)에서는 각도가 작은 것 선택
|
||||
shouldUpdate = angleChange < minAngleChange;
|
||||
}
|
||||
|
||||
if (shouldUpdate)
|
||||
{
|
||||
minAngleChange = angleChange;
|
||||
bestNode = nodeId;
|
||||
}
|
||||
}
|
||||
|
||||
return bestNode ?? availableNodes.FirstOrDefault(n => n != excludeNodeId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 점 사이의 각도 계산 (라디안 단위)
|
||||
/// </summary>
|
||||
private double CalculateAngle(System.Drawing.Point from, System.Drawing.Point to)
|
||||
{
|
||||
double dx = to.X - from.X;
|
||||
double dy = to.Y - from.Y;
|
||||
return Math.Atan2(dy, dx);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 방향 사이의 각도 변화량 계산 (0~180도 범위)
|
||||
/// 0도에 가까울수록 직진, 180도에 가까울수록 유턴
|
||||
/// </summary>
|
||||
private double CalculateAngleChange(double fromAngle, double toAngle)
|
||||
{
|
||||
// 각도 차이 계산
|
||||
double angleDiff = Math.Abs(toAngle - fromAngle);
|
||||
|
||||
// 0~π 범위로 정규화 (0~180도)
|
||||
if (angleDiff > Math.PI)
|
||||
{
|
||||
angleDiff = 2 * Math.PI - angleDiff;
|
||||
}
|
||||
|
||||
return angleDiff;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 실제 방향 전환이 일어나는 노드 찾기
|
||||
/// </summary>
|
||||
private string FindActualDirectionChangeNode(List<string> changePath, string junctionNodeId)
|
||||
{
|
||||
// 방향전환 경로 구조: [start...junction, detourNode, junction...target]
|
||||
// 실제 방향전환은 detourNode에서 일어남 (AGV가 한 태그 더 지나간 후)
|
||||
|
||||
if (changePath.Count < 3)
|
||||
return junctionNodeId; // 기본값으로 갈림길 반환
|
||||
|
||||
// 갈림길이 두 번 나타나는 위치 찾기
|
||||
int firstJunctionIndex = changePath.IndexOf(junctionNodeId);
|
||||
int lastJunctionIndex = changePath.LastIndexOf(junctionNodeId);
|
||||
|
||||
// 갈림길이 두 번 나타나고, 그 사이에 노드가 있는 경우
|
||||
if (firstJunctionIndex != lastJunctionIndex && lastJunctionIndex - firstJunctionIndex == 2)
|
||||
{
|
||||
// 첫 번째와 두 번째 갈림길 사이에 있는 노드가 실제 방향전환 노드
|
||||
string detourNode = changePath[firstJunctionIndex + 1];
|
||||
return detourNode;
|
||||
}
|
||||
|
||||
// 방향전환 구조를 찾지 못한 경우 기본값 반환
|
||||
return junctionNodeId;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 점 사이의 거리 계산
|
||||
/// </summary>
|
||||
private float CalculateDistance(System.Drawing.Point p1, System.Drawing.Point p2)
|
||||
{
|
||||
float dx = p2.X - p1.X;
|
||||
float dy = p2.Y - p1.Y;
|
||||
return (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 목적지 도킹 방향 요구사항 확인
|
||||
/// </summary>
|
||||
public AgvDirection GetRequiredDockingDirection(string targetNodeId)
|
||||
{
|
||||
var targetNode = _mapNodes.FirstOrDefault(n => n.NodeId == targetNodeId);
|
||||
if (targetNode == null)
|
||||
return AgvDirection.Forward;
|
||||
|
||||
switch (targetNode.Type)
|
||||
{
|
||||
case NodeType.Charging:
|
||||
return AgvDirection.Forward; // 충전기는 전진 도킹
|
||||
case NodeType.Docking:
|
||||
return AgvDirection.Backward; // 일반 도킹은 후진 도킹
|
||||
default:
|
||||
return AgvDirection.Forward; // 기본은 전진
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 계획 요약 정보
|
||||
/// </summary>
|
||||
public string GetPlanSummary()
|
||||
{
|
||||
var junctions = _junctionAnalyzer.GetJunctionSummary();
|
||||
return string.Join("\n", junctions);
|
||||
}
|
||||
}
|
||||
}
|
||||
304
Cs_HMI/AGVNavigationCore/PathFinding/JunctionAnalyzer.cs
Normal file
304
Cs_HMI/AGVNavigationCore/PathFinding/JunctionAnalyzer.cs
Normal file
@@ -0,0 +1,304 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 갈림길 분석 및 마그넷 센서 방향 계산 시스템
|
||||
/// </summary>
|
||||
public class JunctionAnalyzer
|
||||
{
|
||||
/// <summary>
|
||||
/// 갈림길 정보
|
||||
/// </summary>
|
||||
public class JunctionInfo
|
||||
{
|
||||
public string NodeId { get; set; }
|
||||
public List<string> ConnectedNodes { get; set; }
|
||||
public Dictionary<string, MagnetDirection> PathDirections { get; set; }
|
||||
public bool IsJunction => ConnectedNodes.Count > 2;
|
||||
|
||||
public JunctionInfo(string nodeId)
|
||||
{
|
||||
NodeId = nodeId;
|
||||
ConnectedNodes = new List<string>();
|
||||
PathDirections = new Dictionary<string, MagnetDirection>();
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
if (!IsJunction)
|
||||
return $"{NodeId}: 일반노드 ({ConnectedNodes.Count}연결)";
|
||||
|
||||
var paths = string.Join(", ", PathDirections.Select(p => $"{p.Key}({p.Value})"));
|
||||
return $"{NodeId}: 갈림길 - {paths}";
|
||||
}
|
||||
}
|
||||
|
||||
private readonly List<MapNode> _mapNodes;
|
||||
private readonly Dictionary<string, JunctionInfo> _junctions;
|
||||
|
||||
public JunctionAnalyzer(List<MapNode> mapNodes)
|
||||
{
|
||||
_mapNodes = mapNodes ?? new List<MapNode>();
|
||||
_junctions = new Dictionary<string, JunctionInfo>();
|
||||
AnalyzeJunctions();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모든 갈림길 분석
|
||||
/// </summary>
|
||||
private void AnalyzeJunctions()
|
||||
{
|
||||
foreach (var node in _mapNodes)
|
||||
{
|
||||
if (node.IsNavigationNode())
|
||||
{
|
||||
var junctionInfo = AnalyzeNode(node);
|
||||
_junctions[node.NodeId] = junctionInfo;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 개별 노드의 갈림길 정보 분석
|
||||
/// </summary>
|
||||
private JunctionInfo AnalyzeNode(MapNode node)
|
||||
{
|
||||
var junction = new JunctionInfo(node.NodeId);
|
||||
|
||||
// 양방향 연결을 고려하여 모든 연결된 노드 찾기
|
||||
var connectedNodes = GetAllConnectedNodes(node);
|
||||
junction.ConnectedNodes = connectedNodes;
|
||||
|
||||
if (connectedNodes.Count > 2)
|
||||
{
|
||||
// 갈림길인 경우 각 방향별 마그넷 센서 방향 계산
|
||||
CalculateMagnetDirections(node, connectedNodes, junction);
|
||||
}
|
||||
|
||||
return junction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 양방향 연결을 고려한 모든 연결 노드 검색
|
||||
/// </summary>
|
||||
private List<string> GetAllConnectedNodes(MapNode node)
|
||||
{
|
||||
var connected = new HashSet<string>();
|
||||
|
||||
// 직접 연결된 노드들
|
||||
foreach (var connectedId in node.ConnectedNodes)
|
||||
{
|
||||
connected.Add(connectedId);
|
||||
}
|
||||
|
||||
// 역방향 연결된 노드들 (다른 노드에서 이 노드로 연결)
|
||||
foreach (var otherNode in _mapNodes)
|
||||
{
|
||||
if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
|
||||
{
|
||||
connected.Add(otherNode.NodeId);
|
||||
}
|
||||
}
|
||||
|
||||
return connected.ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길에서 각 방향별 마그넷 센서 방향 계산
|
||||
/// </summary>
|
||||
private void CalculateMagnetDirections(MapNode junctionNode, List<string> connectedNodes, JunctionInfo junction)
|
||||
{
|
||||
if (connectedNodes.Count < 3) return;
|
||||
|
||||
// 각 연결 노드의 각도 계산
|
||||
var nodeAngles = new List<(string NodeId, double Angle)>();
|
||||
|
||||
foreach (var connectedId in connectedNodes)
|
||||
{
|
||||
var connectedNode = _mapNodes.FirstOrDefault(n => n.NodeId == connectedId);
|
||||
if (connectedNode != null)
|
||||
{
|
||||
double angle = CalculateAngle(junctionNode.Position, connectedNode.Position);
|
||||
nodeAngles.Add((connectedId, angle));
|
||||
}
|
||||
}
|
||||
|
||||
// 각도순으로 정렬
|
||||
nodeAngles.Sort((a, b) => a.Angle.CompareTo(b.Angle));
|
||||
|
||||
// 마그넷 방향 할당
|
||||
AssignMagnetDirections(nodeAngles, junction);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 점 사이의 각도 계산 (라디안)
|
||||
/// </summary>
|
||||
private double CalculateAngle(Point from, Point to)
|
||||
{
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
double angle = Math.Atan2(deltaY, deltaX);
|
||||
|
||||
// 0~2π 범위로 정규화
|
||||
if (angle < 0)
|
||||
angle += 2 * Math.PI;
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길에서 마그넷 센서 방향 할당
|
||||
/// </summary>
|
||||
private void AssignMagnetDirections(List<(string NodeId, double Angle)> sortedNodes, JunctionInfo junction)
|
||||
{
|
||||
int nodeCount = sortedNodes.Count;
|
||||
|
||||
for (int i = 0; i < nodeCount; i++)
|
||||
{
|
||||
string nodeId = sortedNodes[i].NodeId;
|
||||
MagnetDirection direction;
|
||||
|
||||
if (nodeCount == 3)
|
||||
{
|
||||
// 3갈래: 직진, 좌측, 우측
|
||||
switch (i)
|
||||
{
|
||||
case 0: direction = MagnetDirection.Straight; break;
|
||||
case 1: direction = MagnetDirection.Left; break;
|
||||
case 2: direction = MagnetDirection.Right; break;
|
||||
default: direction = MagnetDirection.Straight; break;
|
||||
}
|
||||
}
|
||||
else if (nodeCount == 4)
|
||||
{
|
||||
// 4갈래: 교차로
|
||||
switch (i)
|
||||
{
|
||||
case 0: direction = MagnetDirection.Straight; break;
|
||||
case 1: direction = MagnetDirection.Left; break;
|
||||
case 2: direction = MagnetDirection.Straight; break; // 반대편
|
||||
case 3: direction = MagnetDirection.Right; break;
|
||||
default: direction = MagnetDirection.Straight; break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 5갈래 이상: 각도 기반 배정
|
||||
double angleStep = 2 * Math.PI / nodeCount;
|
||||
double normalizedIndex = (double)i / nodeCount;
|
||||
|
||||
if (normalizedIndex < 0.33)
|
||||
direction = MagnetDirection.Left;
|
||||
else if (normalizedIndex < 0.67)
|
||||
direction = MagnetDirection.Straight;
|
||||
else
|
||||
direction = MagnetDirection.Right;
|
||||
}
|
||||
|
||||
junction.PathDirections[nodeId] = direction;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 경로에서 요구되는 마그넷 방향 계산 (전진 방향 기준)
|
||||
/// </summary>
|
||||
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId)
|
||||
{
|
||||
if (!_junctions.ContainsKey(currentNodeId))
|
||||
return MagnetDirection.Straight;
|
||||
|
||||
var junction = _junctions[currentNodeId];
|
||||
if (!junction.IsJunction)
|
||||
return MagnetDirection.Straight;
|
||||
|
||||
// 실제 각도 기반으로 마그넷 방향 계산
|
||||
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
|
||||
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
|
||||
var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);
|
||||
|
||||
if (fromNode == null || currentNode == null || toNode == null)
|
||||
return MagnetDirection.Straight;
|
||||
|
||||
// 전진 방향(진행 방향) 계산
|
||||
double incomingAngle = CalculateAngle(fromNode.Position, currentNode.Position);
|
||||
|
||||
// 목표 방향 계산
|
||||
double outgoingAngle = CalculateAngle(currentNode.Position, toNode.Position);
|
||||
|
||||
// 각도 차이 계산 (전진 방향 기준)
|
||||
double angleDiff = outgoingAngle - incomingAngle;
|
||||
|
||||
// 각도를 -π ~ π 범위로 정규화
|
||||
while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
|
||||
while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;
|
||||
|
||||
// 전진 방향 기준으로 마그넷 방향 결정
|
||||
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
|
||||
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
|
||||
return MagnetDirection.Straight;
|
||||
else if (angleDiff < 0) // 음수면 왼쪽 회전
|
||||
return MagnetDirection.Left;
|
||||
else // 양수면 오른쪽 회전
|
||||
return MagnetDirection.Right;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 가능한 갈림길 검색
|
||||
/// </summary>
|
||||
public List<string> FindDirectionChangeJunctions(AgvDirection currentDirection, AgvDirection targetDirection)
|
||||
{
|
||||
var availableJunctions = new List<string>();
|
||||
|
||||
if (currentDirection == targetDirection)
|
||||
return availableJunctions;
|
||||
|
||||
foreach (var junction in _junctions.Values)
|
||||
{
|
||||
if (junction.IsJunction)
|
||||
{
|
||||
// 갈림길에서 방향 전환이 가능한지 확인
|
||||
// (실제로는 더 복잡한 로직이 필요하지만, 일단 모든 갈림길을 후보로 함)
|
||||
availableJunctions.Add(junction.NodeId);
|
||||
}
|
||||
}
|
||||
|
||||
return availableJunctions;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길 정보 반환
|
||||
/// </summary>
|
||||
public JunctionInfo GetJunctionInfo(string nodeId)
|
||||
{
|
||||
return _junctions.ContainsKey(nodeId) ? _junctions[nodeId] : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모든 갈림길 목록 반환
|
||||
/// </summary>
|
||||
public List<JunctionInfo> GetAllJunctions()
|
||||
{
|
||||
return _junctions.Values.Where(j => j.IsJunction).ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 디버깅용 갈림길 정보 출력
|
||||
/// </summary>
|
||||
public List<string> GetJunctionSummary()
|
||||
{
|
||||
var summary = new List<string>();
|
||||
|
||||
foreach (var junction in _junctions.Values.Where(j => j.IsJunction))
|
||||
{
|
||||
summary.Add(junction.ToString());
|
||||
}
|
||||
|
||||
return summary;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -3,7 +3,28 @@ using AGVNavigationCore.Models;
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보
|
||||
/// AGV 마그넷 센서 방향 제어
|
||||
/// </summary>
|
||||
public enum MagnetDirection
|
||||
{
|
||||
/// <summary>
|
||||
/// 직진 - 기본 마그넷 라인 추종
|
||||
/// </summary>
|
||||
Straight = 0,
|
||||
|
||||
/// <summary>
|
||||
/// 좌측 - 마그넷 센서 가중치를 좌측으로 조정
|
||||
/// </summary>
|
||||
Left = 1,
|
||||
|
||||
/// <summary>
|
||||
/// 우측 - 마그넷 센서 가중치를 우측으로 조정
|
||||
/// </summary>
|
||||
Right = 2
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보 (방향 전환 지원 포함)
|
||||
/// </summary>
|
||||
public class NodeMotorInfo
|
||||
{
|
||||
@@ -17,16 +38,84 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// </summary>
|
||||
public AgvDirection MotorDirection { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 마그넷 센서 방향 제어 (갈림길 처리용)
|
||||
/// </summary>
|
||||
public MagnetDirection MagnetDirection { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 다음 노드 ID (경로예측용)
|
||||
/// </summary>
|
||||
public string NextNodeId { get; set; }
|
||||
|
||||
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId = null)
|
||||
/// <summary>
|
||||
/// 회전 가능 노드 여부
|
||||
/// </summary>
|
||||
public bool CanRotate { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환이 발생하는 노드 여부
|
||||
/// </summary>
|
||||
public bool IsDirectionChangePoint { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 특수 동작이 필요한 노드 여부 (갈림길 전진/후진 반복)
|
||||
/// </summary>
|
||||
public bool RequiresSpecialAction { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 특수 동작 설명
|
||||
/// </summary>
|
||||
public string SpecialActionDescription { get; set; }
|
||||
|
||||
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
|
||||
{
|
||||
NodeId = nodeId;
|
||||
MotorDirection = motorDirection;
|
||||
MagnetDirection = magnetDirection;
|
||||
NextNodeId = nextNodeId;
|
||||
CanRotate = false;
|
||||
IsDirectionChangePoint = false;
|
||||
RequiresSpecialAction = false;
|
||||
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}";
|
||||
|
||||
// 마그넷 방향이 직진이 아닌 경우 표시
|
||||
if (MagnetDirection != MagnetDirection.Straight)
|
||||
result += $"({MagnetDirection})";
|
||||
|
||||
if (IsDirectionChangePoint)
|
||||
result += " [방향전환]";
|
||||
|
||||
if (CanRotate)
|
||||
result += " [회전가능]";
|
||||
|
||||
if (RequiresSpecialAction)
|
||||
result += $" [특수동작:{SpecialActionDescription}]";
|
||||
|
||||
return result;
|
||||
}
|
||||
}
|
||||
}
|
||||
67
Cs_HMI/AGVNavigationCore/PathFinding/PathfindingOptions.cs
Normal file
67
Cs_HMI/AGVNavigationCore/PathFinding/PathfindingOptions.cs
Normal file
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 경로 탐색 옵션 설정
|
||||
/// </summary>
|
||||
public class PathfindingOptions
|
||||
{
|
||||
/// <summary>
|
||||
/// 회전 가능 노드 회피 여부 (기본값: false - 회전 허용)
|
||||
/// </summary>
|
||||
public bool AvoidRotationNodes { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// 회전 회피 시 추가 비용 가중치 (회전 노드를 완전 차단하지 않고 높은 비용으로 설정)
|
||||
/// </summary>
|
||||
public float RotationAvoidanceCost { get; set; } = 1000.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 회전 비용 가중치 (기존 회전 비용)
|
||||
/// </summary>
|
||||
public float RotationCostWeight { get; set; } = 50.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 접근 거리
|
||||
/// </summary>
|
||||
public float DockingApproachDistance { get; set; } = 100.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 기본 옵션 생성
|
||||
/// </summary>
|
||||
public static PathfindingOptions Default => new PathfindingOptions();
|
||||
|
||||
/// <summary>
|
||||
/// 회전 회피 옵션 생성
|
||||
/// </summary>
|
||||
public static PathfindingOptions AvoidRotation => new PathfindingOptions
|
||||
{
|
||||
AvoidRotationNodes = true
|
||||
};
|
||||
|
||||
/// <summary>
|
||||
/// 옵션 복사
|
||||
/// </summary>
|
||||
public PathfindingOptions Clone()
|
||||
{
|
||||
return new PathfindingOptions
|
||||
{
|
||||
AvoidRotationNodes = this.AvoidRotationNodes,
|
||||
RotationAvoidanceCost = this.RotationAvoidanceCost,
|
||||
RotationCostWeight = this.RotationCostWeight,
|
||||
DockingApproachDistance = this.DockingApproachDistance
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 설정 정보 문자열
|
||||
/// </summary>
|
||||
public override string ToString()
|
||||
{
|
||||
return $"회전회피: {(AvoidRotationNodes ? "ON" : "OFF")}, " +
|
||||
$"회전비용: {RotationCostWeight}, " +
|
||||
$"회피비용: {RotationAvoidanceCost}";
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user