파일정리
This commit is contained in:
ChiKyun Kim
125
AGVLogic/AGVNavigationCore/Utils/AGVDirectionCalculator.cs
Normal file
125
AGVLogic/AGVNavigationCore/Utils/AGVDirectionCalculator.cs
Normal file
@@ -0,0 +1,125 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 방향 기반 다음 노드 계산기
|
||||
/// VirtualAGV 또는 실제 AGV 시스템에서 현재 방향을 알 때, 다음 목적지 노드를 결정
|
||||
/// </summary>
|
||||
public class AGVDirectionCalculator
|
||||
{
|
||||
private DirectionalPathfinder _pathfinder;
|
||||
|
||||
public AGVDirectionCalculator(DirectionalPathfinder.DirectionWeights weights = null)
|
||||
{
|
||||
_pathfinder = new DirectionalPathfinder(weights);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이전 RFID 위치 + 현재 위치 + 현재 방향을 기반으로 다음 노드 ID를 반환
|
||||
///
|
||||
/// 사용 예시:
|
||||
/// - 001에서 002로 이동 후 GetNextNodeId(001_pos, 002_node, 002_pos, Forward) → 003
|
||||
/// - 003에서 004로 이동 후, Left 선택 → 030
|
||||
/// - 004에서 003으로 이동(Backward) 후, GetNextNodeId(..., Backward) → 002
|
||||
/// </summary>
|
||||
/// <param name="previousRfidPos">이전 RFID 감지 위치</param>
|
||||
/// <param name="currentNode">현재 RFID 노드</param>
|
||||
/// <param name="currentRfidPos">현재 RFID 감지 위치</param>
|
||||
/// <param name="direction">이동 방향</param>
|
||||
/// <param name="allNodes">맵의 모든 노드</param>
|
||||
/// <returns>다음 노드 ID (실패 시 null)</returns>
|
||||
public string GetNextNodeId(
|
||||
Point previousRfidPos,
|
||||
MapNode currentNode,
|
||||
Point currentRfidPos,
|
||||
AgvDirection direction,
|
||||
List<MapNode> allNodes)
|
||||
{
|
||||
// 유효성 검사
|
||||
if (previousRfidPos == Point.Empty)
|
||||
{
|
||||
throw new ArgumentException("previousRfidPos는 빈 값일 수 없습니다. 최소 2개의 위치 히스토리가 필요합니다.");
|
||||
}
|
||||
|
||||
if (currentNode == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(currentNode), "currentNode는 null일 수 없습니다.");
|
||||
}
|
||||
|
||||
if (allNodes == null || allNodes.Count == 0)
|
||||
{
|
||||
throw new ArgumentException("allNodes는 비어있을 수 없습니다.");
|
||||
}
|
||||
|
||||
return _pathfinder.GetNextNodeId(
|
||||
previousRfidPos,
|
||||
currentNode,
|
||||
currentRfidPos,
|
||||
direction,
|
||||
allNodes
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 모터 상태를 기반으로 실제 선택된 방향을 분석
|
||||
/// VirtualAGV의 현재/이전 상태로부터 선택된 방향을 역추적
|
||||
/// </summary>
|
||||
public AgvDirection AnalyzeSelectedDirection(
|
||||
Point previousPos,
|
||||
Point currentPos,
|
||||
MapNode selectedNextNode,
|
||||
List<MapNode> connectedNodes)
|
||||
{
|
||||
if (previousPos == Point.Empty || currentPos == Point.Empty || selectedNextNode == null)
|
||||
{
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
|
||||
// 이동 벡터
|
||||
var movementVector = new PointF(
|
||||
currentPos.X - previousPos.X,
|
||||
currentPos.Y - previousPos.Y
|
||||
);
|
||||
|
||||
// 다음 노드 벡터
|
||||
var nextVector = new PointF(
|
||||
selectedNextNode.Position.X - currentPos.X,
|
||||
selectedNextNode.Position.Y - currentPos.Y
|
||||
);
|
||||
|
||||
// 내적 계산 (유사도)
|
||||
float dotProduct = (movementVector.X * nextVector.X) +
|
||||
(movementVector.Y * nextVector.Y);
|
||||
|
||||
// 외적 계산 (좌우 판별)
|
||||
float crossProduct = (movementVector.X * nextVector.Y) -
|
||||
(movementVector.Y * nextVector.X);
|
||||
|
||||
// 진행 방향 판별
|
||||
if (dotProduct > 0) // 같은 방향으로 진행
|
||||
{
|
||||
if (Math.Abs(crossProduct) < 0.1f) // 거의 직진
|
||||
{
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
else if (crossProduct > 0) // 좌측으로 회전
|
||||
{
|
||||
return AgvDirection.Left;
|
||||
}
|
||||
else // 우측으로 회전
|
||||
{
|
||||
return AgvDirection.Right;
|
||||
}
|
||||
}
|
||||
else // 반대 방향으로 진행 (후진)
|
||||
{
|
||||
return AgvDirection.Backward;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
464
AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
Normal file
464
AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
Normal file
@@ -0,0 +1,464 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Analysis;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 방향 계산 헬퍼 유틸리티
|
||||
/// 현재 위치에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 계산
|
||||
/// 이전 이동 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
|
||||
/// </summary>
|
||||
public static class DirectionalHelper
|
||||
{
|
||||
|
||||
/// <summary>
|
||||
/// AGV방향과 일치하는지 확인한다. 단 원본위치에서 dock 위치가 Don't Care 라면 true가 반환 됩니다.
|
||||
/// </summary>
|
||||
/// <param name="dock"></param>
|
||||
/// <param name="agvdirection"></param>
|
||||
/// <returns></returns>
|
||||
public static bool MatchAGVDirection(this DockingDirection dock, AgvDirection agvdirection)
|
||||
{
|
||||
if (dock == DockingDirection.DontCare) return true;
|
||||
if (dock == DockingDirection.Forward && agvdirection == AgvDirection.Forward) return true;
|
||||
if (dock == DockingDirection.Backward && agvdirection == AgvDirection.Backward) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
private static JunctionAnalyzer _junctionAnalyzer;
|
||||
|
||||
/// <summary>
|
||||
/// JunctionAnalyzer 초기화 (첫 호출 시)
|
||||
/// </summary>
|
||||
private static void InitializeJunctionAnalyzer(List<MapNode> allNodes)
|
||||
{
|
||||
if (_junctionAnalyzer == null && allNodes != null)
|
||||
{
|
||||
_junctionAnalyzer = new JunctionAnalyzer(allNodes);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 노드에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 반환
|
||||
/// 이전 모터 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
|
||||
/// </summary>
|
||||
/// <param name="currentNode">현재 노드</param>
|
||||
/// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
|
||||
/// <param name="prevDirection">이전 구간의 모터 방향</param>
|
||||
/// <param name="direction">현재 모터 방향 (Forward 또는 Backward)</param>
|
||||
/// <param name="magnetDirection">현재 마그넷 방향 (Straight/Left/Right)</param>
|
||||
/// <param name="allNodes">모든 맵 노드</param>
|
||||
/// <returns>다음 노드 (또는 null)</returns>
|
||||
public static MapNode GetNextNodeByDirection(
|
||||
MapNode currentNode,
|
||||
MapNode prevNode,
|
||||
AgvDirection prevDirection,
|
||||
AgvDirection direction,
|
||||
MagnetDirection magnetDirection,
|
||||
List<MapNode> allNodes)
|
||||
{
|
||||
if (currentNode == null || prevNode == null || allNodes == null)
|
||||
return null;
|
||||
|
||||
// JunctionAnalyzer 초기화
|
||||
InitializeJunctionAnalyzer(allNodes);
|
||||
|
||||
// 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
|
||||
var connectedMapNodes = currentNode.ConnectedMapNodes;
|
||||
if (connectedMapNodes == null || connectedMapNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
List<MapNode> candidateNodes = new List<MapNode>();
|
||||
if (prevDirection == direction)
|
||||
{
|
||||
candidateNodes = connectedMapNodes.Where(n => n.Id != prevNode.Id).ToList();
|
||||
}
|
||||
else
|
||||
{
|
||||
candidateNodes = connectedMapNodes.ToList();
|
||||
}
|
||||
|
||||
if (candidateNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 이전→현재 이동 벡터
|
||||
var movementVector = new PointF(
|
||||
currentNode.Position.X - prevNode.Position.X,
|
||||
currentNode.Position.Y - prevNode.Position.Y
|
||||
);
|
||||
|
||||
var movementLength = (float)Math.Sqrt(
|
||||
movementVector.X * movementVector.X +
|
||||
movementVector.Y * movementVector.Y
|
||||
);
|
||||
|
||||
if (movementLength < 0.001f)
|
||||
return candidateNodes[0];
|
||||
|
||||
var normalizedMovement = new PointF(
|
||||
movementVector.X / movementLength,
|
||||
movementVector.Y / movementLength
|
||||
);
|
||||
|
||||
// 각 후보 노드에 대해 점수 계산
|
||||
MapNode bestNode = null;
|
||||
float bestScore = float.MinValue;
|
||||
|
||||
Console.WriteLine(
|
||||
$"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 ==========");
|
||||
Console.WriteLine(
|
||||
$" 현재노드: {currentNode.RfidId}[{currentNode.Id}]({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
|
||||
Console.WriteLine(
|
||||
$" 이전노드: {prevNode.RfidId}[{prevNode.Id}]({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
|
||||
Console.WriteLine(
|
||||
$" 이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})");
|
||||
Console.WriteLine(
|
||||
$" 현재방향: {direction}, 이전방향: {prevDirection}, 마그넷방향: {magnetDirection}");
|
||||
Console.WriteLine(
|
||||
$" 후보노드 개수: {candidateNodes.Count}");
|
||||
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
var toNextVector = new PointF(
|
||||
candidate.Position.X - currentNode.Position.X,
|
||||
candidate.Position.Y - currentNode.Position.Y
|
||||
);
|
||||
|
||||
var toNextLength = (float)Math.Sqrt(
|
||||
toNextVector.X * toNextVector.X +
|
||||
toNextVector.Y * toNextVector.Y
|
||||
);
|
||||
|
||||
if (toNextLength < 0.001f)
|
||||
continue;
|
||||
|
||||
var normalizedToNext = new PointF(
|
||||
toNextVector.X / toNextLength,
|
||||
toNextVector.Y / toNextLength
|
||||
);
|
||||
|
||||
// 내적 계산 (유사도: -1 ~ 1)
|
||||
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
|
||||
(normalizedMovement.Y * normalizedToNext.Y);
|
||||
|
||||
float score;
|
||||
if (direction == prevDirection)
|
||||
{
|
||||
// Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
|
||||
score = dotProduct;
|
||||
}
|
||||
else // Backward
|
||||
{
|
||||
// Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
|
||||
score = -dotProduct;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"\n [후보] {candidate.RfidId}[{candidate.Id}]({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
|
||||
Console.WriteLine(
|
||||
$" 벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}");
|
||||
Console.WriteLine(
|
||||
$" 정규화벡터: ({normalizedToNext.X:F3}, {normalizedToNext.Y:F3})");
|
||||
Console.WriteLine(
|
||||
$" 내적(dotProduct): {dotProduct:F4}");
|
||||
Console.WriteLine(
|
||||
$" 기본점수 ({(direction == prevDirection ? "방향유지" : "방향변경")}): {score:F4}");
|
||||
|
||||
// 이전 모터 방향이 제공된 경우: 방향 일관성 보너스 추가
|
||||
var scoreBeforeMotor = score;
|
||||
score = ApplyMotorDirectionConsistencyBonus(
|
||||
score,
|
||||
direction,
|
||||
prevDirection,
|
||||
dotProduct
|
||||
);
|
||||
Console.WriteLine(
|
||||
$" 모터방향 적용 후: {scoreBeforeMotor:F4} → {score:F4}");
|
||||
|
||||
// 마그넷 방향을 고려한 점수 조정
|
||||
var scoreBeforeMagnet = score;
|
||||
score = ApplyMagnetDirectionBonus(
|
||||
score,
|
||||
magnetDirection,
|
||||
normalizedMovement,
|
||||
normalizedToNext,
|
||||
currentNode,
|
||||
candidate,
|
||||
direction
|
||||
);
|
||||
Console.WriteLine(
|
||||
$" 마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}");
|
||||
|
||||
if (score > bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
bestNode = candidate;
|
||||
Console.WriteLine(
|
||||
$" ⭐ 현재 최고점수 선택됨!");
|
||||
}
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"\n 최종선택: {bestNode?.RfidId ?? 0}[{bestNode?.Id ?? "null"}] (점수: {bestScore:F4})");
|
||||
Console.WriteLine(
|
||||
$"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n");
|
||||
|
||||
return bestNode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모터 방향 일관성을 고려한 점수 보정
|
||||
/// 같은 방향으로 계속 이동하는 경우 보너스 점수 부여
|
||||
/// </summary>
|
||||
/// <param name="baseScore">기본 점수</param>
|
||||
/// <param name="currentDirection">현재 모터 방향</param>
|
||||
/// <param name="prevMotorDirection">이전 모터 방향</param>
|
||||
/// <param name="dotProduct">벡터 내적값</param>
|
||||
/// <returns>조정된 점수</returns>
|
||||
private static float ApplyMotorDirectionConsistencyBonus(
|
||||
float baseScore,
|
||||
AgvDirection currentDirection,
|
||||
AgvDirection prevMotorDirection,
|
||||
float dotProduct)
|
||||
{
|
||||
float adjustedScore = baseScore;
|
||||
|
||||
// 모터 방향이 변경되지 않은 경우: 일관성 보너스
|
||||
if (currentDirection == prevMotorDirection)
|
||||
{
|
||||
// Forward 지속: 직진 방향으로의 이동 선호
|
||||
// Backward 지속: 반대 방향으로의 이동 선호
|
||||
const float CONSISTENCY_BONUS = 0.2f;
|
||||
adjustedScore += CONSISTENCY_BONUS;
|
||||
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"[DirectionalHelper] 모터 방향 일관성 보너스: {currentDirection} → {currentDirection} " +
|
||||
$"(점수: {baseScore:F3} → {adjustedScore:F3})");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 모터 방향이 변경된 경우: 방향 변경 페널티
|
||||
const float DIRECTION_CHANGE_PENALTY = 0.15f;
|
||||
adjustedScore -= DIRECTION_CHANGE_PENALTY;
|
||||
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"[DirectionalHelper] 모터 방향 변경 페널티: {prevMotorDirection} → {currentDirection} " +
|
||||
$"(점수: {baseScore:F3} → {adjustedScore:F3})");
|
||||
}
|
||||
|
||||
return adjustedScore;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 마그넷 방향을 고려한 점수 보정
|
||||
/// Straight/Left/Right 마그넷 방향에 따라 후보 노드를 평가
|
||||
/// </summary>
|
||||
/// <param name="baseScore">기본 점수</param>
|
||||
/// <param name="magnetDirection">마그넷 방향 (Straight/Left/Right)</param>
|
||||
/// <param name="normalizedMovement">정규화된 이동 벡터</param>
|
||||
/// <param name="normalizedToNext">정규화된 다음 이동 벡터</param>
|
||||
/// <param name="currentNode">현재 노드</param>
|
||||
/// <param name="candidate">후보 노드</param>
|
||||
/// <returns>조정된 점수</returns>
|
||||
private static float ApplyMagnetDirectionBonus(
|
||||
float baseScore,
|
||||
MagnetDirection magnetDirection,
|
||||
PointF normalizedMovement,
|
||||
PointF normalizedToNext,
|
||||
MapNode currentNode,
|
||||
MapNode candidate,
|
||||
AgvDirection direction)
|
||||
{
|
||||
float adjustedScore = baseScore;
|
||||
|
||||
// Straight: 일직선 방향 (높은 내적 보너스)
|
||||
if (magnetDirection == MagnetDirection.Straight)
|
||||
{
|
||||
const float STRAIGHT_BONUS = 0.5f;
|
||||
adjustedScore += STRAIGHT_BONUS;
|
||||
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}");
|
||||
}
|
||||
// Left 또는 Right: 모터 위치에 따른 회전 방향 판단
|
||||
else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right)
|
||||
{
|
||||
// 2D 외적: movement × toNext = movement.X * toNext.Y - movement.Y * toNext.X
|
||||
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
|
||||
(normalizedMovement.Y * normalizedToNext.X);
|
||||
|
||||
bool isLeftMotorMatch = false;
|
||||
bool isRightMotorMatch = false;
|
||||
|
||||
// ===== 정방향(Forward) 이동 =====
|
||||
if (direction == AgvDirection.Forward)
|
||||
{
|
||||
// Forward 이동 시 외적 판정:
|
||||
// - 외적 < 0 (음수) = 반시계 회전 = Left 모터 멈춤
|
||||
// - 외적 > 0 (양수) = 시계 회전 = Right 모터 멈춤
|
||||
//
|
||||
// 예: 004 → 012 → 016 (Left 모터)
|
||||
// 외적 = -0.9407 (음수) → 반시계 → Left 일치 ✅
|
||||
|
||||
isLeftMotorMatch = crossProduct < 0; // 음수 = 반시계 = Left 멈춤
|
||||
isRightMotorMatch = crossProduct > 0; // 양수 = 시계 = Right 멈춤
|
||||
}
|
||||
// ===== 역방향(Backward) 이동 =====
|
||||
else // Backward
|
||||
{
|
||||
// Backward 이동 시 외적 판정:
|
||||
// - 외적 < 0 (음수) = 시계 회전 = Left 모터 멈춤
|
||||
// - 외적 > 0 (양수) = 반시계 회전 = Right 모터 멈춤
|
||||
//
|
||||
// 예: 012 → 004 → 003 (Left 모터)
|
||||
// 외적 = 0.9334 (양수) → 반시계(역방향 기준 시계) → Left 일치 ✅
|
||||
|
||||
isLeftMotorMatch = crossProduct > 0; // 양수 = 시계(역) = Left 멈춤
|
||||
isRightMotorMatch = crossProduct < 0; // 음수 = 반시계(역) = Right 멈춤
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] 외적(Cross): {crossProduct:F4}, Left모터일치: {isLeftMotorMatch}, Right모터일치: {isRightMotorMatch} [{direction}]");
|
||||
|
||||
// 외적의 절대값으로 회전 강도 판단 (0에 가까우면 약함, 1에 가까우면 강함)
|
||||
float rotationStrength = Math.Abs(crossProduct);
|
||||
|
||||
if ((magnetDirection == MagnetDirection.Left && isLeftMotorMatch) ||
|
||||
(magnetDirection == MagnetDirection.Right && isRightMotorMatch))
|
||||
{
|
||||
// 올바른 모터 방향: 회전 강도에 비례한 보너스
|
||||
// 강한 회전(|외적| ≈ 1): +2.0
|
||||
// 약한 회전(|외적| ≈ 0.2): +0.4
|
||||
float magnetBonus = rotationStrength * 2.0f;
|
||||
adjustedScore += magnetBonus;
|
||||
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] ✅ {magnetDirection} 모터 일치 (회전강도: {rotationStrength:F4}, 보너스 +{magnetBonus:F4}): {baseScore:F4} → {adjustedScore:F4}");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 잘못된 모터 방향: 회전 강도에 비례한 페널티
|
||||
// 강한 회전(|외적| ≈ 1): -2.0
|
||||
// 약한 회전(|외적| ≈ 0.2): -0.4
|
||||
float magnetPenalty = rotationStrength * 2.0f;
|
||||
adjustedScore -= magnetPenalty;
|
||||
|
||||
string actualMotor = crossProduct > 0 ? "Left" : "Right";
|
||||
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] ❌ {magnetDirection} 모터 불일치 (실제: {actualMotor}, 회전강도: {rotationStrength:F4}, 페널티 -{magnetPenalty:F4}): {baseScore:F4} → {adjustedScore:F4}");
|
||||
}
|
||||
}
|
||||
|
||||
return adjustedScore;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모터 방향을 고려한 다음 노드 선택 (디버깅/분석용)
|
||||
/// </summary>
|
||||
public static (MapNode node, float score, string reason) GetNextNodeByDirectionWithDetails(
|
||||
MapNode currentNode,
|
||||
MapNode prevNode,
|
||||
AgvDirection direction,
|
||||
List<MapNode> allNodes,
|
||||
AgvDirection? prevMotorDirection)
|
||||
{
|
||||
if (currentNode == null || prevNode == null || allNodes == null)
|
||||
return (null, 0, "입력 파라미터가 null입니다");
|
||||
|
||||
var connectedMapNodes = currentNode.ConnectedMapNodes;
|
||||
if (connectedMapNodes == null || connectedMapNodes.Count == 0)
|
||||
return (null, 0, "연결된 노드가 없습니다");
|
||||
|
||||
var candidateNodes = connectedMapNodes.ToList();
|
||||
|
||||
if (candidateNodes.Count == 0)
|
||||
return (null, 0, "후보 노드가 없습니다");
|
||||
|
||||
var movementVector = new PointF(
|
||||
currentNode.Position.X - prevNode.Position.X,
|
||||
currentNode.Position.Y - prevNode.Position.Y
|
||||
);
|
||||
|
||||
var movementLength = (float)Math.Sqrt(
|
||||
movementVector.X * movementVector.X +
|
||||
movementVector.Y * movementVector.Y
|
||||
);
|
||||
|
||||
if (movementLength < 0.001f)
|
||||
return (candidateNodes[0], 1.0f, "움직임이 거의 없음");
|
||||
|
||||
var normalizedMovement = new PointF(
|
||||
movementVector.X / movementLength,
|
||||
movementVector.Y / movementLength
|
||||
);
|
||||
|
||||
MapNode bestNode = null;
|
||||
float bestScore = float.MinValue;
|
||||
string reason = "";
|
||||
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
var toNextVector = new PointF(
|
||||
candidate.Position.X - currentNode.Position.X,
|
||||
candidate.Position.Y - currentNode.Position.Y
|
||||
);
|
||||
|
||||
var toNextLength = (float)Math.Sqrt(
|
||||
toNextVector.X * toNextVector.X +
|
||||
toNextVector.Y * toNextVector.Y
|
||||
);
|
||||
|
||||
if (toNextLength < 0.001f)
|
||||
continue;
|
||||
|
||||
var normalizedToNext = new PointF(
|
||||
toNextVector.X / toNextLength,
|
||||
toNextVector.Y / toNextLength
|
||||
);
|
||||
|
||||
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
|
||||
(normalizedMovement.Y * normalizedToNext.Y);
|
||||
|
||||
float score = (direction == AgvDirection.Forward) ? dotProduct : -dotProduct;
|
||||
|
||||
if (prevMotorDirection.HasValue)
|
||||
{
|
||||
score = ApplyMotorDirectionConsistencyBonus(
|
||||
score,
|
||||
direction,
|
||||
prevMotorDirection.Value,
|
||||
dotProduct
|
||||
);
|
||||
}
|
||||
|
||||
if (score > bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
bestNode = candidate;
|
||||
|
||||
// 선택 이유 생성
|
||||
if (prevMotorDirection.HasValue && direction == prevMotorDirection)
|
||||
{
|
||||
reason = $"모터 방향 일관성 유지 ({direction}) → {candidate.Id}";
|
||||
}
|
||||
else if (prevMotorDirection.HasValue)
|
||||
{
|
||||
reason = $"모터 방향 변경 ({prevMotorDirection} → {direction}) → {candidate.Id}";
|
||||
}
|
||||
else
|
||||
{
|
||||
reason = $"방향 기반 선택 ({direction}) → {candidate.Id}";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (bestNode, bestScore, reason);
|
||||
}
|
||||
}
|
||||
}
|
||||
198
AGVLogic/AGVNavigationCore/Utils/DirectionalPathfinderTest.cs
Normal file
198
AGVLogic/AGVNavigationCore/Utils/DirectionalPathfinderTest.cs
Normal file
@@ -0,0 +1,198 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.IO;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// DirectionalPathfinder 테스트 클래스
|
||||
/// NewMap.json 로드하여 방향별 다음 노드를 검증
|
||||
/// </summary>
|
||||
public class DirectionalPathfinderTest
|
||||
{
|
||||
private List<MapNode> _allNodes;
|
||||
private Dictionary<ushort, MapNode> _nodesByRfidId;
|
||||
private AGVDirectionCalculator _calculator;
|
||||
|
||||
public DirectionalPathfinderTest()
|
||||
{
|
||||
_nodesByRfidId = new Dictionary<ushort, MapNode>();
|
||||
_calculator = new AGVDirectionCalculator();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NewMap.json 파일 로드
|
||||
/// </summary>
|
||||
public bool LoadMapFile(string filePath)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!File.Exists(filePath))
|
||||
{
|
||||
Console.WriteLine($"파일을 찾을 수 없습니다: {filePath}");
|
||||
return false;
|
||||
}
|
||||
|
||||
string jsonContent = File.ReadAllText(filePath);
|
||||
var mapData = JsonConvert.DeserializeObject<MapFileData>(jsonContent);
|
||||
|
||||
if (mapData?.Nodes == null || mapData.Nodes.Count == 0)
|
||||
{
|
||||
Console.WriteLine("맵 파일이 비어있습니다.");
|
||||
return false;
|
||||
}
|
||||
|
||||
_allNodes = mapData.Nodes;
|
||||
|
||||
// RFID ID로 인덱싱
|
||||
foreach (var node in _allNodes)
|
||||
{
|
||||
if (node.HasRfid())
|
||||
{
|
||||
_nodesByRfidId[node.RfidId] = node;
|
||||
}
|
||||
}
|
||||
|
||||
Console.WriteLine($"✓ 맵 파일 로드 성공: {_allNodes.Count}개 노드 로드");
|
||||
return true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"✗ 맵 파일 로드 실패: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 테스트: RFID 번호로 노드를 찾고, 다음 노드를 계산
|
||||
/// </summary>
|
||||
public void TestDirectionalMovement(ushort previousRfidId, ushort currentRfidId, AgvDirection direction)
|
||||
{
|
||||
Console.WriteLine($"\n========================================");
|
||||
Console.WriteLine($"테스트: {previousRfidId} → {currentRfidId} (방향: {direction})");
|
||||
Console.WriteLine($"========================================");
|
||||
|
||||
// RFID ID로 노드 찾기
|
||||
if (!_nodesByRfidId.TryGetValue(previousRfidId, out var previousNode))
|
||||
{
|
||||
Console.WriteLine($"✗ 이전 RFID를 찾을 수 없습니다: {previousRfidId}");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_nodesByRfidId.TryGetValue(currentRfidId, out var currentNode))
|
||||
{
|
||||
Console.WriteLine($"✗ 현재 RFID를 찾을 수 없습니다: {currentRfidId}");
|
||||
return;
|
||||
}
|
||||
|
||||
Console.WriteLine($"이전 노드: {previousNode.Id} (RFID: {previousNode.RfidId}) - 위치: {previousNode.Position}");
|
||||
Console.WriteLine($"현재 노드: {currentNode.Id} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
|
||||
Console.WriteLine($"이동 벡터: ({currentNode.Position.X - previousNode.Position.X}, " +
|
||||
$"{currentNode.Position.Y - previousNode.Position.Y})");
|
||||
|
||||
// 다음 노드 계산
|
||||
string nextNodeId = _calculator.GetNextNodeId(
|
||||
previousNode.Position,
|
||||
currentNode,
|
||||
currentNode.Position,
|
||||
direction,
|
||||
_allNodes
|
||||
);
|
||||
|
||||
if (string.IsNullOrEmpty(nextNodeId))
|
||||
{
|
||||
Console.WriteLine($"✗ 다음 노드를 찾을 수 없습니다.");
|
||||
return;
|
||||
}
|
||||
|
||||
// 다음 노드 정보 출력
|
||||
var nextNode = _allNodes.FirstOrDefault(n => n.Id == nextNodeId);
|
||||
if (nextNode != null)
|
||||
{
|
||||
Console.WriteLine($"✓ 다음 노드: {nextNode.Id} (RFID: {nextNode.RfidId}) - 위치: {nextNode.Position}");
|
||||
Console.WriteLine($" ├─ 노드 타입: {GetNodeTypeName(nextNode.Type)}");
|
||||
Console.WriteLine($" └─ 연결된 노드: {string.Join(", ", nextNode.ConnectedNodes)}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine($"✗ 다음 노드 정보를 찾을 수 없습니다: {nextNodeId}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모든 노드 정보 출력
|
||||
/// </summary>
|
||||
public void PrintAllNodes()
|
||||
{
|
||||
Console.WriteLine("\n========== 모든 노드 정보 ==========");
|
||||
foreach (var node in _allNodes.OrderBy(n => n.RfidId))
|
||||
{
|
||||
Console.WriteLine($"{node.RfidId:D3} → {node.Id} ({GetNodeTypeName(node.Type)})");
|
||||
Console.WriteLine($" 위치: {node.Position}, 연결: {string.Join(", ", node.ConnectedNodes)}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 RFID 노드의 상세 정보 출력
|
||||
/// </summary>
|
||||
public void PrintNodeInfo(ushort rfidId)
|
||||
{
|
||||
if (!_nodesByRfidId.TryGetValue(rfidId, out var node))
|
||||
{
|
||||
Console.WriteLine($"노드를 찾을 수 없습니다: {rfidId}");
|
||||
return;
|
||||
}
|
||||
|
||||
Console.WriteLine($"\n========== RFID {rfidId} 상세 정보 ==========");
|
||||
Console.WriteLine($"노드 ID: {node.Id}");
|
||||
Console.WriteLine($"RFID: {node.RfidId}");
|
||||
Console.WriteLine($"ALIAS: {node.AliasName}");
|
||||
Console.WriteLine($"위치: {node.Position}");
|
||||
Console.WriteLine($"타입: {GetNodeTypeName(node.Type)}");
|
||||
Console.WriteLine($"TurnLeft/Right/교차로 : {(node.CanTurnLeft ? "O":"X")}/{(node.CanTurnRight ? "O" : "X")}/{(node.DisableCross ? "X" : "O")}");
|
||||
Console.WriteLine($"활성: {node.IsActive}");
|
||||
Console.WriteLine($"연결된 노드:");
|
||||
|
||||
if (node.ConnectedNodes.Count == 0)
|
||||
{
|
||||
Console.WriteLine(" (없음)");
|
||||
}
|
||||
else
|
||||
{
|
||||
foreach (var connectedId in node.ConnectedNodes)
|
||||
{
|
||||
var connectedNode = _allNodes.FirstOrDefault(n => n.Id == connectedId);
|
||||
if (connectedNode != null)
|
||||
{
|
||||
Console.WriteLine($" → {connectedId} (RFID: {connectedNode.RfidId}) - 위치: {connectedNode.Position}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine($" → {connectedId} (노드 찾을 수 없음)");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private string GetNodeTypeName(NodeType type)
|
||||
{
|
||||
return type.ToString();
|
||||
}
|
||||
|
||||
// JSON 파일 매핑을 위한 임시 클래스
|
||||
[Serializable]
|
||||
private class MapFileData
|
||||
{
|
||||
[JsonProperty("Nodes")]
|
||||
public List<MapNode> Nodes { get; set; }
|
||||
|
||||
[JsonProperty("RfidMappings")]
|
||||
public List<dynamic> RfidMappings { get; set; }
|
||||
}
|
||||
}
|
||||
}
|
||||
346
AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
Normal file
346
AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
Normal file
@@ -0,0 +1,346 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Core;
|
||||
using AGVNavigationCore.PathFinding.Validation;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 도킹 방향 검증 유틸리티
|
||||
/// 경로 계산 후 마지막 도킹 방향이 올바른지 검증
|
||||
/// </summary>
|
||||
public static class DockingValidator
|
||||
{
|
||||
/// <summary>
|
||||
/// 경로의 도킹 방향 검증
|
||||
/// </summary>
|
||||
/// <param name="pathResult">경로 계산 결과</param>
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
/// <param name="currentDirection">AGV 현재 방향</param>
|
||||
/// <returns>도킹 검증 결과</returns>
|
||||
public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes)
|
||||
{
|
||||
// 경로가 없거나 실패한 경우
|
||||
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 경로 없음");
|
||||
return DockingValidationResult.CreateNotRequired();
|
||||
}
|
||||
if (pathResult.DetailedPath.Any() == false && pathResult.Path.Any() && pathResult.Path.Count == 2 &&
|
||||
pathResult.Path[0].Id == pathResult.Path[1].Id)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 동일포인트");
|
||||
return DockingValidationResult.CreateNotRequired();
|
||||
}
|
||||
|
||||
// 목적지 노드 가져오기 (Path는 이제 List<MapNode>)
|
||||
var LastNode = pathResult.Path[pathResult.Path.Count - 1];
|
||||
|
||||
if (LastNode == null)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드가 null입니다");
|
||||
return DockingValidationResult.CreateNotRequired();
|
||||
}
|
||||
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {LastNode.Id} 타입:{LastNode.Type} ({(int)LastNode.Type})");
|
||||
|
||||
////detail 경로 이동 예측 검증
|
||||
//for (int i = 0; i < pathResult.DetailedPath.Count - 1; i++)
|
||||
//{
|
||||
// var curNodeId = pathResult.DetailedPath[i].NodeId;
|
||||
// var nextNodeId = pathResult.DetailedPath[i + 1].NodeId;
|
||||
|
||||
// var curNode = mapNodes?.FirstOrDefault(n => n.Id == curNodeId);
|
||||
// var nextNode = mapNodes?.FirstOrDefault(n => n.Id == nextNodeId);
|
||||
|
||||
// if (curNode != null && nextNode != null)
|
||||
// {
|
||||
// MapNode prevNode = null;
|
||||
// AgvDirection prevDir = AgvDirection.Stop;
|
||||
// if (i == 0)
|
||||
// {
|
||||
// prevNode = pathResult.PrevNode;
|
||||
// prevDir = pathResult.PrevDirection;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// var prevNodeId = pathResult.DetailedPath[i - 1].NodeId;
|
||||
// prevNode = mapNodes?.FirstOrDefault(n => n.Id == prevNodeId);
|
||||
// prevDir = pathResult.DetailedPath[i - 1].MotorDirection;
|
||||
// }
|
||||
|
||||
|
||||
// if (prevNode != null)
|
||||
// {
|
||||
// // DirectionalHelper를 사용하여 예상되는 다음 노드 확인
|
||||
// Console.WriteLine(
|
||||
// $"\n[ValidateDockingDirection] 경로 검증 단계 {i}:");
|
||||
// Console.WriteLine(
|
||||
// $" 이전→현재→다음: {prevNode.Id}({prevNode.RfidId}) → {curNode.Id}({curNode.RfidId}) → {nextNode.Id}({nextNode.RfidId})");
|
||||
// Console.WriteLine(
|
||||
// $" 현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})");
|
||||
// Console.WriteLine(
|
||||
// $" 이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}");
|
||||
// Console.WriteLine(
|
||||
// $" 마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}");
|
||||
|
||||
// var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
|
||||
// curNode,
|
||||
// prevNode,
|
||||
// prevDir,
|
||||
// pathResult.DetailedPath[i].MotorDirection,
|
||||
// pathResult.DetailedPath[i].MagnetDirection,
|
||||
// mapNodes
|
||||
// );
|
||||
|
||||
// var expectedNextNodeL = DirectionalHelper.GetNextNodeByDirection(
|
||||
// curNode,
|
||||
// prevNode,
|
||||
// prevDir,
|
||||
// pathResult.DetailedPath[i].MotorDirection,
|
||||
// PathFinding.Planning.MagnetDirection.Left,
|
||||
// mapNodes
|
||||
// );
|
||||
|
||||
// var expectedNextNodeR = DirectionalHelper.GetNextNodeByDirection(
|
||||
// curNode,
|
||||
// prevNode,
|
||||
// prevDir,
|
||||
// pathResult.DetailedPath[i].MotorDirection,
|
||||
// PathFinding.Planning.MagnetDirection.Right,
|
||||
// mapNodes
|
||||
// );
|
||||
|
||||
// var expectedNextNodeS = DirectionalHelper.GetNextNodeByDirection(
|
||||
// curNode,
|
||||
// prevNode,
|
||||
// prevDir,
|
||||
// pathResult.DetailedPath[i].MotorDirection,
|
||||
// PathFinding.Planning.MagnetDirection.Straight,
|
||||
// mapNodes
|
||||
// );
|
||||
|
||||
|
||||
// Console.WriteLine(
|
||||
// $" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.Id ?? "null"}");
|
||||
// Console.WriteLine(
|
||||
// $" [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.Id}]");
|
||||
|
||||
// if (expectedNextNode != null && !expectedNextNode.Id.Equals(nextNode.Id))
|
||||
// {
|
||||
// string error =
|
||||
// $"[DockingValidator] ⚠️ 경로 방향 불일치" +
|
||||
// $"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.Id ?? string.Empty)}] " +
|
||||
// $"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.Id}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
|
||||
// Console.WriteLine(
|
||||
// $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
|
||||
// Console.WriteLine(
|
||||
// $" 이동 벡터:");
|
||||
// Console.WriteLine(
|
||||
// $" 이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})");
|
||||
// Console.WriteLine(
|
||||
// $" 현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})");
|
||||
// Console.WriteLine(
|
||||
// $" 현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})");
|
||||
// Console.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}");
|
||||
// return DockingValidationResult.CreateInvalid(
|
||||
// LastNode.Id,
|
||||
// LastNode.Type,
|
||||
// pathResult.DetailedPath[i].MotorDirection,
|
||||
// pathResult.DetailedPath[i].MotorDirection,
|
||||
// error);
|
||||
|
||||
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// Console.WriteLine(
|
||||
// $" ✅ 경로 방향 일치!");
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
//}
|
||||
|
||||
|
||||
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
|
||||
if (LastNode.DockDirection == DockingDirection.DontCare)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {LastNode.DockDirection}");
|
||||
return DockingValidationResult.CreateNotRequired();
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 필요한 도킹 방향 확인
|
||||
var requiredDirection = GetRequiredDockingDirection(LastNode.DockDirection);
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 필요한 도킹 방향: {requiredDirection}");
|
||||
|
||||
var LastNodeInfo = pathResult.DetailedPath.Last();
|
||||
if (LastNodeInfo.NodeId != LastNode.Id)
|
||||
{
|
||||
string error = $"마지막 노드의 도킹방향과 경로정보의 노드ID 불일치: 필요={LastNode.Id}, 계산됨={LastNodeInfo.NodeId }";
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
|
||||
return DockingValidationResult.CreateInvalid(
|
||||
LastNode.Id,
|
||||
LastNode.Type,
|
||||
requiredDirection,
|
||||
LastNodeInfo.MotorDirection,
|
||||
error);
|
||||
}
|
||||
|
||||
// 검증 수행
|
||||
if (LastNodeInfo.MotorDirection == requiredDirection && pathResult.DetailedPath[pathResult.DetailedPath.Count - 1].MotorDirection == requiredDirection)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ✅ 도킹 검증 성공");
|
||||
return DockingValidationResult.CreateValid(
|
||||
LastNode.Id,
|
||||
LastNode.Type,
|
||||
requiredDirection,
|
||||
LastNodeInfo.MotorDirection);
|
||||
}
|
||||
else
|
||||
{
|
||||
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(LastNodeInfo.MotorDirection)}";
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
|
||||
return DockingValidationResult.CreateInvalid(
|
||||
LastNode.Id,
|
||||
LastNode.Type,
|
||||
requiredDirection,
|
||||
LastNodeInfo.MotorDirection,
|
||||
error);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 도킹이 필요한 노드인지 확인 (도킹방향이 DontCare가 아닌 경우)
|
||||
/// </summary>
|
||||
private static bool IsDockingRequired(DockingDirection dockDirection)
|
||||
{
|
||||
return dockDirection != DockingDirection.DontCare;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
|
||||
/// </summary>
|
||||
private static AgvDirection GetRequiredDockingDirection(DockingDirection dockDirection)
|
||||
{
|
||||
switch (dockDirection)
|
||||
{
|
||||
case DockingDirection.Forward:
|
||||
return AgvDirection.Forward; // 전진 도킹
|
||||
case DockingDirection.Backward:
|
||||
return AgvDirection.Backward; // 후진 도킹
|
||||
case DockingDirection.DontCare:
|
||||
default:
|
||||
return AgvDirection.Forward; // 기본값 (사실상 사용되지 않음)
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 기반 최종 방향 계산
|
||||
/// 개선된 구현: 경로 진행 방향과 목적지 노드 타입을 고려
|
||||
/// </summary>
|
||||
private static AgvDirection CalculateFinalDirection(List<MapNode> path, List<MapNode> mapNodes, AgvDirection currentDirection)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 입력 - 경로 수: {path?.Count}, 현재 방향: {currentDirection}");
|
||||
|
||||
// 경로가 1개 이하면 현재 방향 유지
|
||||
if (path.Count < 2)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 경로가 짧음, 현재 방향 유지: {currentDirection}");
|
||||
return currentDirection;
|
||||
}
|
||||
|
||||
// 목적지 노드 확인 (Path는 이제 List<MapNode>)
|
||||
var lastNode = path[path.Count - 1];
|
||||
|
||||
if (lastNode == null)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 목적지 노드가 null입니다");
|
||||
return currentDirection;
|
||||
}
|
||||
|
||||
// 도킹 노드인 경우, 필요한 도킹 방향으로 설정
|
||||
if (IsDockingRequired(lastNode.DockDirection))
|
||||
{
|
||||
var requiredDockingDirection = GetRequiredDockingDirection(lastNode.DockDirection);
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 도킹 노드(DockDirection={lastNode.DockDirection}) 감지, 필요 방향: {requiredDockingDirection}");
|
||||
|
||||
// 현재 방향이 필요한 도킹 방향과 다르면 경고 로그
|
||||
if (currentDirection != requiredDockingDirection)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] ⚠️ 현재 방향({currentDirection})과 필요 도킹 방향({requiredDockingDirection}) 불일치");
|
||||
}
|
||||
|
||||
// 도킹 노드의 경우 항상 필요한 도킹 방향 반환
|
||||
return requiredDockingDirection;
|
||||
}
|
||||
|
||||
// 일반 노드인 경우 마지막 구간의 이동 방향 분석
|
||||
var secondLastNode = path[path.Count - 2];
|
||||
|
||||
if (secondLastNode == null)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이전 노드가 null입니다");
|
||||
return currentDirection;
|
||||
}
|
||||
|
||||
// 마지막 구간의 이동 벡터 계산
|
||||
var deltaX = lastNode.Position.X - secondLastNode.Position.X;
|
||||
var deltaY = lastNode.Position.Y - secondLastNode.Position.Y;
|
||||
var distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 마지막 구간: {secondLastNode.Id} → {lastNode.Id}, 벡터: ({deltaX}, {deltaY}), 거리: {distance:F2}");
|
||||
|
||||
// 이동 거리가 매우 작으면 현재 방향 유지
|
||||
if (distance < 1.0)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이동 거리 너무 짧음, 현재 방향 유지: {currentDirection}");
|
||||
return currentDirection;
|
||||
}
|
||||
|
||||
// 일반 노드의 경우 현재 방향 유지 (방향 전환은 회전 노드에서만 발생)
|
||||
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 일반 노드, 현재 방향 유지: {currentDirection}");
|
||||
return currentDirection;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향을 텍스트로 변환
|
||||
/// </summary>
|
||||
private static string GetDirectionText(AgvDirection direction)
|
||||
{
|
||||
switch (direction)
|
||||
{
|
||||
case AgvDirection.Forward:
|
||||
return "전진";
|
||||
case AgvDirection.Backward:
|
||||
return "후진";
|
||||
default:
|
||||
return direction.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 검증 결과를 문자열로 변환 (디버깅용)
|
||||
/// </summary>
|
||||
public static string GetValidationSummary(DockingValidationResult validation)
|
||||
{
|
||||
if (validation == null)
|
||||
return "검증 결과 없음";
|
||||
|
||||
if (!validation.IsValidationRequired)
|
||||
return "도킹 검증 불필요";
|
||||
|
||||
if (validation.IsValid)
|
||||
{
|
||||
return $"도킹 검증 통과: {validation.TargetNodeId}({validation.TargetNodeType}) - {GetDirectionText(validation.RequiredDockingDirection)} 도킹";
|
||||
}
|
||||
else
|
||||
{
|
||||
return $"도킹 검증 실패: {validation.TargetNodeId}({validation.TargetNodeType}) - {validation.ValidationError}";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
153
AGVLogic/AGVNavigationCore/Utils/ImageConverterUtil.cs
Normal file
153
AGVLogic/AGVNavigationCore/Utils/ImageConverterUtil.cs
Normal file
@@ -0,0 +1,153 @@
|
||||
using System;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Imaging;
|
||||
using System.IO;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// 이미지와 문자열 간 변환을 위한 유틸리티 클래스
|
||||
/// Base64 인코딩을 사용하여 이미지를 문자열로 변환하거나 그 반대로 수행
|
||||
/// </summary>
|
||||
public static class ImageConverterUtil
|
||||
{
|
||||
/// <summary>
|
||||
/// Image 객체를 Base64 문자열로 변환
|
||||
/// </summary>
|
||||
/// <param name="image">변환할 이미지</param>
|
||||
/// <param name="format">이미지 포맷 (기본값: PNG)</param>
|
||||
/// <returns>Base64 인코딩된 문자열, null인 경우 빈 문자열 반환</returns>
|
||||
public static string ImageToBase64(Image image, ImageFormat format = null)
|
||||
{
|
||||
if (image == null)
|
||||
return string.Empty;
|
||||
|
||||
try
|
||||
{
|
||||
format = format ?? ImageFormat.Png;
|
||||
using (var memoryStream = new MemoryStream())
|
||||
{
|
||||
image.Save(memoryStream, format);
|
||||
byte[] imageBytes = memoryStream.ToArray();
|
||||
return Convert.ToBase64String(imageBytes);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"이미지 변환 실패: {ex.Message}");
|
||||
return string.Empty;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 파일 경로의 이미지를 Base64 문자열로 변환
|
||||
/// </summary>
|
||||
/// <param name="filePath">이미지 파일 경로</param>
|
||||
/// <param name="format">변환할 포맷 (기본값: PNG, 원본 포맷 유지하려면 null)</param>
|
||||
/// <returns>Base64 인코딩된 문자열</returns>
|
||||
public static string FileToBase64(string filePath, ImageFormat format = null)
|
||||
{
|
||||
if (string.IsNullOrEmpty(filePath) || !File.Exists(filePath))
|
||||
return string.Empty;
|
||||
|
||||
try
|
||||
{
|
||||
using (var image = Image.FromFile(filePath))
|
||||
{
|
||||
return ImageToBase64(image, format ?? ImageFormat.Png);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"파일 변환 실패: {ex.Message}");
|
||||
return string.Empty;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Base64 문자열을 Image 객체로 변환
|
||||
/// </summary>
|
||||
/// <param name="base64String">Base64 인코딩된 문자열</param>
|
||||
/// <returns>변환된 Image 객체, 실패 시 null</returns>
|
||||
public static Image Base64ToImage(string base64String)
|
||||
{
|
||||
if (string.IsNullOrEmpty(base64String))
|
||||
return null;
|
||||
|
||||
try
|
||||
{
|
||||
byte[] imageBytes = Convert.FromBase64String(base64String);
|
||||
using (var memoryStream = new MemoryStream(imageBytes))
|
||||
{
|
||||
return Image.FromStream(memoryStream);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"Base64 이미지 변환 실패: {ex.Message}");
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Base64 문자열을 Bitmap 객체로 변환
|
||||
/// Image 대신 Bitmap을 반환하므로 메모리 관리가 더 안정적
|
||||
/// </summary>
|
||||
/// <param name="base64String">Base64 인코딩된 문자열</param>
|
||||
/// <returns>변환된 Bitmap 객체, 실패 시 null</returns>
|
||||
public static Bitmap Base64ToBitmap(string base64String)
|
||||
{
|
||||
if (string.IsNullOrEmpty(base64String))
|
||||
return null;
|
||||
|
||||
try
|
||||
{
|
||||
byte[] imageBytes = Convert.FromBase64String(base64String);
|
||||
using (var memoryStream = new MemoryStream(imageBytes))
|
||||
{
|
||||
return new Bitmap(memoryStream);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"Base64 Bitmap 변환 실패: {ex.Message}");
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Base64 문자열이 유효한지 확인
|
||||
/// </summary>
|
||||
/// <param name="base64String">검증할 Base64 문자열</param>
|
||||
/// <returns>유효하면 true, 그 외 false</returns>
|
||||
public static bool IsValidBase64(string base64String)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(base64String))
|
||||
return false;
|
||||
|
||||
try
|
||||
{
|
||||
Convert.FromBase64String(base64String);
|
||||
return true;
|
||||
}
|
||||
catch
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Base64 이미지 데이터의 크기를 대략적으로 계산 (바이트 단위)
|
||||
/// </summary>
|
||||
/// <param name="base64String">Base64 문자열</param>
|
||||
/// <returns>예상 바이트 크기</returns>
|
||||
public static long GetApproximateSize(string base64String)
|
||||
{
|
||||
if (string.IsNullOrEmpty(base64String))
|
||||
return 0;
|
||||
|
||||
// Base64는 원본 데이터보다 약 33% 더 큼
|
||||
return (long)(base64String.Length * 0.75);
|
||||
}
|
||||
}
|
||||
}
|
||||
281
AGVLogic/AGVNavigationCore/Utils/LiftCalculator.cs
Normal file
281
AGVLogic/AGVNavigationCore/Utils/LiftCalculator.cs
Normal file
@@ -0,0 +1,281 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 리프트 방향 계산 유틸리티 클래스
|
||||
/// 모든 리프트 방향 계산 로직을 중앙화하여 일관성 보장
|
||||
/// </summary>
|
||||
public static class LiftCalculator
|
||||
{
|
||||
|
||||
/// <summary>
|
||||
/// 경로 예측 기반 리프트 방향 계산
|
||||
/// 현재 노드에서 연결된 다음 노드들을 분석하여 리프트 방향 결정
|
||||
/// </summary>
|
||||
/// <param name="currentPos">현재 위치</param>
|
||||
/// <param name="previousPos">이전 위치</param>
|
||||
/// <param name="motorDirection">모터 방향</param>
|
||||
/// <param name="mapNodes">맵 노드 리스트 (경로 예측용)</param>
|
||||
/// <param name="tolerance">위치 허용 오차</param>
|
||||
/// <returns>리프트 계산 결과</returns>
|
||||
public static LiftCalculationResult CalculateLiftInfoWithPathPrediction(
|
||||
Point currentPos, Point previousPos, AgvDirection motorDirection,
|
||||
List<MapNode> mapNodes, int tolerance = 10)
|
||||
{
|
||||
if (mapNodes == null || mapNodes.Count == 0)
|
||||
{
|
||||
// 맵 노드 정보가 없으면 기존 방식 사용
|
||||
return CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
||||
}
|
||||
|
||||
// 현재 위치에 해당하는 노드 찾기
|
||||
var currentNode = FindNodeByPosition(mapNodes, currentPos, tolerance);
|
||||
|
||||
if (currentNode == null)
|
||||
{
|
||||
// 현재 노드를 찾을 수 없으면 기존 방식 사용
|
||||
return CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
||||
}
|
||||
|
||||
// 이전 위치에 해당하는 노드 찾기
|
||||
var previousNode = FindNodeByPosition(mapNodes, previousPos, tolerance);
|
||||
|
||||
Point targetPosition;
|
||||
string calculationMethod;
|
||||
|
||||
// 모터 방향에 따른 예측 방향 결정
|
||||
if (motorDirection == AgvDirection.Backward)
|
||||
{
|
||||
// 후진 모터: AGV가 리프트 쪽(목표 위치)으로 이동
|
||||
// 경로 예측 없이 단순히 현재→목표 방향 사용
|
||||
return CalculateLiftInfo(currentPos, previousPos, motorDirection);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 전진 모터: 기존 로직 (다음 노드 예측)
|
||||
var nextNodes = GetConnectedNodes(mapNodes, currentNode);
|
||||
|
||||
// 이전 노드 제외 (되돌아가는 방향 제외)
|
||||
if (previousNode != null)
|
||||
{
|
||||
nextNodes = nextNodes.Where(n => n.Id != previousNode.Id).ToList();
|
||||
}
|
||||
|
||||
if (nextNodes.Count == 1)
|
||||
{
|
||||
// 직선 경로: 다음 노드 방향으로 예측
|
||||
targetPosition = nextNodes.First().Position;
|
||||
calculationMethod = $"전진 경로 예측 ({currentNode.Id}→{nextNodes.First().Id})";
|
||||
}
|
||||
else if (nextNodes.Count > 1)
|
||||
{
|
||||
// 갈래길: 이전 위치 기반 계산 사용
|
||||
var prevResult = CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
||||
prevResult.CalculationMethod += " (전진 갈래길)";
|
||||
return prevResult;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 연결된 노드가 없으면 기존 방식 사용
|
||||
return CalculateLiftInfo(previousPos, currentPos, motorDirection);
|
||||
}
|
||||
}
|
||||
|
||||
// 리프트 각도 계산
|
||||
var angleRadians = CalculateLiftAngleRadians(currentPos, targetPosition, motorDirection);
|
||||
var angleDegrees = angleRadians * 180.0 / Math.PI;
|
||||
|
||||
// 0-360도 범위로 정규화
|
||||
while (angleDegrees < 0) angleDegrees += 360;
|
||||
while (angleDegrees >= 360) angleDegrees -= 360;
|
||||
|
||||
var directionString = AngleToDirectionString(angleDegrees);
|
||||
|
||||
return new LiftCalculationResult
|
||||
{
|
||||
AngleRadians = angleRadians,
|
||||
AngleDegrees = angleDegrees,
|
||||
DirectionString = directionString,
|
||||
CalculationMethod = calculationMethod,
|
||||
MotorDirection = motorDirection
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 이동 방향과 모터 방향을 기반으로 리프트 각도 계산
|
||||
/// </summary>
|
||||
/// <param name="currentPos">현재 위치</param>
|
||||
/// <param name="targetPos">목표 위치</param>
|
||||
/// <param name="motorDirection">모터 방향</param>
|
||||
/// <returns>리프트 각도 (라디안)</returns>
|
||||
public static double CalculateLiftAngleRadians(Point currentPos, Point targetPos, AgvDirection motorDirection)
|
||||
{
|
||||
// 모터 방향에 따른 리프트 위치 계산
|
||||
if (motorDirection == AgvDirection.Forward)
|
||||
{
|
||||
// 전진 모터: AGV가 앞으로 가므로 리프트는 뒤쪽 (타겟 → 현재 방향)
|
||||
var dx = currentPos.X - targetPos.X;
|
||||
var dy = currentPos.Y - targetPos.Y;
|
||||
return Math.Atan2(dy, dx);
|
||||
}
|
||||
else if (motorDirection == AgvDirection.Backward)
|
||||
{
|
||||
// 후진 모터: AGV가 리프트 쪽으로 이동하므로 리프트는 AGV 이동 방향에 위치
|
||||
// 007→006 후진시: 리프트는 006방향(이동방향)을 향해야 함 (타겟→현재 반대방향)
|
||||
var dx = currentPos.X - targetPos.X;
|
||||
var dy = currentPos.Y - targetPos.Y;
|
||||
return Math.Atan2(dy, dx);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 기본값: 전진 모터와 동일
|
||||
var dx = currentPos.X - targetPos.X;
|
||||
var dy = currentPos.Y - targetPos.Y;
|
||||
return Math.Atan2(dy, dx);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 이동 방향과 모터 방향을 기반으로 리프트 각도 계산 (도 단위)
|
||||
/// </summary>
|
||||
/// <param name="currentPos">현재 위치</param>
|
||||
/// <param name="targetPos">목표 위치</param>
|
||||
/// <param name="motorDirection">모터 방향</param>
|
||||
/// <returns>리프트 각도 (도)</returns>
|
||||
public static double CalculateLiftAngleDegrees(Point currentPos, Point targetPos, AgvDirection motorDirection)
|
||||
{
|
||||
var radians = CalculateLiftAngleRadians(currentPos, targetPos, motorDirection);
|
||||
var degrees = radians * 180.0 / Math.PI;
|
||||
|
||||
// 0-360도 범위로 정규화
|
||||
while (degrees < 0) degrees += 360;
|
||||
while (degrees >= 360) degrees -= 360;
|
||||
|
||||
return degrees;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 각도를 8방향 문자열로 변환 (화면 좌표계 기준)
|
||||
/// 화면 좌표계: 0°=동쪽, 90°=남쪽, 180°=서쪽, 270°=북쪽
|
||||
/// </summary>
|
||||
/// <param name="angleDegrees">각도 (도)</param>
|
||||
/// <returns>방향 문자열</returns>
|
||||
public static string AngleToDirectionString(double angleDegrees)
|
||||
{
|
||||
// 0-360도 범위로 정규화
|
||||
while (angleDegrees < 0) angleDegrees += 360;
|
||||
while (angleDegrees >= 360) angleDegrees -= 360;
|
||||
|
||||
// 8방향으로 분류 (화면 좌표계)
|
||||
if (angleDegrees >= 337.5 || angleDegrees < 22.5)
|
||||
return "동쪽(→)";
|
||||
else if (angleDegrees >= 22.5 && angleDegrees < 67.5)
|
||||
return "남동쪽(↘)";
|
||||
else if (angleDegrees >= 67.5 && angleDegrees < 112.5)
|
||||
return "남쪽(↓)";
|
||||
else if (angleDegrees >= 112.5 && angleDegrees < 157.5)
|
||||
return "남서쪽(↙)";
|
||||
else if (angleDegrees >= 157.5 && angleDegrees < 202.5)
|
||||
return "서쪽(←)";
|
||||
else if (angleDegrees >= 202.5 && angleDegrees < 247.5)
|
||||
return "북서쪽(↖)";
|
||||
else if (angleDegrees >= 247.5 && angleDegrees < 292.5)
|
||||
return "북쪽(↑)";
|
||||
else if (angleDegrees >= 292.5 && angleDegrees < 337.5)
|
||||
return "북동쪽(↗)";
|
||||
else
|
||||
return "알 수 없음";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 리프트 계산 결과 정보
|
||||
/// </summary>
|
||||
public class LiftCalculationResult
|
||||
{
|
||||
public double AngleRadians { get; set; }
|
||||
public double AngleDegrees { get; set; }
|
||||
public string DirectionString { get; set; }
|
||||
public string CalculationMethod { get; set; }
|
||||
public AgvDirection MotorDirection { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 종합적인 리프트 계산 (모든 정보 포함)
|
||||
/// </summary>
|
||||
/// <param name="currentPos">현재 위치</param>
|
||||
/// <param name="targetPos">목표 위치</param>
|
||||
/// <param name="motorDirection">모터 방향</param>
|
||||
/// <returns>리프트 계산 결과</returns>
|
||||
public static LiftCalculationResult CalculateLiftInfo(Point currentPos, Point targetPos, AgvDirection motorDirection)
|
||||
{
|
||||
var angleRadians = CalculateLiftAngleRadians(currentPos, targetPos, motorDirection);
|
||||
var angleDegrees = angleRadians * 180.0 / Math.PI;
|
||||
|
||||
// 0-360도 범위로 정규화
|
||||
while (angleDegrees < 0) angleDegrees += 360;
|
||||
while (angleDegrees >= 360) angleDegrees -= 360;
|
||||
|
||||
var directionString = AngleToDirectionString(angleDegrees);
|
||||
|
||||
string calculationMethod;
|
||||
if (motorDirection == AgvDirection.Forward)
|
||||
calculationMethod = "이동방향 + 180도 (전진모터)";
|
||||
else if (motorDirection == AgvDirection.Backward)
|
||||
calculationMethod = "이동방향과 동일 (후진모터 - 리프트는 이동방향에 위치)";
|
||||
else
|
||||
calculationMethod = "기본값 (전진모터)";
|
||||
|
||||
return new LiftCalculationResult
|
||||
{
|
||||
AngleRadians = angleRadians,
|
||||
AngleDegrees = angleDegrees,
|
||||
DirectionString = directionString,
|
||||
CalculationMethod = calculationMethod,
|
||||
MotorDirection = motorDirection
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 위치 기반 노드 찾기
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 리스트</param>
|
||||
/// <param name="position">찾을 위치</param>
|
||||
/// <param name="tolerance">허용 오차</param>
|
||||
/// <returns>해당하는 노드 또는 null</returns>
|
||||
private static MapNode FindNodeByPosition(List<MapNode> mapNodes, Point position, int tolerance)
|
||||
{
|
||||
return mapNodes.FirstOrDefault(node =>
|
||||
Math.Abs(node.Position.X - position.X) <= tolerance &&
|
||||
Math.Abs(node.Position.Y - position.Y) <= tolerance);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드에서 연결된 다른 노드들 찾기
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 리스트</param>
|
||||
/// <param name="currentNode">현재 노드</param>
|
||||
/// <returns>연결된 노드 리스트</returns>
|
||||
private static List<MapNode> GetConnectedNodes(List<MapNode> mapNodes, MapNode currentNode)
|
||||
{
|
||||
var connectedNodes = new List<MapNode>();
|
||||
|
||||
foreach (var nodeId in currentNode.ConnectedNodes)
|
||||
{
|
||||
var connectedNode = mapNodes.FirstOrDefault(n => n.Id == nodeId);
|
||||
if (connectedNode != null)
|
||||
{
|
||||
connectedNodes.Add(connectedNode);
|
||||
}
|
||||
}
|
||||
|
||||
return connectedNodes;
|
||||
}
|
||||
}
|
||||
}
|
||||
56
AGVLogic/AGVNavigationCore/Utils/TestRunner.cs
Normal file
56
AGVLogic/AGVNavigationCore/Utils/TestRunner.cs
Normal file
@@ -0,0 +1,56 @@
|
||||
using System;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.Utils
|
||||
{
|
||||
/// <summary>
|
||||
/// DirectionalPathfinder 테스트 실행 프로그램
|
||||
///
|
||||
/// 사용법:
|
||||
/// var runner = new TestRunner();
|
||||
/// runner.RunTests();
|
||||
/// </summary>
|
||||
public class TestRunner
|
||||
{
|
||||
public void RunTests()
|
||||
{
|
||||
string mapFilePath = @"C:\Data\Source\(5613#) ENIG AGV\Source\Cs_HMI\Data\NewMap.json";
|
||||
|
||||
var tester = new DirectionalPathfinderTest();
|
||||
|
||||
// 맵 파일 로드
|
||||
if (!tester.LoadMapFile(mapFilePath))
|
||||
{
|
||||
Console.WriteLine("맵 파일 로드 실패!");
|
||||
return;
|
||||
}
|
||||
|
||||
// 모든 노드 정보 출력
|
||||
tester.PrintAllNodes();
|
||||
|
||||
// 테스트 시나리오 1: 001 → 002 → Forward (003 기대)
|
||||
tester.PrintNodeInfo(001);
|
||||
tester.PrintNodeInfo(002);
|
||||
tester.TestDirectionalMovement(001, 002, AgvDirection.Forward);
|
||||
|
||||
// 테스트 시나리오 2: 002 → 001 → Backward (000 또는 이전 기대)
|
||||
tester.TestDirectionalMovement(002, 001, AgvDirection.Backward);
|
||||
|
||||
// 테스트 시나리오 3: 002 → 003 → Forward
|
||||
tester.PrintNodeInfo(003);
|
||||
tester.TestDirectionalMovement(002, 003, AgvDirection.Forward);
|
||||
|
||||
// 테스트 시나리오 4: 003 → 004 → Forward
|
||||
tester.PrintNodeInfo(004);
|
||||
tester.TestDirectionalMovement(003, 004, AgvDirection.Forward);
|
||||
|
||||
// 테스트 시나리오 5: 003 → 004 → Right (030 기대)
|
||||
tester.TestDirectionalMovement(003, 004, AgvDirection.Right);
|
||||
|
||||
// 테스트 시나리오 6: 004 → 003 → Backward
|
||||
tester.TestDirectionalMovement(004, 003, AgvDirection.Backward);
|
||||
|
||||
Console.WriteLine("\n\n=== 테스트 완료 ===");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user