feat: Add next node direction detection and path visualization improvements
Path Direction Detection (AGVPathfinder.cs): - Added GetNextNodeByDirection() method to determine next node based on Forward/Backward direction - Implements vector-based angle calculation for intelligent node selection - Forward: selects node in continuous direction - Backward: selects node in opposite direction - Validates if selected direction matches path requirements Logic additions at line 150-167: - Detects next node for Forward and Backward directions - Checks if backward movement aligns with path's next node - Returns path with appropriate motor direction (ReverseDirection when applicable) Improved Path Visualization (UnifiedAGVCanvas.Events.cs): - Refined equilateral triangle arrows (8 pixels, symmetric) - 50% transparency for purple path lines with 2x thickness - Bidirectional path detection (darker color for repeated segments) - Better visual distinction for calculated paths Technical Details: - Added System.Drawing using statement for PointF operations - Added DirectionalPathfinder initialization - Vector normalization for angle-based decisions - Dot product calculation for direction similarity scoring Result: AGV can now intelligently select next node based on current movement direction and validate path feasibility 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.Utils;
|
||||
@@ -17,6 +18,7 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
|
||||
private readonly List<MapNode> _mapNodes;
|
||||
private readonly AStarPathfinder _basicPathfinder;
|
||||
private readonly DirectionalPathfinder _directionPathfinder;
|
||||
private readonly JunctionAnalyzer _junctionAnalyzer;
|
||||
private readonly DirectionChangePlanner _directionChangePlanner;
|
||||
|
||||
@@ -27,6 +29,7 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
_basicPathfinder.SetMapNodes(_mapNodes);
|
||||
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
|
||||
_directionChangePlanner = new DirectionChangePlanner(_mapNodes);
|
||||
_directionPathfinder = new DirectionalPathfinder();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -132,6 +135,37 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
return pathResult;
|
||||
}
|
||||
|
||||
//2-1 현재위치의 반대방향과 대상의 방향이 맞는 경우에도 그대로 사용가능하다.
|
||||
//if (targetNode.DockDirection == DockingDirection.DontCare ||
|
||||
// (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Backward) ||
|
||||
// (targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Forward))
|
||||
//{
|
||||
// // 반대 방향으로 이동하여 목적지에 진입해야 함
|
||||
// // 현재 방향으로 가면서 역방향으로 도킹
|
||||
// MakeDetailData(pathResult, ReverseDirection);
|
||||
// MakeMagnetDirection(pathResult);
|
||||
// return pathResult;
|
||||
//}
|
||||
|
||||
//2-2 정방향/역방향 이동 시 다음 노드 확인
|
||||
var nextNodeForward = GetNextNodeByDirection(startNode, prevNode, currentDirection, _mapNodes);
|
||||
var nextNodeBackward = GetNextNodeByDirection(startNode, prevNode, ReverseDirection, _mapNodes);
|
||||
|
||||
//뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다.
|
||||
if (nextNodeBackward.NodeId == pathResult.Path[1])
|
||||
{
|
||||
MakeDetailData(pathResult, ReverseDirection);
|
||||
MakeMagnetDirection(pathResult);
|
||||
return pathResult;
|
||||
}
|
||||
|
||||
//if(nextNodeForward.NodeId == pathResult.Path[1])
|
||||
//{
|
||||
// MakeDetailData(pathResult, currentDirection);
|
||||
// MakeMagnetDirection(pathResult);
|
||||
// return pathResult;
|
||||
//}
|
||||
|
||||
//3. 도킹방향이 일치하지 않으니 교차로에서 방향을 회전시켜야 한다
|
||||
//최단거리(=minpath)경로에 속하는 교차로가 있다면 그것을 사용하고 없다면 가장 가까운 교차로를 찾는다.
|
||||
var JunctionInPath = FindNearestJunctionOnPath(pathResult);
|
||||
@@ -231,6 +265,101 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 노드에서 주어진 방향(Forward/Backward)으로 이동할 때 다음 노드를 반환
|
||||
/// </summary>
|
||||
/// <param name="currentNode">현재 노드</param>
|
||||
/// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
|
||||
/// <param name="direction">이동 방향 (Forward 또는 Backward)</param>
|
||||
/// <param name="allNodes">모든 맵 노드</param>
|
||||
/// <returns>다음 노드 (또는 null)</returns>
|
||||
private MapNode GetNextNodeByDirection(MapNode currentNode, MapNode prevNode, AgvDirection direction, List<MapNode> allNodes)
|
||||
{
|
||||
if (currentNode == null || prevNode == null || allNodes == null)
|
||||
return null;
|
||||
|
||||
// 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
|
||||
var connectedNodeIds = currentNode.ConnectedNodes;
|
||||
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
|
||||
return null;
|
||||
|
||||
var candidateNodes = allNodes.Where(n =>
|
||||
connectedNodeIds.Contains(n.NodeId)
|
||||
).ToList();
|
||||
|
||||
if (candidateNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// Forward인 경우: 이전→현재 방향으로 계속 직진하는 노드 우선
|
||||
// Backward인 경우: 이전→현재 방향의 반대로 이동하는 노드 우선
|
||||
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;
|
||||
|
||||
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 == AgvDirection.Forward)
|
||||
{
|
||||
// Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
|
||||
score = dotProduct;
|
||||
}
|
||||
else // Backward
|
||||
{
|
||||
// Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
|
||||
score = -dotProduct;
|
||||
}
|
||||
|
||||
if (score > bestScore)
|
||||
{
|
||||
bestScore = score;
|
||||
bestNode = candidate;
|
||||
}
|
||||
}
|
||||
|
||||
return bestNode;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다
|
||||
/// </summary>
|
||||
|
||||
Reference in New Issue
Block a user