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:
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user