feat: 방향전환 경로 검증 시스템 구현

- PathValidationResult 클래스를 Validation 폴더에 적절히 배치
- BacktrackingPattern 클래스로 A→B→A 패턴 상세 검출
- DirectionChangePlanner에서 되돌아가기 패턴 자동 검증
- CLAUDE.md에 AGVNavigationCore 프로젝트 구조 가이드 추가
- 빌드 시스템 오류 모두 해결

🤖 Generated with [Claude Code](https://claude.ai/code)

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
ChiKyun Kim
2025-09-17 09:24:45 +09:00
parent 8d5ddbe008
commit cacd7fab1b
15 changed files with 789 additions and 237 deletions

View File

@@ -32,29 +32,23 @@ namespace AGVNavigationCore.PathFinding.Planning
/// <summary>
/// AGV 경로 계산
/// </summary>
public AGVPathResult FindPath(string startNodeId, string targetNodeId, AgvDirection currentDirection = AgvDirection.Forward)
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode, AgvDirection currentDirection = AgvDirection.Forward)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
// 1. 목적지 도킹 방향 요구사항 확인
var requiredDirection = _directionChangePlanner.GetRequiredDockingDirection(targetNodeId);
// 입력 검증
if (startNode == null)
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
if (targetNode == null)
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
// 2. 방향 전환이 필요한지 확인
bool needDirectionChange = (currentDirection != requiredDirection);
// 1. 목적지 도킹 방향 요구사항 확인 (노드의 도킹방향 속성에서 확인)
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
AGVPathResult result;
if (needDirectionChange)
{
// 방향 전환이 필요한 경우
result = PlanPathWithDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);
}
else
{
// 직접 경로 계산
result = PlanDirectPath(startNodeId, targetNodeId, currentDirection);
}
// 통합된 경로 계획 함수 사용
AGVPathResult result = PlanPath(startNode, targetNode, currentDirection, requiredDirection);
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
@@ -73,58 +67,76 @@ namespace AGVNavigationCore.PathFinding.Planning
}
/// <summary>
/// 직접 경로 계획
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
/// </summary>
private AGVPathResult PlanDirectPath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
private AgvDirection? GetRequiredDockingDirection(DockingDirection dockDirection)
{
var basicResult = _basicPathfinder.FindPath(startNodeId, targetNodeId);
if (!basicResult.Success)
switch (dockDirection)
{
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
case DockingDirection.Forward:
return AgvDirection.Forward; // 전진 도킹
case DockingDirection.Backward:
return AgvDirection.Backward; // 후진 도킹
case DockingDirection.DontCare:
default:
return null; // 도킹 방향 상관없음
}
// 기본 경로를 상세 경로로 변환
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
return AGVPathResult.CreateSuccess(
detailedPath,
basicResult.TotalDistance,
basicResult.CalculationTimeMs,
basicResult.ExploredNodeCount,
"직접 경로 - 방향 전환 불필요"
);
}
/// <summary>
/// 방향 전환을 포함한 경로 계획
/// 통합 경로 계획 (직접 경로 또는 방향 전환 경로)
/// </summary>
private AGVPathResult PlanPathWithDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
private AGVPathResult PlanPath(MapNode startNode, MapNode targetNode, AgvDirection currentDirection, AgvDirection? requiredDirection = null)
{
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(startNodeId, targetNodeId, currentDirection, requiredDirection);
bool needDirectionChange = requiredDirection.HasValue && (currentDirection != requiredDirection.Value);
if (!directionChangePlan.Success)
if (needDirectionChange)
{
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
// 방향 전환 경로 계획
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(
startNode.NodeId, targetNode.NodeId, currentDirection, requiredDirection.Value);
if (!directionChangePlan.Success)
{
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
}
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection.Value);
float totalDistance = CalculatePathDistance(detailedPath);
return AGVPathResult.CreateSuccess(
detailedPath,
totalDistance,
0,
0,
directionChangePlan.PlanDescription,
true,
directionChangePlan.DirectionChangeNode
);
}
else
{
// 직접 경로 계획
var basicResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
// 방향 전환 경로를 상세 경로로 변환
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection);
if (!basicResult.Success)
{
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
}
// 거리 계산
float totalDistance = CalculatePathDistance(detailedPath);
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
return AGVPathResult.CreateSuccess(
detailedPath,
totalDistance,
0,
0,
directionChangePlan.PlanDescription,
true,
directionChangePlan.DirectionChangeNode
);
return AGVPathResult.CreateSuccess(
detailedPath,
basicResult.TotalDistance,
basicResult.CalculationTimeMs,
basicResult.ExploredNodeCount,
"직접 경로 - 방향 전환 불필요"
);
}
}
/// <summary>
/// 기본 경로를 상세 경로로 변환
/// </summary>

