enhance: Display RFID values in path UI and improve AGV lift direction visualization
- Replace NodeID with RFID values in path display for better field mapping
- Add ComboBoxItem<T> class for {rfid} - [{node}] format in combo boxes
- Implement GetRfidByNodeId helper method for NodeID to RFID conversion
- Enhanced UpdatePathDebugInfo to show both RFID and NodeID information
- Improved path visualization with RFID-based route display
- Users can now easily match displayed paths with physical RFID tags
🤖 Generated with [Claude Code](https://claude.ai/code)
Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
277
Cs_HMI/AGVNavigationCore/Utils/LiftCalculator.cs
Normal file
277
Cs_HMI/AGVNavigationCore/Utils/LiftCalculator.cs
Normal file
@@ -0,0 +1,277 @@
|
||||
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>
|
||||
/// 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가 리프트 쪽으로 이동 (현재 → 타겟 방향이 리프트 방향)
|
||||
var dx = targetPos.X - currentPos.X;
|
||||
var dy = targetPos.Y - currentPos.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="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.NodeId != previousNode.NodeId).ToList();
|
||||
}
|
||||
|
||||
if (nextNodes.Count == 1)
|
||||
{
|
||||
// 직선 경로: 다음 노드 방향으로 예측
|
||||
targetPosition = nextNodes.First().Position;
|
||||
calculationMethod = $"전진 경로 예측 ({currentNode.NodeId}→{nextNodes.First().NodeId})";
|
||||
}
|
||||
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>
|
||||
/// 위치 기반 노드 찾기
|
||||
/// </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.NodeId == nodeId);
|
||||
if (connectedNode != null)
|
||||
{
|
||||
connectedNodes.Add(connectedNode);
|
||||
}
|
||||
}
|
||||
|
||||
return connectedNodes;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user