feat: Add magnet direction support to GetNextNodeByDirection function
- Added MagnetDirection parameter to GetNextNodeByDirection() function in DirectionalHelper - Implemented ApplyMagnetDirectionBonus() helper method using vector cross product for turn direction detection - Straight magnet direction: +0.3f bonus for consistent forward direction - Left/Right magnet direction: +0.25f bonus for correct turn, -0.2f penalty for wrong turn direction - Updated DockingValidator to pass actual magnet direction from DetailedPath - Updated AGVPathfinder calls to use MagnetDirection.Straight as default during path planning phase - Includes debug output for scoring decisions with magnet direction analysis 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
@@ -126,8 +126,9 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
|
||||
|
||||
//정방향/역방향 이동 시 다음 노드 확인
|
||||
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, _mapNodes);
|
||||
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, _mapNodes);
|
||||
// 경로 계획 단계에서는 마그넷 방향이 미리 알려지지 않으므로 Straight로 기본값 사용
|
||||
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, currentDirection, MagnetDirection.Straight, _mapNodes);
|
||||
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(startNode, prevNode, prevDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes);
|
||||
|
||||
|
||||
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
|
||||
|
||||
@@ -3,37 +3,56 @@ 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
|
||||
{
|
||||
|
||||
private static JunctionAnalyzer _junctionAnalyzer;
|
||||
|
||||
/// <summary>
|
||||
/// 현재 노드에서 주어진 방향(Forward/Backward)으로 이동할 때 다음 노드를 반환 (개선된 버전)
|
||||
/// 이전 모터 방향 정보를 고려하여 더 정확한 경로 예측
|
||||
/// 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="direction">이동 방향 (Forward 또는 Backward)</param>
|
||||
/// <param name="prevDirection">이전 구간의 모터 방향</param>
|
||||
/// <param name="direction">현재 모터 방향 (Forward 또는 Backward)</param>
|
||||
/// <param name="magnetDirection">현재 마그넷 방향 (Straight/Left/Right)</param>
|
||||
/// <param name="allNodes">모든 맵 노드</param>
|
||||
/// <param name="prevMotorDirection">이전 구간에서의 모터 방향 (선택사항)</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 connectedNodeIds = currentNode.ConnectedNodes;
|
||||
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
|
||||
@@ -113,6 +132,16 @@ namespace AGVNavigationCore.Utils
|
||||
dotProduct
|
||||
);
|
||||
|
||||
// 마그넷 방향을 고려한 점수 조정
|
||||
score = ApplyMagnetDirectionBonus(
|
||||
score,
|
||||
magnetDirection,
|
||||
normalizedMovement,
|
||||
normalizedToNext,
|
||||
currentNode,
|
||||
candidate
|
||||
);
|
||||
|
||||
if (score > bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
@@ -166,6 +195,71 @@ namespace AGVNavigationCore.Utils
|
||||
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)
|
||||
{
|
||||
float adjustedScore = baseScore;
|
||||
|
||||
// Straight: 일직선 방향 (높은 내적 보너스)
|
||||
if (magnetDirection == MagnetDirection.Straight)
|
||||
{
|
||||
const float STRAIGHT_BONUS = 0.3f;
|
||||
adjustedScore += STRAIGHT_BONUS;
|
||||
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"[DirectionalHelper] 마그넷 Straight 보너스: {baseScore:F3} → {adjustedScore:F3}");
|
||||
}
|
||||
// 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;
|
||||
|
||||
if ((magnetDirection == MagnetDirection.Left && isLeftTurn) ||
|
||||
(magnetDirection == MagnetDirection.Right && isRightTurn))
|
||||
{
|
||||
// 올바른 회전 방향: 보너스
|
||||
const float CORRECT_TURN_BONUS = 0.25f;
|
||||
adjustedScore += CORRECT_TURN_BONUS;
|
||||
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"[DirectionalHelper] 마그넷 {magnetDirection} 회전 보너스: {baseScore:F3} → {adjustedScore:F3}");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 잘못된 회전 방향: 페널티
|
||||
const float WRONG_TURN_PENALTY = 0.2f;
|
||||
adjustedScore -= WRONG_TURN_PENALTY;
|
||||
|
||||
System.Diagnostics.Debug.WriteLine(
|
||||
$"[DirectionalHelper] 마그넷 {magnetDirection} 회전 페널티 (실제: {(isLeftTurn ? "Left" : "Right")}): {baseScore:F3} → {adjustedScore:F3}");
|
||||
}
|
||||
}
|
||||
|
||||
return adjustedScore;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모터 방향을 고려한 다음 노드 선택 (디버깅/분석용)
|
||||
/// </summary>
|
||||
|
||||
@@ -74,6 +74,7 @@ namespace AGVNavigationCore.Utils
|
||||
prevNode,
|
||||
prevDir,
|
||||
pathResult.DetailedPath[i].MotorDirection,
|
||||
pathResult.DetailedPath[i].MagnetDirection,
|
||||
mapNodes
|
||||
);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user