..
This commit is contained in:
backuppc
@@ -194,6 +194,477 @@ 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
|
||||
{
|
||||
public MapNode CurrentNode { get; set; }
|
||||
public MapNode PreviousNode { get; set; }
|
||||
public AgvDirection CurrentDirection { get; set; } // 현재 모터 방향 (Forward/Backward)
|
||||
|
||||
public float GCost { get; set; }
|
||||
public float HCost { get; set; }
|
||||
public float FCost => GCost + HCost;
|
||||
|
||||
public SearchState Parent { get; set; }
|
||||
public string TurnType { get; set; } // Debug info
|
||||
}
|
||||
|
||||
private struct MoveResult
|
||||
{
|
||||
public bool IsPossible;
|
||||
public float Cost;
|
||||
public AgvDirection NextDirection;
|
||||
public string TurnType;
|
||||
}
|
||||
|
||||
private string GetStateKey(SearchState state)
|
||||
{
|
||||
string prevId = state.PreviousNode?.Id ?? "null";
|
||||
// 모터 방향도 상태에 포함해야 함 (같은 노드, 같은 진입이라도 모터방향 다르면 다른 상태 - 스위치백 때문)
|
||||
return $"{state.CurrentNode.Id}_{prevId}_{state.CurrentDirection}";
|
||||
}
|
||||
|
||||
private SearchState GetLowestFCostState(List<SearchState> openSet)
|
||||
{
|
||||
SearchState lowest = openSet[0];
|
||||
for (int i = 1; i < openSet.Count; i++)
|
||||
{
|
||||
if (openSet[i].FCost < lowest.FCost)
|
||||
lowest = openSet[i];
|
||||
}
|
||||
return lowest;
|
||||
}
|
||||
|
||||
private float CalculateHeuristic(MapNode from, MapNode to)
|
||||
{
|
||||
// 유클리드 거리
|
||||
float dx = from.Position.X - to.Position.X;
|
||||
float dy = from.Position.Y - to.Position.Y;
|
||||
return (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
private MoveResult CheckMove(SearchState current, MapNode next)
|
||||
{
|
||||
var res = new MoveResult { IsPossible = false, Cost = float.MaxValue };
|
||||
|
||||
// 1. 기본 거리 비용
|
||||
float dist = (float)Math.Sqrt(Math.Pow(current.CurrentNode.Position.X - next.Position.X, 2) + Math.Pow(current.CurrentNode.Position.Y - next.Position.Y, 2));
|
||||
|
||||
// 2. 회전 각도 계산
|
||||
// Vector Prev -> Curr
|
||||
// Vector Curr -> Next
|
||||
|
||||
double angle = 180.0; // Straight
|
||||
if (current.PreviousNode != null)
|
||||
{
|
||||
// Vector 1 (Prev -> Curr)
|
||||
double v1x = current.CurrentNode.Position.X - current.PreviousNode.Position.X;
|
||||
double v1y = current.CurrentNode.Position.Y - current.PreviousNode.Position.Y;
|
||||
|
||||
// Vector 2 (Curr -> Next)
|
||||
double v2x = next.Position.X - current.CurrentNode.Position.X;
|
||||
double v2y = next.Position.Y - current.CurrentNode.Position.Y;
|
||||
|
||||
// 각도 계산 (0 ~ 180)
|
||||
// 내적 이용: a·b = |a||b|cosθ
|
||||
double dot = v1x * v2x + v1y * v2y;
|
||||
double mag1 = Math.Sqrt(v1x * v1x + v1y * v1y);
|
||||
double mag2 = Math.Sqrt(v2x * v2x + v2y * v2y);
|
||||
|
||||
if (mag1 > 0 && mag2 > 0)
|
||||
{
|
||||
double cosTheta = dot / (mag1 * mag2);
|
||||
// 부동소수점 오차 보정
|
||||
if (cosTheta > 1.0) cosTheta = 1.0;
|
||||
if (cosTheta < -1.0) cosTheta = -1.0;
|
||||
|
||||
double rad = Math.Acos(cosTheta);
|
||||
// 외적을 이용해 좌/우 판별이 가능하지만, 여기서는 "꺾인 정도"만 중요하므로 내적각(0~180)만 사용
|
||||
// 180도 = 직진, 90도 = 직각, 0도 = U턴(완전 뒤로)
|
||||
// 주의: 벡터 방향 기준임.
|
||||
// Prev->Curr 가 (1,0) 이고 Curr->Next가 (1,0) 이면 각도 0도? 아니면 180?
|
||||
// 위 내적 계산에서 같은 방향이면 cos=1 -> acos=0. 즉 0도가 직진임.
|
||||
// 180도가 U턴(역방향).
|
||||
|
||||
// 변환: 사용자가 "120도 이상 완만해야" 라고 했음.
|
||||
// 그림상 11->3->4는 예각(Sharp turn).
|
||||
// 직선 주행시 각도 변화량(Deviation)으로 생각하면:
|
||||
// 직진 = 0도 변화.
|
||||
// 90도 턴 = 90도 변화.
|
||||
// U턴 = 180도 변화.
|
||||
|
||||
angle = rad * (180.0 / Math.PI); // 0(직진) ~ 180(U턴)
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
angle = 0; // 시작점에서는 직진으로 간주 (또는 자유)
|
||||
}
|
||||
|
||||
// 제약 조건 체크
|
||||
// 조건 2: 120도 미만 예각 회전 불가?
|
||||
// 사용자 멘트: "11->3->4 ... 120도 이상 완만해야 이동 가능... 3->4는 각도가 훨씬 작아서 커브 못틈"
|
||||
// 여기서 "각도"가 내각(Inner Angle)을 말하는지, 진행 경로의 꺾임각을 말하는지 중요.
|
||||
// 11->3->4 그림을 보면 "V"자 형태에 가까움. (Sharp Turn)
|
||||
// 즉, "진행 방향의 꺾임각"이 크다(예: 150도 꺾임) = "내각"이 작다(30도).
|
||||
// "120도 이상 완만해야" -> 내각이 120도 이상(점잖은 턴)이어야 한다.
|
||||
// 즉, 꺾임각(Deviation)은 60도 이하여야 한다.
|
||||
|
||||
// 내 알고리즘의 `angle`은 "꺾임각(Deviation)"임. (0=직진, 180=U턴)
|
||||
// 따라서 "내각 120도 이상" == "꺾임각 60도 이하".
|
||||
// 그러므로 angle <= 60 이어야 Normal Turn 가능.
|
||||
|
||||
// 3번 노드 (RFID 3) 특수성:
|
||||
// "3번 노드에서만 180도 턴 가능"
|
||||
// "3->4 처럼 각이 좁은 경우 3번으로 가서 모터방향 바꿔서 4번으로 감"
|
||||
|
||||
bool isRfid3 = current.CurrentNode.RfidId == 3;
|
||||
bool isSharpTurn = angle > 60.0; // 60도보다 많이 꺾이면 Sharp Turn (내각 120도 미만)
|
||||
|
||||
// Case A: Normal Move (완만한 턴)
|
||||
if (!isSharpTurn)
|
||||
{
|
||||
res.IsPossible = true;
|
||||
res.Cost = dist;
|
||||
res.NextDirection = current.CurrentDirection; // 방향 유지
|
||||
res.TurnType = "Normal";
|
||||
return res;
|
||||
}
|
||||
|
||||
// Case B: Sharp Turn or U-Turn
|
||||
if (isSharpTurn)
|
||||
{
|
||||
if (isRfid3)
|
||||
{
|
||||
// 3번 노드에서는 무슨 짓이든(?) 가능하다. (Switch Back)
|
||||
// 멈춰서 방향을 바꾸므로, 모터 방향도 바뀜 (FWD -> BWD or BWD -> FWD)
|
||||
// 비용 패널티 추가 (멈추고 바꾸는 시간 등)
|
||||
res.IsPossible = true;
|
||||
res.Cost = dist + 5000; // 큰 비용 (Switch Penalty)
|
||||
res.NextDirection = (current.CurrentDirection == AgvDirection.Forward)
|
||||
? AgvDirection.Backward
|
||||
: AgvDirection.Forward;
|
||||
res.TurnType = "SwitchBack";
|
||||
return res;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 3번 노드가 아닌데 Sharp Turn 하려고 함 -> 불가
|
||||
res.IsPossible = false;
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
private bool IsDockingDirectionValid(SearchState state, MapNode target)
|
||||
{
|
||||
if (target.DockDirection == DockingDirection.DontCare) return true;
|
||||
|
||||
// 도착 시 모터 방향 확인
|
||||
// SearchState에 저장된 CurrentDirection이 도착 시 모터 방향임.
|
||||
|
||||
if (target.DockDirection == DockingDirection.Forward)
|
||||
return state.CurrentDirection == AgvDirection.Forward;
|
||||
|
||||
if (target.DockDirection == DockingDirection.Backward)
|
||||
return state.CurrentDirection == AgvDirection.Backward;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private AGVPathResult ReconstructPath_New(SearchState endState)
|
||||
{
|
||||
var result = new AGVPathResult { Success = true, TotalDistance = endState.GCost };
|
||||
var pathList = new List<MapNode>();
|
||||
var detailedList = new List<NodeMotorInfo>();
|
||||
|
||||
var current = endState;
|
||||
var pathStack = new Stack<SearchState>();
|
||||
|
||||
while (current != null)
|
||||
{
|
||||
pathStack.Push(current);
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
// Path 재구성
|
||||
int seq = 1;
|
||||
while (pathStack.Count > 0)
|
||||
{
|
||||
var step = pathStack.Pop();
|
||||
pathList.Add(step.CurrentNode);
|
||||
|
||||
// Detailed Info (마그넷 방향 계산 등은 후처리 필요할 수도 있으나 여기선 단순화)
|
||||
// SearchState에는 "어떤 방향으로 왔는지"가 저장되어 있음.
|
||||
|
||||
// 마그넷 방향 계산 (다음 노드가 있을 때)
|
||||
MagnetDirection magDir = MagnetDirection.Straight;
|
||||
if (pathStack.Count > 0)
|
||||
{
|
||||
var nextStep = pathStack.Peek();
|
||||
// Current -> NextStep 이동 시 마그넷 방향
|
||||
if (step.CurrentNode.MagnetDirections.ContainsKey(nextStep.CurrentNode.Id))
|
||||
{
|
||||
var magPos = step.CurrentNode.MagnetDirections[nextStep.CurrentNode.Id];
|
||||
if (magPos == MagnetPosition.L) magDir = MagnetDirection.Left;
|
||||
else if (magPos == MagnetPosition.R) magDir = MagnetDirection.Right;
|
||||
|
||||
// 만약 SwitchBack 상황이라면?
|
||||
// 모터 방향이 바뀌었다면 마그넷 방향도 그에 맞춰야 하나?
|
||||
// 기존 로직 참고: MagnetDirection은 "진행 방향 기준" 좌/우 인가? 아님 절대적?
|
||||
// 보통 "갈림길에서 어느 쪽" 인지 나타냄.
|
||||
}
|
||||
}
|
||||
|
||||
detailedList.Add(new NodeMotorInfo(seq++, step.CurrentNode.Id, step.CurrentNode.RfidId, step.CurrentDirection, null, magDir)); // NextNode는 일단 null
|
||||
}
|
||||
|
||||
// NextNode 정보 채우기
|
||||
for (int i = 0; i < detailedList.Count - 1; i++)
|
||||
{
|
||||
detailedList[i].NextNode = _mapNodes.FirstOrDefault(n => n.Id == detailedList[i+1].NodeId);
|
||||
}
|
||||
|
||||
result.Path = pathList;
|
||||
result.DetailedPath = detailedList;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
public enum MapZone
|
||||
{
|
||||
None,
|
||||
Buffer, // 91 ~ 07
|
||||
Charger, // 73 ~ 10
|
||||
Plating, // 72 ~ 05
|
||||
Loader, // 71 ~ 04
|
||||
Cleaner, // 70 ~ 01
|
||||
Junction // Hub (11, 12, etc)
|
||||
}
|
||||
|
||||
public MapZone GetMapZone(MapNode node)
|
||||
{
|
||||
if (node == null) return MapZone.None;
|
||||
int rfid = node.RfidId;
|
||||
|
||||
// Buffer: 91~07 (Linear)
|
||||
// Assuming 91 is start, 07 is end.
|
||||
// Range check might be tricky if IDs are not sequential.
|
||||
// Using precise list based on map description if possible, acts as a catch-all for now.
|
||||
if (rfid == 91 || (rfid >= 31 && rfid <= 36) || (rfid >= 7 && rfid <= 9)) return MapZone.Buffer;
|
||||
|
||||
// Charger: 73~10
|
||||
if (rfid == 73 || rfid == 6 || rfid == 10) return MapZone.Charger;
|
||||
|
||||
// Plating: 72~5
|
||||
if (rfid == 72 || rfid == 5) return MapZone.Plating;
|
||||
|
||||
// Loader: 71~4
|
||||
if (rfid == 71 || rfid == 4) return MapZone.Loader;
|
||||
|
||||
// Cleaner: 70~1
|
||||
if (rfid == 70 || rfid == 1 || rfid == 2 || rfid == 3) return MapZone.Cleaner;
|
||||
|
||||
// Junction (Hub)
|
||||
if (rfid == 11 || rfid == 12) return MapZone.Junction;
|
||||
|
||||
return MapZone.None;
|
||||
}
|
||||
|
||||
public AGVPathResult CalculateScriptedPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
|
||||
{
|
||||
var startZone = GetMapZone(startNode);
|
||||
var targetZone = GetMapZone(targetNode);
|
||||
|
||||
// 1. Same Zone or Trivial Case -> Use CalculatePath_new
|
||||
if (startZone == targetZone && startZone != MapZone.None && startZone != MapZone.Junction)
|
||||
{
|
||||
return CalculatePath_new(startNode, targetNode, prevNode, prevDir);
|
||||
}
|
||||
|
||||
// 2. Hub Logic (Buffer -> Hub -> Target, etc.)
|
||||
// Logic: Start -> ExitNode -> Hub -> EntryNode -> Target
|
||||
|
||||
MapNode exitNode = GetZoneExitNode(startZone);
|
||||
MapNode entryNode = GetZoneEntryNode(targetZone);
|
||||
|
||||
// If Start/Target are in Junction or Unknown, handle gracefully
|
||||
if (startZone == MapZone.Junction) exitNode = startNode;
|
||||
if (targetZone == MapZone.Junction) entryNode = targetNode;
|
||||
|
||||
if (exitNode == null || entryNode == null)
|
||||
{
|
||||
// Fallback to normal search if zone logic fails
|
||||
return CalculatePath_new(startNode, targetNode, prevNode, prevDir);
|
||||
}
|
||||
|
||||
// Path 1: Start -> Exit
|
||||
var path1 = CalculatePath_new(startNode, exitNode, prevNode, prevDir);
|
||||
if (!path1.Success) return AGVPathResult.CreateFailure($"Zone Exit Failure: {startNode.ID2}->{exitNode.ID2}");
|
||||
|
||||
// Path 2: Exit -> Entry (Hub Crossing)
|
||||
// Use CalculatePath_new for Hub crossing relative to Arrival Direction
|
||||
var lastNode1 = path1.Path.Last();
|
||||
var lastDir1 = path1.DetailedPath.Last().MotorDirection;
|
||||
var prevNode1 = path1.Path.Count > 1 ? path1.Path[path1.Path.Count - 2] : prevNode;
|
||||
|
||||
var path2 = CalculatePath_new(exitNode, entryNode, prevNode1, lastDir1);
|
||||
if (!path2.Success) return AGVPathResult.CreateFailure($"Hub Crossing Failure: {exitNode.ID2}->{entryNode.ID2}");
|
||||
|
||||
// Path 3: Entry -> Target
|
||||
var lastNode2 = path2.Path.Last();
|
||||
var lastDir2 = path2.DetailedPath.Last().MotorDirection;
|
||||
var prevNode2 = path2.Path.Count > 1 ? path2.Path[path2.Path.Count - 2] : lastNode1;
|
||||
|
||||
var path3 = CalculatePath_new(entryNode, targetNode, prevNode2, lastDir2);
|
||||
if (!path3.Success) return AGVPathResult.CreateFailure($"Zone Entry Failure: {entryNode.ID2}->{targetNode.ID2}");
|
||||
|
||||
// Merge Paths
|
||||
var merged = Utility.CombineResults(path1, path2);
|
||||
merged = Utility.CombineResults(merged, path3);
|
||||
|
||||
return merged;
|
||||
}
|
||||
|
||||
private MapNode GetZoneExitNode(MapZone zone)
|
||||
{
|
||||
int exitRfid = -1;
|
||||
switch (zone)
|
||||
{
|
||||
case MapZone.Buffer: exitRfid = 7; break;
|
||||
case MapZone.Charger: exitRfid = 10; break; // Or 6? Assuming 10 based on flow
|
||||
case MapZone.Plating: exitRfid = 5; break;
|
||||
case MapZone.Loader: exitRfid = 4; break;
|
||||
case MapZone.Cleaner: exitRfid = 1; break;
|
||||
case MapZone.Junction: return null;
|
||||
}
|
||||
return _mapNodes.FirstOrDefault(n => n.RfidId == exitRfid);
|
||||
}
|
||||
|
||||
private MapNode GetZoneEntryNode(MapZone zone)
|
||||
{
|
||||
int entryRfid = -1;
|
||||
switch (zone)
|
||||
{
|
||||
case MapZone.Buffer: entryRfid = 7; break; // Bi-directional entry/exit?
|
||||
// Usually Buffer entry might be different (e.g. 91?).
|
||||
// But user didn't specify directional flow constraints for zones other than turn logic.
|
||||
// Let's assume Entry = Exit for single-lane spurs, or define specific entry points.
|
||||
// If Buffer is 91~07, maybe Entry is 7?
|
||||
case MapZone.Charger: entryRfid = 10; break;
|
||||
case MapZone.Plating: entryRfid = 5; break;
|
||||
case MapZone.Loader: entryRfid = 4; break;
|
||||
case MapZone.Cleaner: entryRfid = 1; break;
|
||||
}
|
||||
return _mapNodes.FirstOrDefault(n => n.RfidId == entryRfid);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 길목(Gateway) 기반 고급 경로 계산 (기존 SimulatorForm.CalcPath 이관)
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user