fix: RFID duplicate validation and correct magnet direction calculation
- Add real-time RFID duplicate validation in map editor with automatic rollback - Remove RFID auto-assignment to maintain data consistency between editor and simulator - Fix magnet direction calculation to use actual forward direction angles instead of arbitrary assignment - Add node names to simulator combo boxes for better identification - Improve UI layout by drawing connection lines before text for better visibility 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
ChiKyun Kim
304
Cs_HMI/AGVNavigationCore/PathFinding/JunctionAnalyzer.cs
Normal file
304
Cs_HMI/AGVNavigationCore/PathFinding/JunctionAnalyzer.cs
Normal file
@@ -0,0 +1,304 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 갈림길 분석 및 마그넷 센서 방향 계산 시스템
|
||||
/// </summary>
|
||||
public class JunctionAnalyzer
|
||||
{
|
||||
/// <summary>
|
||||
/// 갈림길 정보
|
||||
/// </summary>
|
||||
public class JunctionInfo
|
||||
{
|
||||
public string NodeId { get; set; }
|
||||
public List<string> ConnectedNodes { get; set; }
|
||||
public Dictionary<string, MagnetDirection> PathDirections { get; set; }
|
||||
public bool IsJunction => ConnectedNodes.Count > 2;
|
||||
|
||||
public JunctionInfo(string nodeId)
|
||||
{
|
||||
NodeId = nodeId;
|
||||
ConnectedNodes = new List<string>();
|
||||
PathDirections = new Dictionary<string, MagnetDirection>();
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
if (!IsJunction)
|
||||
return $"{NodeId}: 일반노드 ({ConnectedNodes.Count}연결)";
|
||||
|
||||
var paths = string.Join(", ", PathDirections.Select(p => $"{p.Key}({p.Value})"));
|
||||
return $"{NodeId}: 갈림길 - {paths}";
|
||||
}
|
||||
}
|
||||
|
||||
private readonly List<MapNode> _mapNodes;
|
||||
private readonly Dictionary<string, JunctionInfo> _junctions;
|
||||
|
||||
public JunctionAnalyzer(List<MapNode> mapNodes)
|
||||
{
|
||||
_mapNodes = mapNodes ?? new List<MapNode>();
|
||||
_junctions = new Dictionary<string, JunctionInfo>();
|
||||
AnalyzeJunctions();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모든 갈림길 분석
|
||||
/// </summary>
|
||||
private void AnalyzeJunctions()
|
||||
{
|
||||
foreach (var node in _mapNodes)
|
||||
{
|
||||
if (node.IsNavigationNode())
|
||||
{
|
||||
var junctionInfo = AnalyzeNode(node);
|
||||
_junctions[node.NodeId] = junctionInfo;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 개별 노드의 갈림길 정보 분석
|
||||
/// </summary>
|
||||
private JunctionInfo AnalyzeNode(MapNode node)
|
||||
{
|
||||
var junction = new JunctionInfo(node.NodeId);
|
||||
|
||||
// 양방향 연결을 고려하여 모든 연결된 노드 찾기
|
||||
var connectedNodes = GetAllConnectedNodes(node);
|
||||
junction.ConnectedNodes = connectedNodes;
|
||||
|
||||
if (connectedNodes.Count > 2)
|
||||
{
|
||||
// 갈림길인 경우 각 방향별 마그넷 센서 방향 계산
|
||||
CalculateMagnetDirections(node, connectedNodes, junction);
|
||||
}
|
||||
|
||||
return junction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 양방향 연결을 고려한 모든 연결 노드 검색
|
||||
/// </summary>
|
||||
private List<string> GetAllConnectedNodes(MapNode node)
|
||||
{
|
||||
var connected = new HashSet<string>();
|
||||
|
||||
// 직접 연결된 노드들
|
||||
foreach (var connectedId in node.ConnectedNodes)
|
||||
{
|
||||
connected.Add(connectedId);
|
||||
}
|
||||
|
||||
// 역방향 연결된 노드들 (다른 노드에서 이 노드로 연결)
|
||||
foreach (var otherNode in _mapNodes)
|
||||
{
|
||||
if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
|
||||
{
|
||||
connected.Add(otherNode.NodeId);
|
||||
}
|
||||
}
|
||||
|
||||
return connected.ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길에서 각 방향별 마그넷 센서 방향 계산
|
||||
/// </summary>
|
||||
private void CalculateMagnetDirections(MapNode junctionNode, List<string> connectedNodes, JunctionInfo junction)
|
||||
{
|
||||
if (connectedNodes.Count < 3) return;
|
||||
|
||||
// 각 연결 노드의 각도 계산
|
||||
var nodeAngles = new List<(string NodeId, double Angle)>();
|
||||
|
||||
foreach (var connectedId in connectedNodes)
|
||||
{
|
||||
var connectedNode = _mapNodes.FirstOrDefault(n => n.NodeId == connectedId);
|
||||
if (connectedNode != null)
|
||||
{
|
||||
double angle = CalculateAngle(junctionNode.Position, connectedNode.Position);
|
||||
nodeAngles.Add((connectedId, angle));
|
||||
}
|
||||
}
|
||||
|
||||
// 각도순으로 정렬
|
||||
nodeAngles.Sort((a, b) => a.Angle.CompareTo(b.Angle));
|
||||
|
||||
// 마그넷 방향 할당
|
||||
AssignMagnetDirections(nodeAngles, junction);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 점 사이의 각도 계산 (라디안)
|
||||
/// </summary>
|
||||
private double CalculateAngle(Point from, Point to)
|
||||
{
|
||||
double deltaX = to.X - from.X;
|
||||
double deltaY = to.Y - from.Y;
|
||||
double angle = Math.Atan2(deltaY, deltaX);
|
||||
|
||||
// 0~2π 범위로 정규화
|
||||
if (angle < 0)
|
||||
angle += 2 * Math.PI;
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길에서 마그넷 센서 방향 할당
|
||||
/// </summary>
|
||||
private void AssignMagnetDirections(List<(string NodeId, double Angle)> sortedNodes, JunctionInfo junction)
|
||||
{
|
||||
int nodeCount = sortedNodes.Count;
|
||||
|
||||
for (int i = 0; i < nodeCount; i++)
|
||||
{
|
||||
string nodeId = sortedNodes[i].NodeId;
|
||||
MagnetDirection direction;
|
||||
|
||||
if (nodeCount == 3)
|
||||
{
|
||||
// 3갈래: 직진, 좌측, 우측
|
||||
switch (i)
|
||||
{
|
||||
case 0: direction = MagnetDirection.Straight; break;
|
||||
case 1: direction = MagnetDirection.Left; break;
|
||||
case 2: direction = MagnetDirection.Right; break;
|
||||
default: direction = MagnetDirection.Straight; break;
|
||||
}
|
||||
}
|
||||
else if (nodeCount == 4)
|
||||
{
|
||||
// 4갈래: 교차로
|
||||
switch (i)
|
||||
{
|
||||
case 0: direction = MagnetDirection.Straight; break;
|
||||
case 1: direction = MagnetDirection.Left; break;
|
||||
case 2: direction = MagnetDirection.Straight; break; // 반대편
|
||||
case 3: direction = MagnetDirection.Right; break;
|
||||
default: direction = MagnetDirection.Straight; break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 5갈래 이상: 각도 기반 배정
|
||||
double angleStep = 2 * Math.PI / nodeCount;
|
||||
double normalizedIndex = (double)i / nodeCount;
|
||||
|
||||
if (normalizedIndex < 0.33)
|
||||
direction = MagnetDirection.Left;
|
||||
else if (normalizedIndex < 0.67)
|
||||
direction = MagnetDirection.Straight;
|
||||
else
|
||||
direction = MagnetDirection.Right;
|
||||
}
|
||||
|
||||
junction.PathDirections[nodeId] = direction;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 경로에서 요구되는 마그넷 방향 계산 (전진 방향 기준)
|
||||
/// </summary>
|
||||
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId)
|
||||
{
|
||||
if (!_junctions.ContainsKey(currentNodeId))
|
||||
return MagnetDirection.Straight;
|
||||
|
||||
var junction = _junctions[currentNodeId];
|
||||
if (!junction.IsJunction)
|
||||
return MagnetDirection.Straight;
|
||||
|
||||
// 실제 각도 기반으로 마그넷 방향 계산
|
||||
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
|
||||
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
|
||||
var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);
|
||||
|
||||
if (fromNode == null || currentNode == null || toNode == null)
|
||||
return MagnetDirection.Straight;
|
||||
|
||||
// 전진 방향(진행 방향) 계산
|
||||
double incomingAngle = CalculateAngle(fromNode.Position, currentNode.Position);
|
||||
|
||||
// 목표 방향 계산
|
||||
double outgoingAngle = CalculateAngle(currentNode.Position, toNode.Position);
|
||||
|
||||
// 각도 차이 계산 (전진 방향 기준)
|
||||
double angleDiff = outgoingAngle - incomingAngle;
|
||||
|
||||
// 각도를 -π ~ π 범위로 정규화
|
||||
while (angleDiff > Math.PI) angleDiff -= 2 * Math.PI;
|
||||
while (angleDiff < -Math.PI) angleDiff += 2 * Math.PI;
|
||||
|
||||
// 전진 방향 기준으로 마그넷 방향 결정
|
||||
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
|
||||
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
|
||||
return MagnetDirection.Straight;
|
||||
else if (angleDiff < 0) // 음수면 왼쪽 회전
|
||||
return MagnetDirection.Left;
|
||||
else // 양수면 오른쪽 회전
|
||||
return MagnetDirection.Right;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 가능한 갈림길 검색
|
||||
/// </summary>
|
||||
public List<string> FindDirectionChangeJunctions(AgvDirection currentDirection, AgvDirection targetDirection)
|
||||
{
|
||||
var availableJunctions = new List<string>();
|
||||
|
||||
if (currentDirection == targetDirection)
|
||||
return availableJunctions;
|
||||
|
||||
foreach (var junction in _junctions.Values)
|
||||
{
|
||||
if (junction.IsJunction)
|
||||
{
|
||||
// 갈림길에서 방향 전환이 가능한지 확인
|
||||
// (실제로는 더 복잡한 로직이 필요하지만, 일단 모든 갈림길을 후보로 함)
|
||||
availableJunctions.Add(junction.NodeId);
|
||||
}
|
||||
}
|
||||
|
||||
return availableJunctions;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길 정보 반환
|
||||
/// </summary>
|
||||
public JunctionInfo GetJunctionInfo(string nodeId)
|
||||
{
|
||||
return _junctions.ContainsKey(nodeId) ? _junctions[nodeId] : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모든 갈림길 목록 반환
|
||||
/// </summary>
|
||||
public List<JunctionInfo> GetAllJunctions()
|
||||
{
|
||||
return _junctions.Values.Where(j => j.IsJunction).ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 디버깅용 갈림길 정보 출력
|
||||
/// </summary>
|
||||
public List<string> GetJunctionSummary()
|
||||
{
|
||||
var summary = new List<string>();
|
||||
|
||||
foreach (var junction in _junctions.Values.Where(j => j.IsJunction))
|
||||
{
|
||||
summary.Add(junction.ToString());
|
||||
}
|
||||
|
||||
return summary;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user