파일정리
This commit is contained in:
ChiKyun Kim
342
AGVLogic/AGVNavigationCore/Utils/GetNextNodeIdTest.cs
Normal file
342
AGVLogic/AGVNavigationCore/Utils/GetNextNodeIdTest.cs
Normal file
@@ -0,0 +1,342 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// GetNextNodeId() 메서드의 동작을 검증하는 테스트 클래스
|
||||
///
|
||||
/// 테스트 시나리오:
|
||||
/// - 001(65,229) → 002(206,244) → Forward → 003이 나와야 함
|
||||
/// - 001(65,229) → 002(206,244) → Backward → 001이 나와야 함
|
||||
/// - 002(206,244) → 003(278,278) → Forward → 004가 나와야 함
|
||||
/// - 002(206,244) → 003(278,278) → Backward → 002가 나와야 함
|
||||
/// </summary>
|
||||
public class GetNextNodeIdTest
|
||||
{
|
||||
/// <summary>
|
||||
/// 가상의 VirtualAGV 상태를 시뮬레이션하여 GetNextNodeId 테스트
|
||||
/// </summary>
|
||||
public void TestGetNextNodeId()
|
||||
{
|
||||
Console.WriteLine("\n================================================");
|
||||
Console.WriteLine("GetNextNodeId() 동작 검증");
|
||||
Console.WriteLine("================================================\n");
|
||||
|
||||
// 테스트 노드 생성
|
||||
var node001 = new MapNode { Id = "N001", RfidId = 001, Position = new Point(65, 229), ConnectedNodes = new List<string> { "N002" } };
|
||||
var node002 = new MapNode { Id = "N002", RfidId = 002, Position = new Point(206, 244), ConnectedNodes = new List<string> { "N001", "N003" } };
|
||||
var node003 = new MapNode { Id = "N003", RfidId = 003, Position = new Point(278, 278), ConnectedNodes = new List<string> { "N002", "N004" } };
|
||||
var node004 = new MapNode { Id = "N004", RfidId = 004, Position = new Point(380, 340), ConnectedNodes = new List<string> { "N003", "N022", "N031" } };
|
||||
|
||||
var allNodes = new List<MapNode> { node001, node002, node003, node004 };
|
||||
|
||||
// VirtualAGV 시뮬레이션 (실제 인스턴스 생성 불가하므로 로직만 재현)
|
||||
Console.WriteLine("테스트 시나리오 1: 001 → 002 → Forward");
|
||||
Console.WriteLine("─────────────────────────────────────────");
|
||||
TestScenario(
|
||||
"Forward 이동: 001에서 002로, 다음은 Forward",
|
||||
node001.Position, node002, node003,
|
||||
AgvDirection.Forward, allNodes,
|
||||
"003 (예상)"
|
||||
);
|
||||
|
||||
Console.WriteLine("\n테스트 시나리오 2: 001 → 002 → Backward");
|
||||
Console.WriteLine("─────────────────────────────────────────");
|
||||
TestScenario(
|
||||
"Backward 이동: 001에서 002로, 다음은 Backward",
|
||||
node001.Position, node002, node001,
|
||||
AgvDirection.Backward, allNodes,
|
||||
"001 (예상)"
|
||||
);
|
||||
|
||||
Console.WriteLine("\n테스트 시나리오 3: 002 → 003 → Forward");
|
||||
Console.WriteLine("─────────────────────────────────────────");
|
||||
TestScenario(
|
||||
"Forward 이동: 002에서 003으로, 다음은 Forward",
|
||||
node002.Position, node003, node004,
|
||||
AgvDirection.Forward, allNodes,
|
||||
"004 (예상)"
|
||||
);
|
||||
|
||||
Console.WriteLine("\n테스트 시나리오 4: 002 → 003 Forward → Backward");
|
||||
Console.WriteLine("─────────────────────────────────────────");
|
||||
TestScenario(
|
||||
"Forward 이동: 002에서 003으로, 다음은 Backward (경로 반대)",
|
||||
node002.Position, node003, node002,
|
||||
AgvDirection.Backward, allNodes,
|
||||
"002 (예상 - 경로 반대)"
|
||||
);
|
||||
|
||||
Console.WriteLine("\n테스트 시나리오 5: 002 → 003 Backward → Forward");
|
||||
Console.WriteLine("─────────────────────────────────────────");
|
||||
TestScenario(
|
||||
"Backward 이동: 002에서 003으로, 다음은 Forward (경로 반대)",
|
||||
node002.Position, node003, node002,
|
||||
AgvDirection.Forward, allNodes,
|
||||
"002 (예상 - 경로 반대)",
|
||||
AgvDirection.Backward // 현재 모터 방향
|
||||
);
|
||||
|
||||
Console.WriteLine("\n테스트 시나리오 6: 002 → 003 Backward → Backward");
|
||||
Console.WriteLine("─────────────────────────────────────────");
|
||||
TestScenario(
|
||||
"Backward 이동: 002에서 003으로, 다음은 Backward (경로 계속)",
|
||||
node002.Position, node003, node004,
|
||||
AgvDirection.Backward, allNodes,
|
||||
"004 (예상 - 경로 계속)",
|
||||
AgvDirection.Backward // 현재 모터 방향
|
||||
);
|
||||
|
||||
Console.WriteLine("\n\n================================================");
|
||||
Console.WriteLine("테스트 완료");
|
||||
Console.WriteLine("================================================\n");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 개별 테스트 시나리오 실행
|
||||
/// </summary>
|
||||
private void TestScenario(
|
||||
string description,
|
||||
Point prevPos,
|
||||
MapNode currentNode,
|
||||
MapNode expectedNextNode,
|
||||
AgvDirection direction,
|
||||
List<MapNode> allNodes,
|
||||
string expectedNodeIdStr,
|
||||
AgvDirection? currentMotorDirection = null)
|
||||
{
|
||||
// 현재 모터 방향이 지정되지 않으면 direction과 동일하다고 가정
|
||||
AgvDirection motorDir = currentMotorDirection ?? direction;
|
||||
|
||||
Console.WriteLine($"설명: {description}");
|
||||
Console.WriteLine($"이전 위치: {prevPos} (RFID: {allNodes.First(n => n.Position == prevPos)?.RfidId.ToString("0000") ?? "?"})");
|
||||
Console.WriteLine($"현재 노드: {currentNode.Id} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
|
||||
Console.WriteLine($"현재 모터 방향: {motorDir}");
|
||||
Console.WriteLine($"요청 방향: {direction}");
|
||||
|
||||
// 이동 벡터 계산
|
||||
var movementVector = new PointF(
|
||||
currentNode.Position.X - prevPos.X,
|
||||
currentNode.Position.Y - prevPos.Y
|
||||
);
|
||||
|
||||
Console.WriteLine($"이동 벡터: ({movementVector.X}, {movementVector.Y})");
|
||||
|
||||
// 각 후보 노드에 대한 점수 계산
|
||||
Console.WriteLine($"\n현재 노드({currentNode.Id})의 ConnectedNodes: {string.Join(", ", currentNode.ConnectedNodes)}");
|
||||
Console.WriteLine($"가능한 다음 노드들:");
|
||||
|
||||
var candidateNodes = allNodes.Where(n =>
|
||||
currentNode.ConnectedNodes.Contains(n.Id) && n.Id != currentNode.Id
|
||||
).ToList();
|
||||
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
var score = CalculateScoreAndPrint(movementVector, currentNode.Position, candidate, direction);
|
||||
string isExpected = (candidate.Id == expectedNextNode.Id) ? " ← 예상 노드" : "";
|
||||
Console.WriteLine($" {candidate.Id} (RFID: {candidate.RfidId}) - 위치: {candidate.Position} - 점수: {score:F1}{isExpected}");
|
||||
}
|
||||
|
||||
// 최고 점수 노드 선택
|
||||
var bestCandidate = GetBestCandidate(movementVector, currentNode.Position, candidateNodes, direction);
|
||||
|
||||
Console.WriteLine($"\n✓ 선택된 노드: {bestCandidate.Id} (RFID: {bestCandidate.RfidId})");
|
||||
|
||||
if (bestCandidate.Id == expectedNextNode.Id)
|
||||
{
|
||||
Console.WriteLine($"✅ 정답! ({expectedNodeIdStr})");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine($"❌ 오답! 예상: {expectedNextNode.Id}, 실제: {bestCandidate.Id}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 점수 계산 및 상세 정보 출력
|
||||
/// </summary>
|
||||
private float CalculateScoreAndPrint(PointF movementVector, Point currentPos, MapNode candidate, AgvDirection direction)
|
||||
{
|
||||
// 벡터 정규화
|
||||
var movementLength = (float)Math.Sqrt(
|
||||
movementVector.X * movementVector.X +
|
||||
movementVector.Y * movementVector.Y
|
||||
);
|
||||
|
||||
var normalizedMovement = new PointF(
|
||||
movementVector.X / movementLength,
|
||||
movementVector.Y / movementLength
|
||||
);
|
||||
|
||||
// 다음 벡터
|
||||
var toNextVector = new PointF(
|
||||
candidate.Position.X - currentPos.X,
|
||||
candidate.Position.Y - currentPos.Y
|
||||
);
|
||||
|
||||
var toNextLength = (float)Math.Sqrt(
|
||||
toNextVector.X * toNextVector.X +
|
||||
toNextVector.Y * toNextVector.Y
|
||||
);
|
||||
|
||||
var normalizedToNext = new PointF(
|
||||
toNextVector.X / toNextLength,
|
||||
toNextVector.Y / toNextLength
|
||||
);
|
||||
|
||||
// 내적 및 외적 계산
|
||||
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
|
||||
(normalizedMovement.Y * normalizedToNext.Y);
|
||||
|
||||
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
|
||||
(normalizedMovement.Y * normalizedToNext.X);
|
||||
|
||||
float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);
|
||||
|
||||
return score;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 점수 계산 (VirtualAGV.CalculateDirectionalScore()와 동일)
|
||||
/// </summary>
|
||||
private float CalculateDirectionalScore(float dotProduct, float crossProduct, AgvDirection direction)
|
||||
{
|
||||
float baseScore = 0;
|
||||
|
||||
switch (direction)
|
||||
{
|
||||
case AgvDirection.Forward:
|
||||
if (dotProduct > 0.9f)
|
||||
baseScore = 100.0f;
|
||||
else if (dotProduct > 0.5f)
|
||||
baseScore = 80.0f;
|
||||
else if (dotProduct > 0.0f)
|
||||
baseScore = 50.0f;
|
||||
else if (dotProduct > -0.5f)
|
||||
baseScore = 20.0f;
|
||||
break;
|
||||
|
||||
case AgvDirection.Backward:
|
||||
if (dotProduct < -0.9f)
|
||||
baseScore = 100.0f;
|
||||
else if (dotProduct < -0.5f)
|
||||
baseScore = 80.0f;
|
||||
else if (dotProduct < 0.0f)
|
||||
baseScore = 50.0f;
|
||||
else if (dotProduct < 0.5f)
|
||||
baseScore = 20.0f;
|
||||
break;
|
||||
|
||||
case AgvDirection.Left:
|
||||
if (dotProduct > 0.0f)
|
||||
{
|
||||
if (crossProduct > 0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct > 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct > -0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (crossProduct < -0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct < 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct < 0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
break;
|
||||
|
||||
case AgvDirection.Right:
|
||||
if (dotProduct > 0.0f)
|
||||
{
|
||||
if (crossProduct < -0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct < 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct < 0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (crossProduct > 0.5f)
|
||||
baseScore = 100.0f;
|
||||
else if (crossProduct > 0.0f)
|
||||
baseScore = 70.0f;
|
||||
else if (crossProduct > -0.5f)
|
||||
baseScore = 50.0f;
|
||||
else
|
||||
baseScore = 30.0f;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
return baseScore;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 최고 점수 노드 반환
|
||||
/// </summary>
|
||||
private MapNode GetBestCandidate(PointF movementVector, Point currentPos, List<MapNode> candidates, AgvDirection direction)
|
||||
{
|
||||
var movementLength = (float)Math.Sqrt(
|
||||
movementVector.X * movementVector.X +
|
||||
movementVector.Y * movementVector.Y
|
||||
);
|
||||
|
||||
var normalizedMovement = new PointF(
|
||||
movementVector.X / movementLength,
|
||||
movementVector.Y / movementLength
|
||||
);
|
||||
|
||||
MapNode bestCandidate = null;
|
||||
float bestScore = -1;
|
||||
|
||||
foreach (var candidate in candidates)
|
||||
{
|
||||
var toNextVector = new PointF(
|
||||
candidate.Position.X - currentPos.X,
|
||||
candidate.Position.Y - currentPos.Y
|
||||
);
|
||||
|
||||
var toNextLength = (float)Math.Sqrt(
|
||||
toNextVector.X * toNextVector.X +
|
||||
toNextVector.Y * toNextVector.Y
|
||||
);
|
||||
|
||||
var normalizedToNext = new PointF(
|
||||
toNextVector.X / toNextLength,
|
||||
toNextVector.Y / toNextLength
|
||||
);
|
||||
|
||||
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
|
||||
(normalizedMovement.Y * normalizedToNext.Y);
|
||||
|
||||
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
|
||||
(normalizedMovement.Y * normalizedToNext.X);
|
||||
|
||||
float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);
|
||||
|
||||
if (score > bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
bestCandidate = candidate;
|
||||
}
|
||||
}
|
||||
|
||||
return bestCandidate;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user