..
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
|
#region Helper Classes & Methods for CalculatePath_new
|
||||||
|
|
||||||
private class SearchState
|
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