..
This commit is contained in:
@@ -194,107 +194,6 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 새로운 경로 계산 로직 (방향성 A* + 제약조건)
|
||||
/// 1. 180도 회전은 RFID 3번에서만 가능
|
||||
/// 2. 120도 미만 예각 회전 불가 (단, RFID 3번에서 스위치백은 가능)
|
||||
/// 3. 목적지 도킹 방향 준수
|
||||
/// </summary>
|
||||
public AGVPathResult CalculatePath_new(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
|
||||
{
|
||||
if (startNode == null || targetNode == null)
|
||||
return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음");
|
||||
|
||||
// 초기 상태 설정
|
||||
var openSet = new List<SearchState>();
|
||||
var closedSet = new HashSet<string>(); // Key: "CurrentID_PrevID"
|
||||
|
||||
// 시작 상태 생성
|
||||
var startState = new SearchState
|
||||
{
|
||||
CurrentNode = startNode,
|
||||
PreviousNode = prevNode, // 진입 방향 계산용
|
||||
CurrentDirection = prevDir, // 현재 모터 방향
|
||||
GCost = 0,
|
||||
HCost = CalculateHeuristic(startNode, targetNode),
|
||||
Parent = null
|
||||
};
|
||||
|
||||
openSet.Add(startState);
|
||||
|
||||
while (openSet.Count > 0)
|
||||
{
|
||||
// F Cost가 가장 낮은 상태 선택
|
||||
var currentState = GetLowestFCostState(openSet);
|
||||
openSet.Remove(currentState);
|
||||
|
||||
// 방문 기록 (상태 기반: 현재노드 + 진입노드)
|
||||
string stateKey = GetStateKey(currentState);
|
||||
if (closedSet.Contains(stateKey)) continue;
|
||||
closedSet.Add(stateKey);
|
||||
|
||||
// 목적지 도달 검사
|
||||
if (currentState.CurrentNode.Id == targetNode.Id)
|
||||
{
|
||||
// 도킹 방향 제약 조건 확인
|
||||
if (IsDockingDirectionValid(currentState, targetNode))
|
||||
{
|
||||
return ReconstructPath_New(currentState);
|
||||
}
|
||||
// 도킹 방향이 안 맞으면? -> 이 경로로는 불가. 다른 경로 탐색 계속.
|
||||
// (단, 제자리 회전이 가능한 경우라면 여기서 추가 처리를 할 수도 있음)
|
||||
// 현재 로직상 도착 후 제자리 회전은 없으므로 Pass.
|
||||
}
|
||||
|
||||
// 이웃 노드 탐색
|
||||
foreach (var nextNodeId in currentState.CurrentNode.ConnectedNodes)
|
||||
{
|
||||
var nextNode = _mapNodes.FirstOrDefault(n => n.Id == nextNodeId);
|
||||
if (nextNode == null || !nextNode.IsActive) continue;
|
||||
|
||||
// 이동 가능 여부 및 비용 계산 (회전 제약 포함)
|
||||
var moveTry = CheckMove(currentState, nextNode);
|
||||
|
||||
if (moveTry.IsPossible)
|
||||
{
|
||||
var newState = new SearchState
|
||||
{
|
||||
CurrentNode = nextNode,
|
||||
PreviousNode = currentState.CurrentNode,
|
||||
CurrentDirection = moveTry.NextDirection,
|
||||
GCost = currentState.GCost + moveTry.Cost,
|
||||
HCost = CalculateHeuristic(nextNode, targetNode),
|
||||
Parent = currentState,
|
||||
TurnType = moveTry.TurnType // 디버깅용
|
||||
};
|
||||
|
||||
// 이미 방문한 더 나은 경로가 있는지 확인
|
||||
// (여기서는 ClosedSet만 체크하고 OpenSet 내 중복 처리는 생략 - 간단 구현)
|
||||
// A* 최적화를 위해 OpenSet 내 동일 상태(Key)가 있고 G Cost가 더 낮다면 Skip해야 함.
|
||||
|
||||
string newStateKey = GetStateKey(newState);
|
||||
if (closedSet.Contains(newStateKey)) continue;
|
||||
|
||||
var existingOpen = openSet.FirstOrDefault(s => GetStateKey(s) == newStateKey);
|
||||
if (existingOpen != null)
|
||||
{
|
||||
if (newState.GCost < existingOpen.GCost)
|
||||
{
|
||||
openSet.Remove(existingOpen);
|
||||
openSet.Add(newState);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
openSet.Add(newState);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return AGVPathResult.CreateFailure("조건을 만족하는 경로를 찾을 수 없습니다.");
|
||||
}
|
||||
|
||||
#region Helper Classes & Methods for CalculatePath_new
|
||||
|
||||
private class SearchState
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
,BACKUPPC/1,backuppc,13.02.2026 10:03,file:///C:/Users/1/AppData/Roaming/LibreOffice/4;
|
||||
Reference in New Issue
Block a user