경로예측로직 수정 중
This commit is contained in:
backuppc
@@ -122,6 +122,7 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
//1.목적지까지의 최단거리 경로를 찾는다.
|
||||
var pathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
pathResult.PrevNode = prevNode;
|
||||
pathResult.PrevDirection = prevDirection;
|
||||
if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
|
||||
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
|
||||
|
||||
@@ -198,6 +199,7 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
//1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
|
||||
var path1 = _basicPathfinder.FindPath(startNode.NodeId, JunctionInPath.NodeId);
|
||||
path1.PrevNode = prevNode;
|
||||
path1.PrevDirection = prevDirection;
|
||||
|
||||
//다음좌표를 보고 정방향인지 역방향인지 체크한다.
|
||||
bool ReverseCheck = false;
|
||||
@@ -220,6 +222,8 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
//2.교차로 - 종료위치
|
||||
var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId);
|
||||
path2.PrevNode = prevNode;
|
||||
path2.PrevDirection = prevDirection;
|
||||
|
||||
if (ReverseCheck) MakeDetailData(path2, currentDirection);
|
||||
else MakeDetailData(path2, ReverseDirection);
|
||||
|
||||
|
||||
@@ -91,9 +91,9 @@ namespace AGVNavigationCore.Utils
|
||||
Console.WriteLine(
|
||||
$"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 ==========");
|
||||
Console.WriteLine(
|
||||
$" 현재노드: {currentNode.NodeId}({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
|
||||
$" 현재노드: {currentNode.RfidId}[{currentNode.NodeId}]({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
|
||||
Console.WriteLine(
|
||||
$" 이전노드: {prevNode.NodeId}({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
|
||||
$" 이전노드: {prevNode.RfidId}[{prevNode.NodeId}]({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
|
||||
Console.WriteLine(
|
||||
$" 이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})");
|
||||
Console.WriteLine(
|
||||
@@ -138,7 +138,7 @@ namespace AGVNavigationCore.Utils
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"\n [후보] {candidate.NodeId}({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
|
||||
$"\n [후보] {candidate.RfidId}[{candidate.NodeId}]({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
|
||||
Console.WriteLine(
|
||||
$" 벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}");
|
||||
Console.WriteLine(
|
||||
@@ -167,7 +167,8 @@ namespace AGVNavigationCore.Utils
|
||||
normalizedMovement,
|
||||
normalizedToNext,
|
||||
currentNode,
|
||||
candidate
|
||||
candidate,
|
||||
direction
|
||||
);
|
||||
Console.WriteLine(
|
||||
$" 마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}");
|
||||
@@ -182,7 +183,7 @@ namespace AGVNavigationCore.Utils
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"\n 최종선택: {bestNode?.NodeId ?? "null"} (점수: {bestScore:F4})");
|
||||
$"\n 최종선택: {bestNode?.RfidId ?? "null"}[{bestNode?.NodeId ?? "null"}] (점수: {bestScore:F4})");
|
||||
Console.WriteLine(
|
||||
$"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n");
|
||||
|
||||
@@ -249,7 +250,8 @@ namespace AGVNavigationCore.Utils
|
||||
PointF normalizedMovement,
|
||||
PointF normalizedToNext,
|
||||
MapNode currentNode,
|
||||
MapNode candidate)
|
||||
MapNode candidate,
|
||||
AgvDirection direction)
|
||||
{
|
||||
float adjustedScore = baseScore;
|
||||
|
||||
@@ -262,38 +264,73 @@ namespace AGVNavigationCore.Utils
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}");
|
||||
}
|
||||
// Left 또는 Right: 회전 방향 판단을 위해 외적 사용
|
||||
// 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 isLeftTurn = crossProduct > 0;
|
||||
bool isRightTurn = crossProduct < 0;
|
||||
bool isLeftMotorMatch = false;
|
||||
bool isRightMotorMatch = false;
|
||||
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] 외적(Cross): {crossProduct:F4}, 좌회전: {isLeftTurn}, 우회전: {isRightTurn}");
|
||||
|
||||
if ((magnetDirection == MagnetDirection.Left && isLeftTurn) ||
|
||||
(magnetDirection == MagnetDirection.Right && isRightTurn))
|
||||
// ===== 정방향(Forward) 이동 =====
|
||||
if (direction == AgvDirection.Forward)
|
||||
{
|
||||
// 올바른 회전 방향: 강한 보너스
|
||||
const float CORRECT_TURN_BONUS = 1.0f;
|
||||
adjustedScore += CORRECT_TURN_BONUS;
|
||||
// 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(
|
||||
$" [마그넷 판정] ✅ {magnetDirection} 방향 일치 (보너스 +1.0): {baseScore:F4} → {adjustedScore:F4}");
|
||||
$" [마그넷 판정] 외적(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
|
||||
{
|
||||
// 잘못된 회전 방향: 강한 페널티
|
||||
const float WRONG_TURN_PENALTY = 0.8f;
|
||||
adjustedScore -= WRONG_TURN_PENALTY;
|
||||
// 잘못된 모터 방향: 회전 강도에 비례한 페널티
|
||||
// 강한 회전(|외적| ≈ 1): -2.0
|
||||
// 약한 회전(|외적| ≈ 0.2): -0.4
|
||||
float magnetPenalty = rotationStrength * 2.0f;
|
||||
adjustedScore -= magnetPenalty;
|
||||
|
||||
string actualMotor = crossProduct > 0 ? "Left" : "Right";
|
||||
|
||||
Console.WriteLine(
|
||||
$" [마그넷 판정] ❌ {magnetDirection} 방향 불일치 (실제: {(isLeftTurn ? "Left" : "Right")}, 페널티 -0.8): {baseScore:F4} → {adjustedScore:F4}");
|
||||
$" [마그넷 판정] ❌ {magnetDirection} 모터 불일치 (실제: {actualMotor}, 회전강도: {rotationStrength:F4}, 페널티 -{magnetPenalty:F4}): {baseScore:F4} → {adjustedScore:F4}");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -92,14 +92,14 @@ namespace AGVNavigationCore.Utils
|
||||
Console.WriteLine(
|
||||
$" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.NodeId ?? "null"}");
|
||||
Console.WriteLine(
|
||||
$" [실제] DetailedPath 다음 노드: {nextNode.NodeId}");
|
||||
$" [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.NodeId}]");
|
||||
|
||||
if (expectedNextNode != null && !expectedNextNode.NodeId.Equals(nextNode.NodeId))
|
||||
{
|
||||
string error =
|
||||
$"[DockingValidator] ⚠️ 경로 방향 불일치: " +
|
||||
$"현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " +
|
||||
$"예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
|
||||
$"[DockingValidator] ⚠️ 경로 방향 불일치" +
|
||||
$"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.NodeId ?? string.Empty)}] " +
|
||||
$"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.NodeId}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
|
||||
Console.WriteLine(
|
||||
$"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
|
||||
Console.WriteLine(
|
||||
|
||||
Reference in New Issue
Block a user