View File

@@ -4,6 +4,7 @@ using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.PathFinding.Planning
{
@@ -114,37 +115,50 @@ namespace AGVNavigationCore.PathFinding.Planning
}
/// <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)
// 1. 시작점 인근의 갈림길들 우선 검색 (경로 진행 중 우회용)
var nearbyJunctions = FindNearbyJunctions(startNodeId, 2); // 2단계 내의 갈림길
foreach (var junction in nearbyJunctions)
{
foreach (var nodeId in directPath.Path)
if (junction == startNodeId) continue; // 시작점 제외
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junction);
if (junctionInfo != null && junctionInfo.IsJunction)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
// 이 갈림길을 통해 목적지로 갈 수 있는지 확인
if (CanReachTargetViaJunction(junction, targetNodeId) &&
HasSuitableDetourOptions(junction, startNodeId))
{
suitableJunctions.Add(nodeId);
suitableJunctions.Add(junction);
}
}
}
// 추가로 시작점 주변의 갈림길들도 검색
var nearbyJunctions = FindNearbyJunctions(startNodeId, 3); // 3단계 내의 갈림길
foreach (var junction in nearbyJunctions)
// 2. 직진 경로상의 갈림길들도 검색 (단, 되돌아가기 방지)
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath.Success)
{
if (!suitableJunctions.Contains(junction))
foreach (var nodeId in directPath.Path.Skip(2)) // 시작점과 다음 노드는 제외
{
suitableJunctions.Add(junction);
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 직진 경로상에서는 더 엄격한 조건 적용
if (!suitableJunctions.Contains(nodeId) &&
HasMultipleExitOptions(nodeId))
{
suitableJunctions.Add(nodeId);
}
}
}
}
// 거리순으로 정렬 (시작점에서 가까운 순)
// 거리순으로 정렬 (가까운 갈림길 우선 - 인근 우회용)
return SortJunctionsByDistance(startNodeId, suitableJunctions);
}
@@ -244,10 +258,19 @@ namespace AGVNavigationCore.PathFinding.Planning
if (changePath.Count > 0)
{
// **VALIDATION**: 되돌아가기 패턴 검증
var validationResult = ValidateDirectionChangePath(changePath, startNodeId, junctionNodeId);
if (!validationResult.IsValid)
{
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ❌ 갈림길 {junctionNodeId} 경로 검증 실패: {validationResult.ValidationError}");
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId} 검증 실패: {validationResult.ValidationError}");
}
// 실제 방향 전환 노드 찾기 (우회 노드)
string actualDirectionChangeNode = FindActualDirectionChangeNode(changePath, junctionNodeId);
string description = $"갈림길 {GetDisplayName(junctionNodeId)}를 통해 {GetDisplayName(actualDirectionChangeNode)}에서 방향 전환: {currentDirection} → {requiredDirection}";
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ✅ 유효한 방향전환 경로: {string.Join(" ", changePath)}");
return DirectionChangePlan.CreateSuccess(changePath, actualDirectionChangeNode, description);
}
@@ -260,7 +283,7 @@ namespace AGVNavigationCore.PathFinding.Planning
}
/// <summary>
/// 방향 전환 경로 생성
/// 방향 전환 경로 생성 (인근 갈림길 우회 방식)
/// </summary>
private List<string> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
@@ -271,17 +294,69 @@ namespace AGVNavigationCore.PathFinding.Planning
if (!toJunctionPath.Success)
return fullPath;
// 2. 인근 갈림길을 통한 우회인지, 직진 경로상 갈림길인지 판단
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
bool isNearbyDetour = !directPath.Success || !directPath.Path.Contains(junctionNodeId);
if (isNearbyDetour)
{
// 인근 갈림길 우회: 직진하다가 마그넷으로 방향 전환
return GenerateNearbyDetourPath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
}
else
{
// 직진 경로상 갈림길: 기존 방식으로 처리 (단, 되돌아가기 방지)
return GenerateDirectPathChangeRoute(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
}
}
/// <summary>
/// 인근 갈림길을 통한 우회 경로 생성 (예: 012 → 013 → 마그넷으로 016 방향)
/// </summary>
private List<string> GenerateNearbyDetourPath(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. 갈림길에서 방향 전환 처리
// 2. 갈림길에서 방향 전환 후 목적지로
// 이때 마그넷 센서를 이용해 목적지 방향으로 진입
var fromJunctionPath = _pathfinder.FindPath(junctionNodeId, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
}
return fullPath;
}
/// <summary>
/// 직진 경로상 갈림길에서 방향 전환 경로 생성 (기존 방식 개선)
/// </summary>
private List<string> GenerateDirectPathChangeRoute(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) // 첫 번째는 갈림길 자체이므로 제외
if (changeSequence.Count > 1)
{
fullPath.AddRange(changeSequence.Skip(1));
}
@@ -292,7 +367,7 @@ namespace AGVNavigationCore.PathFinding.Planning
var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
fullPath.AddRange(fromJunctionPath.Path.Skip(1));
}
return fullPath;
@@ -361,7 +436,7 @@ namespace AGVNavigationCore.PathFinding.Planning
private string FindBestDetourNode(string junctionNodeId, List<string> availableNodes, string excludeNodeId)
{
// 왔던 길(excludeNodeId)를 제외한 노드 중에서 최적의 우회 노드 선택
// 우선순위: 1) 직진방향 2) 가장 작은 각도 변화 3) 막다른 길이 아닌 노드
// 우선순위: 1) 막다른 길이 아닌 노드 (우회 후 복귀 가능) 2) 직진방향 3) 목적지 방향
var junctionNode = _mapNodes.FirstOrDefault(n => n.NodeId == junctionNodeId);
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == excludeNodeId);
@@ -478,6 +553,163 @@ namespace AGVNavigationCore.PathFinding.Planning
return junctionNodeId;
}
/// <summary>
/// 갈림길에서 적절한 우회 옵션이 있는지 확인
/// </summary>
private bool HasSuitableDetourOptions(string junctionNodeId, string excludeNodeId)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return false;
// 제외할 노드(직전 노드)를 뺀 연결된 노드가 2개 이상이어야 적절한 우회 가능
var availableConnections = junctionInfo.ConnectedNodes
.Where(nodeId => nodeId != excludeNodeId)
.ToList();
// 최소 2개의 우회 옵션이 있어야 함 (갈림길에서 방향전환 후 다시 나갈 수 있어야 함)
return availableConnections.Count >= 2;
}
/// <summary>
/// 갈림길을 통해 목적지에 도달할 수 있는지 확인
/// </summary>
private bool CanReachTargetViaJunction(string junctionNodeId, string targetNodeId)
{
// 갈림길에서 목적지까지의 경로가 존재하는지 확인
var pathToTarget = _pathfinder.FindPath(junctionNodeId, targetNodeId);
return pathToTarget.Success;
}
/// <summary>
/// 갈림길에서 여러 출구 옵션이 있는지 확인 (직진 경로상 갈림길용)
/// </summary>
private bool HasMultipleExitOptions(string junctionNodeId)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return false;
// 최소 3개 이상의 연결 노드가 있어야 적절한 방향전환 가능
return junctionInfo.ConnectedNodes.Count >= 3;
}
/// <summary>
/// 방향전환 경로 검증 - 되돌아가기 패턴 및 물리적 실현성 검증
/// </summary>
private PathValidationResult ValidateDirectionChangePath(List<string> path, string startNodeId, string junctionNodeId)
{
if (path == null || path.Count == 0)
{
return PathValidationResult.CreateInvalid(startNodeId, "", "경로가 비어있습니다.");
}
// 1. 되돌아가기 패턴 검증 (A → B → A)
var backtrackingPatterns = DetectBacktrackingPatterns(path);
if (backtrackingPatterns.Count > 0)
{
var issues = new List<string>();
foreach (var pattern in backtrackingPatterns)
{
issues.Add($"되돌아가기 패턴 발견: {pattern}");
}
string errorMessage = $"되돌아가기 패턴 검출 ({backtrackingPatterns.Count}개): {string.Join(", ", issues)}";
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 경로: {string.Join(" ", path)}");
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 되돌아가기 패턴: {errorMessage}");
return PathValidationResult.CreateInvalidWithBacktracking(
path, backtrackingPatterns, startNodeId, "", junctionNodeId, errorMessage);
}
// 2. 연속된 중복 노드 검증
var duplicates = DetectConsecutiveDuplicates(path);
if (duplicates.Count > 0)
{
string errorMessage = $"연속된 중복 노드 발견: {string.Join(", ", duplicates)}";
return PathValidationResult.CreateInvalid(startNodeId, "", errorMessage);
}
// 3. 경로 연결성 검증
var connectivity = ValidatePathConnectivity(path);
if (!connectivity.IsValid)
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"경로 연결성 오류: {connectivity.ValidationError}");
}
// 4. 갈림길 포함 여부 검증
if (!path.Contains(junctionNodeId))
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"갈림길 {junctionNodeId}이 경로에 포함되지 않음");
}
System.Diagnostics.Debug.WriteLine($"[PathValidation] ✅ 유효한 경로: {string.Join(" ", path)}");
return PathValidationResult.CreateValid(path, startNodeId, "", junctionNodeId);
}
/// <summary>
/// 되돌아가기 패턴 검출 (A → B → A)
/// </summary>
private List<BacktrackingPattern> DetectBacktrackingPatterns(List<string> path)
{
var patterns = new List<BacktrackingPattern>();
for (int i = 0; i < path.Count - 2; i++)
{
string nodeA = path[i];
string nodeB = path[i + 1];
string nodeC = path[i + 2];
// A → B → A 패턴 검출
if (nodeA == nodeC && nodeA != nodeB)
{
var pattern = BacktrackingPattern.Create(nodeA, nodeB, nodeA, i, i + 2);
patterns.Add(pattern);
}
}
return patterns;
}
/// <summary>
/// 연속된 중복 노드 검출
/// </summary>
private List<string> DetectConsecutiveDuplicates(List<string> path)
{
var duplicates = new List<string>();
for (int i = 0; i < path.Count - 1; i++)
{
if (path[i] == path[i + 1])
{
duplicates.Add(path[i]);
}
}
return duplicates;
}
/// <summary>
/// 경로 연결성 검증
/// </summary>
private PathValidationResult ValidatePathConnectivity(List<string> path)
{
for (int i = 0; i < path.Count - 1; i++)
{
string currentNode = path[i];
string nextNode = path[i + 1];
// 두 노드간 직접 연결성 확인 (맵 노드의 ConnectedNodes 리스트 사용)
var currentMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNode);
if (currentMapNode == null || !currentMapNode.ConnectedNodes.Contains(nextNode))
{
return PathValidationResult.CreateInvalid(currentNode, nextNode, $"노드 {currentNode}와 {nextNode} 사이에 연결이 없음");
}
}
return PathValidationResult.CreateNotRequired();
}
/// <summary>
/// 두 점 사이의 거리 계산
/// </summary>
@@ -488,25 +720,6 @@ namespace AGVNavigationCore.PathFinding.Planning
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>
/// 경로 계획 요약 정보