343 lines
15 KiB
C#
343 lines
15 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|