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:
@@ -71,6 +71,7 @@
|
||||
<Compile Include="PathFinding\AStarPathfinder.cs" />
|
||||
<Compile Include="PathFinding\AGVPathfinder.cs" />
|
||||
<Compile Include="PathFinding\AGVPathResult.cs" />
|
||||
<Compile Include="PathFinding\NodeMotorInfo.cs" />
|
||||
<Compile Include="PathFinding\RfidBasedPathfinder.cs" />
|
||||
<Compile Include="PathFinding\RfidPathResult.cs" />
|
||||
<Compile Include="Controls\UnifiedAGVCanvas.cs">
|
||||
@@ -87,11 +88,9 @@
|
||||
<DependentUpon>UnifiedAGVCanvas.cs</DependentUpon>
|
||||
<SubType>UserControl</SubType>
|
||||
</Compile>
|
||||
<Compile Include="Utils\LiftCalculator.cs" />
|
||||
<Compile Include="Properties\AssemblyInfo.cs" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<Folder Include="Utils\" />
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<None Include="build.bat" />
|
||||
<None Include="packages.config" />
|
||||
|
||||
@@ -150,10 +150,14 @@ namespace AGVNavigationCore.Controls
|
||||
}
|
||||
}
|
||||
|
||||
// 현재 선택된 경로 그리기
|
||||
// 현재 선택된 경로 그리기 (AGVPathResult 활용)
|
||||
if (_currentPath != null)
|
||||
{
|
||||
DrawPath(g, _currentPath, Color.Purple);
|
||||
|
||||
// AGVPathResult의 모터방향 정보가 있다면 향상된 경로 그리기
|
||||
// 현재는 기본 PathResult를 사용하므로 향후 AGVPathResult로 업그레이드 시 활성화
|
||||
// TODO: AGVPathfinder 사용시 AGVPathResult로 업그레이드
|
||||
}
|
||||
}
|
||||
|
||||
@@ -191,6 +195,87 @@ namespace AGVNavigationCore.Controls
|
||||
pathPen.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 및 모터방향 정보를 시각화
|
||||
/// </summary>
|
||||
/// <param name="g">Graphics 객체</param>
|
||||
/// <param name="agvResult">AGV 경로 계산 결과</param>
|
||||
private void DrawAGVPath(Graphics g, AGVPathResult agvResult)
|
||||
{
|
||||
if (agvResult?.NodeMotorInfos == null || agvResult.NodeMotorInfos.Count == 0) return;
|
||||
|
||||
// 노드별 모터방향 정보를 기반으로 향상된 경로 표시
|
||||
for (int i = 0; i < agvResult.NodeMotorInfos.Count - 1; i++)
|
||||
{
|
||||
var currentMotorInfo = agvResult.NodeMotorInfos[i];
|
||||
var nextMotorInfo = agvResult.NodeMotorInfos[i + 1];
|
||||
|
||||
var currentNode = _nodes?.FirstOrDefault(n => n.NodeId == currentMotorInfo.NodeId);
|
||||
var nextNode = _nodes?.FirstOrDefault(n => n.NodeId == nextMotorInfo.NodeId);
|
||||
|
||||
if (currentNode != null && nextNode != null)
|
||||
{
|
||||
// 모터방향에 따른 색상 결정
|
||||
var motorDirection = currentMotorInfo.MotorDirection;
|
||||
Color pathColor = motorDirection == AgvDirection.Forward ? Color.Green : Color.Orange;
|
||||
|
||||
// 강조된 경로 선 그리기
|
||||
var enhancedPen = new Pen(pathColor, 6) { DashStyle = DashStyle.Solid };
|
||||
g.DrawLine(enhancedPen, currentNode.Position, nextNode.Position);
|
||||
|
||||
// 중간점에 모터방향 화살표 표시
|
||||
var midPoint = new Point(
|
||||
(currentNode.Position.X + nextNode.Position.X) / 2,
|
||||
(currentNode.Position.Y + nextNode.Position.Y) / 2
|
||||
);
|
||||
|
||||
var angle = Math.Atan2(nextNode.Position.Y - currentNode.Position.Y,
|
||||
nextNode.Position.X - currentNode.Position.X);
|
||||
|
||||
// 모터방향별 화살표 그리기
|
||||
DrawDirectionArrow(g, midPoint, angle, motorDirection);
|
||||
|
||||
// 노드 옆에 모터방향 텍스트 표시
|
||||
DrawMotorDirectionLabel(g, currentNode.Position, motorDirection);
|
||||
|
||||
enhancedPen.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
// 마지막 노드의 모터방향 표시
|
||||
if (agvResult.NodeMotorInfos.Count > 0)
|
||||
{
|
||||
var lastMotorInfo = agvResult.NodeMotorInfos[agvResult.NodeMotorInfos.Count - 1];
|
||||
var lastNode = _nodes?.FirstOrDefault(n => n.NodeId == lastMotorInfo.NodeId);
|
||||
if (lastNode != null)
|
||||
{
|
||||
DrawMotorDirectionLabel(g, lastNode.Position, lastMotorInfo.MotorDirection);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모터방향 레이블 표시
|
||||
/// </summary>
|
||||
/// <param name="g">Graphics 객체</param>
|
||||
/// <param name="nodePosition">노드 위치</param>
|
||||
/// <param name="motorDirection">모터방향</param>
|
||||
private void DrawMotorDirectionLabel(Graphics g, Point nodePosition, AgvDirection motorDirection)
|
||||
{
|
||||
string motorText = motorDirection == AgvDirection.Forward ? "전진" : "후진";
|
||||
Color textColor = motorDirection == AgvDirection.Forward ? Color.DarkGreen : Color.DarkOrange;
|
||||
|
||||
var font = new Font("맑은 고딕", 8, FontStyle.Bold);
|
||||
var brush = new SolidBrush(textColor);
|
||||
|
||||
// 노드 우측 상단에 모터방향 텍스트 표시
|
||||
var textPosition = new Point(nodePosition.X + NODE_RADIUS + 2, nodePosition.Y - NODE_RADIUS - 2);
|
||||
g.DrawString(motorText, font, brush, textPosition);
|
||||
|
||||
font.Dispose();
|
||||
brush.Dispose();
|
||||
}
|
||||
|
||||
private void DrawNodes(Graphics g)
|
||||
{
|
||||
if (_nodes == null) return;
|
||||
@@ -254,6 +339,17 @@ namespace AGVNavigationCore.Controls
|
||||
g.DrawEllipse(_selectedNodePen, rect);
|
||||
}
|
||||
|
||||
// 목적지 노드 강조
|
||||
if (node == _destinationNode)
|
||||
{
|
||||
// 금색 테두리로 목적지 강조
|
||||
g.DrawEllipse(_destinationNodePen, rect);
|
||||
|
||||
// 펄싱 효과를 위한 추가 원 그리기
|
||||
var pulseRect = new Rectangle(rect.X - 3, rect.Y - 3, rect.Width + 6, rect.Height + 6);
|
||||
g.DrawEllipse(new Pen(Color.Gold, 2) { DashStyle = DashStyle.Dash }, pulseRect);
|
||||
}
|
||||
|
||||
// 호버된 노드 강조
|
||||
if (node == _hoveredNode)
|
||||
{
|
||||
@@ -290,6 +386,25 @@ namespace AGVNavigationCore.Controls
|
||||
g.DrawPolygon(_selectedNodePen, points);
|
||||
}
|
||||
|
||||
// 목적지 노드 강조
|
||||
if (node == _destinationNode)
|
||||
{
|
||||
// 금색 테두리로 목적지 강조
|
||||
g.DrawPolygon(_destinationNodePen, points);
|
||||
|
||||
// 펄싱 효과를 위한 추가 오각형 그리기
|
||||
var pulsePoints = new Point[5];
|
||||
for (int i = 0; i < 5; i++)
|
||||
{
|
||||
var angle = (Math.PI * 2 * i / 5) - Math.PI / 2;
|
||||
pulsePoints[i] = new Point(
|
||||
(int)(center.X + (radius + 4) * Math.Cos(angle)),
|
||||
(int)(center.Y + (radius + 4) * Math.Sin(angle))
|
||||
);
|
||||
}
|
||||
g.DrawPolygon(new Pen(Color.Gold, 2) { DashStyle = DashStyle.Dash }, pulsePoints);
|
||||
}
|
||||
|
||||
// 호버된 노드 강조
|
||||
if (node == _hoveredNode)
|
||||
{
|
||||
@@ -335,6 +450,25 @@ namespace AGVNavigationCore.Controls
|
||||
g.DrawPolygon(_selectedNodePen, points);
|
||||
}
|
||||
|
||||
// 목적지 노드 강조
|
||||
if (node == _destinationNode)
|
||||
{
|
||||
// 금색 테두리로 목적지 강조
|
||||
g.DrawPolygon(_destinationNodePen, points);
|
||||
|
||||
// 펄싱 효과를 위한 추가 삼각형 그리기
|
||||
var pulsePoints = new Point[3];
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
var angle = (Math.PI * 2 * i / 3) - Math.PI / 2;
|
||||
pulsePoints[i] = new Point(
|
||||
(int)(center.X + (radius + 4) * Math.Cos(angle)),
|
||||
(int)(center.Y + (radius + 4) * Math.Sin(angle))
|
||||
);
|
||||
}
|
||||
g.DrawPolygon(new Pen(Color.Gold, 2) { DashStyle = DashStyle.Dash }, pulsePoints);
|
||||
}
|
||||
|
||||
// 호버된 노드 강조
|
||||
if (node == _hoveredNode)
|
||||
{
|
||||
@@ -643,7 +777,7 @@ namespace AGVNavigationCore.Controls
|
||||
// AGV 색상 결정
|
||||
var brush = GetAGVBrush(state);
|
||||
|
||||
// AGV 사각형 그리기
|
||||
// AGV 개선된 원형 디자인 그리기
|
||||
var rect = new Rectangle(
|
||||
position.X - AGV_SIZE / 2,
|
||||
position.Y - AGV_SIZE / 2,
|
||||
@@ -651,11 +785,82 @@ namespace AGVNavigationCore.Controls
|
||||
AGV_SIZE
|
||||
);
|
||||
|
||||
g.FillRectangle(brush, rect);
|
||||
g.DrawRectangle(_agvPen, rect);
|
||||
// 방사형 그라디언트 효과를 위한 브러시 생성
|
||||
using (var gradientBrush = new System.Drawing.Drawing2D.PathGradientBrush(
|
||||
new Point[] {
|
||||
new Point(rect.Left, rect.Top),
|
||||
new Point(rect.Right, rect.Top),
|
||||
new Point(rect.Right, rect.Bottom),
|
||||
new Point(rect.Left, rect.Bottom)
|
||||
}))
|
||||
{
|
||||
gradientBrush.CenterPoint = new PointF(position.X, position.Y);
|
||||
gradientBrush.CenterColor = GetAGVCenterColor(state);
|
||||
gradientBrush.SurroundColors = new Color[] {
|
||||
GetAGVOuterColor(state), GetAGVOuterColor(state),
|
||||
GetAGVOuterColor(state), GetAGVOuterColor(state)
|
||||
};
|
||||
|
||||
// 원형으로 AGV 본체 그리기 (3D 효과)
|
||||
g.FillEllipse(gradientBrush, rect);
|
||||
|
||||
// 외곽선 (두께 조절)
|
||||
using (var outerPen = new Pen(Color.DarkGray, 3))
|
||||
{
|
||||
g.DrawEllipse(outerPen, rect);
|
||||
}
|
||||
|
||||
// 내부 링 (입체감)
|
||||
var innerSize = AGV_SIZE - 8;
|
||||
var innerRect = new Rectangle(
|
||||
position.X - innerSize / 2,
|
||||
position.Y - innerSize / 2,
|
||||
innerSize,
|
||||
innerSize
|
||||
);
|
||||
using (var innerPen = new Pen(GetAGVInnerRingColor(state), 2))
|
||||
{
|
||||
g.DrawEllipse(innerPen, innerRect);
|
||||
}
|
||||
|
||||
// 중앙 상태 표시등 (더 크고 화려하게)
|
||||
var indicatorSize = 10;
|
||||
var indicatorRect = new Rectangle(
|
||||
position.X - indicatorSize / 2,
|
||||
position.Y - indicatorSize / 2,
|
||||
indicatorSize,
|
||||
indicatorSize
|
||||
);
|
||||
|
||||
// 표시등 글로우 효과
|
||||
using (var glowBrush = new System.Drawing.Drawing2D.PathGradientBrush(
|
||||
new Point[] {
|
||||
new Point(indicatorRect.Left, indicatorRect.Top),
|
||||
new Point(indicatorRect.Right, indicatorRect.Top),
|
||||
new Point(indicatorRect.Right, indicatorRect.Bottom),
|
||||
new Point(indicatorRect.Left, indicatorRect.Bottom)
|
||||
}))
|
||||
{
|
||||
glowBrush.CenterPoint = new PointF(position.X, position.Y);
|
||||
glowBrush.CenterColor = Color.White;
|
||||
glowBrush.SurroundColors = new Color[] {
|
||||
GetStatusIndicatorColor(state), GetStatusIndicatorColor(state),
|
||||
GetStatusIndicatorColor(state), GetStatusIndicatorColor(state)
|
||||
};
|
||||
|
||||
g.FillEllipse(glowBrush, indicatorRect);
|
||||
g.DrawEllipse(new Pen(Color.DarkGray, 1), indicatorRect);
|
||||
}
|
||||
|
||||
// 원형 센서 패턴 (방사형 배치)
|
||||
DrawCircularSensors(g, position, state);
|
||||
}
|
||||
|
||||
// 방향 표시 (화살표)
|
||||
DrawAGVDirection(g, position, direction);
|
||||
// 리프트 그리기 (이동 경로 기반)
|
||||
DrawAGVLiftAdvanced(g, agv);
|
||||
|
||||
// 모니터 그리기 (리프트 반대편)
|
||||
DrawAGVMonitor(g, agv);
|
||||
|
||||
// AGV ID 표시
|
||||
var font = new Font("Arial", 10, FontStyle.Bold);
|
||||
@@ -700,35 +905,194 @@ namespace AGVNavigationCore.Controls
|
||||
}
|
||||
}
|
||||
|
||||
private void DrawAGVDirection(Graphics g, Point position, AgvDirection direction)
|
||||
{
|
||||
var arrowSize = 10;
|
||||
Point[] arrowPoints = null;
|
||||
|
||||
switch (direction)
|
||||
/// <summary>
|
||||
/// 이동 경로 기반 개선된 리프트 그리기 (각도 기반 동적 디자인)
|
||||
/// </summary>
|
||||
private void DrawAGVLiftAdvanced(Graphics g, IAGV agv)
|
||||
{
|
||||
const int liftLength = 28; // 리프트 길이 (더욱 크게)
|
||||
const int liftWidth = 16; // 리프트 너비 (더욱 크게)
|
||||
const int liftDistance = AGV_SIZE / 2 + 2; // AGV 본체 면에 바로 붙도록
|
||||
|
||||
var currentPos = agv.CurrentPosition;
|
||||
var targetPos = agv.TargetPosition;
|
||||
var dockingDirection = agv.DockingDirection;
|
||||
var currentDirection = agv.CurrentDirection;
|
||||
|
||||
// 경로 예측 기반 LiftCalculator 사용
|
||||
Point fallbackTarget = targetPos ?? new Point(currentPos.X + 1, currentPos.Y); // 기본 타겟
|
||||
var liftInfo = AGVNavigationCore.Utils.LiftCalculator.CalculateLiftInfoWithPathPrediction(
|
||||
currentPos, fallbackTarget, currentDirection, _nodes);
|
||||
|
||||
float liftAngle = (float)liftInfo.AngleRadians;
|
||||
|
||||
// 리프트 위치 계산 (각도 기반)
|
||||
Point liftPosition = new Point(
|
||||
(int)(currentPos.X + liftDistance * Math.Cos(liftAngle)),
|
||||
(int)(currentPos.Y + liftDistance * Math.Sin(liftAngle))
|
||||
);
|
||||
|
||||
// 방향을 알 수 있는지 확인
|
||||
bool hasDirection = targetPos.HasValue && targetPos.Value != currentPos;
|
||||
|
||||
// 방향에 따른 리프트 색상 및 스타일 결정
|
||||
Color liftColor;
|
||||
Color borderColor;
|
||||
|
||||
// 모터 방향과 경로 상태에 따른 색상 결정 (더 눈에 띄게)
|
||||
switch (currentDirection)
|
||||
{
|
||||
case AgvDirection.Forward:
|
||||
arrowPoints = new Point[]
|
||||
{
|
||||
new Point(position.X + arrowSize, position.Y),
|
||||
new Point(position.X - arrowSize/2, position.Y - arrowSize/2),
|
||||
new Point(position.X - arrowSize/2, position.Y + arrowSize/2)
|
||||
};
|
||||
liftColor = Color.Yellow; // 전진 - 노란색 (잘 보임)
|
||||
borderColor = Color.Orange;
|
||||
break;
|
||||
case AgvDirection.Backward:
|
||||
arrowPoints = new Point[]
|
||||
{
|
||||
new Point(position.X - arrowSize, position.Y),
|
||||
new Point(position.X + arrowSize/2, position.Y - arrowSize/2),
|
||||
new Point(position.X + arrowSize/2, position.Y + arrowSize/2)
|
||||
};
|
||||
liftColor = Color.Cyan; // 후진 - 시안색 (잘 보임)
|
||||
borderColor = Color.DarkCyan;
|
||||
break;
|
||||
default:
|
||||
liftColor = Color.LightGray; // 방향 불명 - 회색
|
||||
borderColor = Color.Gray;
|
||||
break;
|
||||
}
|
||||
|
||||
if (arrowPoints != null)
|
||||
|
||||
// 경로 예측 결과에 따른 색상 조정
|
||||
if (liftInfo.CalculationMethod.Contains("경로 예측"))
|
||||
{
|
||||
g.FillPolygon(Brushes.White, arrowPoints);
|
||||
g.DrawPolygon(Pens.Black, arrowPoints);
|
||||
// 경로 예측이 성공한 경우 더 선명한 색상
|
||||
if (currentDirection == AgvDirection.Forward)
|
||||
{
|
||||
liftColor = Color.Gold; // 전진 예측 - 골드
|
||||
borderColor = Color.DarkGoldenrod;
|
||||
}
|
||||
else if (currentDirection == AgvDirection.Backward)
|
||||
{
|
||||
liftColor = Color.DeepSkyBlue; // 후진 예측 - 딥 스카이 블루
|
||||
borderColor = Color.Navy;
|
||||
}
|
||||
}
|
||||
else if (liftInfo.CalculationMethod.Contains("갈래길"))
|
||||
{
|
||||
// 갈래길에서는 약간 흐린 색상
|
||||
liftColor = Color.FromArgb(200, liftColor.R, liftColor.G, liftColor.B);
|
||||
borderColor = Color.FromArgb(150, borderColor.R, borderColor.G, borderColor.B);
|
||||
}
|
||||
|
||||
// Graphics 상태 저장
|
||||
var oldTransform = g.Transform;
|
||||
|
||||
// 리프트 중심으로 회전 변환 적용
|
||||
g.TranslateTransform(liftPosition.X, liftPosition.Y);
|
||||
g.RotateTransform((float)(liftAngle * 180.0 / Math.PI));
|
||||
|
||||
// 회전된 좌표계에서 리프트 그리기 (중심이 원점)
|
||||
var liftRect = new Rectangle(
|
||||
-liftLength / 2, // 중심 기준 왼쪽
|
||||
-liftWidth / 2, // 중심 기준 위쪽
|
||||
liftLength,
|
||||
liftWidth
|
||||
);
|
||||
|
||||
using (var liftBrush = new SolidBrush(liftColor))
|
||||
using (var liftPen = new Pen(borderColor, 3)) // 두껍게 테두리
|
||||
{
|
||||
// 둥근 사각형으로 리프트 그리기
|
||||
DrawRoundedRectangle(g, liftBrush, liftPen, liftRect, 4);
|
||||
|
||||
// 추가 강조 테두리 (더 잘 보이게)
|
||||
using (var emphasisPen = new Pen(Color.Black, 1))
|
||||
{
|
||||
DrawRoundedRectangle(g, null, emphasisPen, liftRect, 4);
|
||||
}
|
||||
}
|
||||
|
||||
if (hasDirection)
|
||||
{
|
||||
// 리프트 세부 표현 (방향 표시용 선들)
|
||||
using (var detailPen = new Pen(Color.Gray, 1))
|
||||
{
|
||||
int lineSpacing = 4;
|
||||
for (int i = -liftLength / 2 + 3; i < liftLength / 2 - 3; i += lineSpacing)
|
||||
{
|
||||
g.DrawLine(detailPen, i, -liftWidth / 2 + 2, i, liftWidth / 2 - 2);
|
||||
}
|
||||
}
|
||||
|
||||
// 이동 방향 표시 화살표 (리프트 끝쪽)
|
||||
using (var arrowPen = new Pen(borderColor, 2))
|
||||
{
|
||||
var arrowSize = 3;
|
||||
var arrowX = liftLength / 2 - 4;
|
||||
|
||||
// 화살표 모양
|
||||
g.DrawLine(arrowPen, arrowX, 0, arrowX + arrowSize, -arrowSize);
|
||||
g.DrawLine(arrowPen, arrowX, 0, arrowX + arrowSize, arrowSize);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 방향을 모를 때 '?' 표시
|
||||
using (var questionBrush = new SolidBrush(Color.DarkGray))
|
||||
using (var questionFont = new Font("Arial", 9, FontStyle.Bold))
|
||||
{
|
||||
var questionSize = g.MeasureString("?", questionFont);
|
||||
var questionPoint = new PointF(
|
||||
-questionSize.Width / 2,
|
||||
-questionSize.Height / 2
|
||||
);
|
||||
g.DrawString("?", questionFont, questionBrush, questionPoint);
|
||||
}
|
||||
}
|
||||
|
||||
// Graphics 상태 복원
|
||||
g.Transform = oldTransform;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 둥근 모서리 사각형 그리기
|
||||
/// </summary>
|
||||
private void DrawRoundedRectangle(Graphics g, Brush fillBrush, Pen borderPen, Rectangle rect, int cornerRadius)
|
||||
{
|
||||
if (cornerRadius <= 0)
|
||||
{
|
||||
if (fillBrush != null)
|
||||
g.FillRectangle(fillBrush, rect);
|
||||
if (borderPen != null)
|
||||
g.DrawRectangle(borderPen, rect);
|
||||
return;
|
||||
}
|
||||
|
||||
using (var path = new System.Drawing.Drawing2D.GraphicsPath())
|
||||
{
|
||||
// 둥근 모서리 경로 생성
|
||||
path.AddArc(rect.X, rect.Y, cornerRadius * 2, cornerRadius * 2, 180, 90);
|
||||
path.AddArc(rect.Right - cornerRadius * 2, rect.Y, cornerRadius * 2, cornerRadius * 2, 270, 90);
|
||||
path.AddArc(rect.Right - cornerRadius * 2, rect.Bottom - cornerRadius * 2, cornerRadius * 2, cornerRadius * 2, 0, 90);
|
||||
path.AddArc(rect.X, rect.Bottom - cornerRadius * 2, cornerRadius * 2, cornerRadius * 2, 90, 90);
|
||||
path.CloseFigure();
|
||||
|
||||
// 채우기와 테두리 그리기 (null 체크 추가)
|
||||
if (fillBrush != null)
|
||||
g.FillPath(fillBrush, path);
|
||||
if (borderPen != null)
|
||||
g.DrawPath(borderPen, path);
|
||||
}
|
||||
}
|
||||
|
||||
private void DrawAGVLiftDebugInfo(Graphics g, IAGV agv)
|
||||
{
|
||||
var currentPos = agv.CurrentPosition;
|
||||
var targetPos = agv.TargetPosition;
|
||||
|
||||
// 디버그 정보 (개발용)
|
||||
if (targetPos.HasValue)
|
||||
{
|
||||
// 이동 방향 벡터 그리기 (얇은 점선)
|
||||
using (var debugPen = new Pen(Color.Red, 1) { DashStyle = System.Drawing.Drawing2D.DashStyle.Dot })
|
||||
{
|
||||
g.DrawLine(debugPen, currentPos, targetPos.Value);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -740,6 +1104,289 @@ namespace AGVNavigationCore.Controls
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 중앙 색상 (방사형 그라디언트)
|
||||
/// </summary>
|
||||
private Color GetAGVCenterColor(AGVState state)
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case AGVState.Moving: return Color.White;
|
||||
case AGVState.Charging: return Color.LightCyan;
|
||||
case AGVState.Error: return Color.LightPink;
|
||||
case AGVState.Docking: return Color.LightYellow;
|
||||
default: return Color.WhiteSmoke;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 외곽 색상 (방사형 그라디언트)
|
||||
/// </summary>
|
||||
private Color GetAGVOuterColor(AGVState state)
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case AGVState.Moving: return Color.DarkGreen;
|
||||
case AGVState.Charging: return Color.DarkBlue;
|
||||
case AGVState.Error: return Color.DarkRed;
|
||||
case AGVState.Docking: return Color.DarkOrange;
|
||||
default: return Color.DarkGray;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 내부 링 색상
|
||||
/// </summary>
|
||||
private Color GetAGVInnerRingColor(AGVState state)
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case AGVState.Moving: return Color.Green;
|
||||
case AGVState.Charging: return Color.Blue;
|
||||
case AGVState.Error: return Color.Red;
|
||||
case AGVState.Docking: return Color.Orange;
|
||||
default: return Color.Gray;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 중앙 표시등 색상
|
||||
/// </summary>
|
||||
private Color GetStatusIndicatorColor(AGVState state)
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case AGVState.Moving: return Color.Lime;
|
||||
case AGVState.Charging: return Color.Blue;
|
||||
case AGVState.Error: return Color.Red;
|
||||
case AGVState.Docking: return Color.Yellow;
|
||||
default: return Color.Gray;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 원형 센서 패턴 (방사형 배치)
|
||||
/// </summary>
|
||||
private void DrawCircularSensors(Graphics g, Point center, AGVState state)
|
||||
{
|
||||
var sensorSize = 3;
|
||||
var sensorColor = state == AGVState.Error ? Color.Red : Color.Silver;
|
||||
var radius = AGV_SIZE / 2 - 6;
|
||||
|
||||
// 8방향으로 센서 배치 (45도씩)
|
||||
for (int i = 0; i < 8; i++)
|
||||
{
|
||||
double angle = i * Math.PI / 4; // 45도씩
|
||||
int sensorX = (int)(center.X + radius * Math.Cos(angle));
|
||||
int sensorY = (int)(center.Y + radius * Math.Sin(angle));
|
||||
|
||||
var sensorRect = new Rectangle(
|
||||
sensorX - sensorSize / 2,
|
||||
sensorY - sensorSize / 2,
|
||||
sensorSize,
|
||||
sensorSize
|
||||
);
|
||||
|
||||
using (var sensorBrush = new SolidBrush(sensorColor))
|
||||
{
|
||||
g.FillEllipse(sensorBrush, sensorRect);
|
||||
g.DrawEllipse(new Pen(Color.Black, 1), sensorRect);
|
||||
}
|
||||
}
|
||||
|
||||
// 내부 원형 패턴 (장식)
|
||||
var innerRadius = AGV_SIZE / 2 - 12;
|
||||
for (int i = 0; i < 4; i++)
|
||||
{
|
||||
double angle = i * Math.PI / 2 + Math.PI / 4; // 45도 오프셋된 4방향
|
||||
int dotX = (int)(center.X + innerRadius * Math.Cos(angle));
|
||||
int dotY = (int)(center.Y + innerRadius * Math.Sin(angle));
|
||||
|
||||
var dotRect = new Rectangle(dotX - 1, dotY - 1, 2, 2);
|
||||
using (var dotBrush = new SolidBrush(GetAGVInnerRingColor(state)))
|
||||
{
|
||||
g.FillEllipse(dotBrush, dotRect);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 모니터 그리기 (리프트 반대편)
|
||||
/// </summary>
|
||||
private void DrawAGVMonitor(Graphics g, IAGV agv)
|
||||
{
|
||||
const int monitorWidth = 12; // 모니터 너비 (가로로 더 길게)
|
||||
const int monitorHeight = 24; // 모니터 높이 (세로는 줄임)
|
||||
const int monitorDistance = AGV_SIZE / 2 + 2; // AGV 본체에서 거리 (리프트와 동일)
|
||||
|
||||
var currentPos = agv.CurrentPosition;
|
||||
var targetPos = agv.TargetPosition;
|
||||
var dockingDirection = agv.DockingDirection;
|
||||
var currentDirection = agv.CurrentDirection;
|
||||
|
||||
// 리프트 방향 계산 (모니터는 정반대)
|
||||
Point fallbackTarget = targetPos ?? new Point(currentPos.X + 50, currentPos.Y);
|
||||
|
||||
var liftInfo = AGVNavigationCore.Utils.LiftCalculator.CalculateLiftInfoWithPathPrediction(
|
||||
currentPos, fallbackTarget, currentDirection, _nodes);
|
||||
|
||||
bool hasDirection = liftInfo != null;
|
||||
double monitorAngle = hasDirection ? liftInfo.AngleRadians + Math.PI : 0; // 리프트 반대 방향 (180도 회전)
|
||||
|
||||
// Graphics 변환 설정
|
||||
var oldTransform = g.Transform;
|
||||
var transform = oldTransform.Clone();
|
||||
transform.Translate(currentPos.X, currentPos.Y);
|
||||
transform.Rotate((float)(monitorAngle * 180.0 / Math.PI));
|
||||
g.Transform = transform;
|
||||
|
||||
// 모니터 위치 (AGV 면에 붙도록)
|
||||
var monitorRect = new Rectangle(
|
||||
monitorDistance - monitorWidth / 2,
|
||||
-monitorHeight / 2,
|
||||
monitorWidth,
|
||||
monitorHeight
|
||||
);
|
||||
|
||||
if (hasDirection)
|
||||
{
|
||||
// 모니터 상태에 따른 색상
|
||||
var monitorBackColor = GetMonitorBackColor(agv.CurrentState);
|
||||
var monitorBorderColor = Color.DarkGray;
|
||||
|
||||
// 모니터 본체 (둥근 모서리)
|
||||
using (var monitorBrush = new SolidBrush(monitorBackColor))
|
||||
using (var borderPen = new Pen(monitorBorderColor, 2))
|
||||
{
|
||||
DrawRoundedRectangle(g, monitorBrush, borderPen, monitorRect, 2);
|
||||
}
|
||||
|
||||
// 모니터 화면 (내부 사각형)
|
||||
var screenRect = new Rectangle(
|
||||
monitorRect.X + 2,
|
||||
monitorRect.Y + 2,
|
||||
monitorRect.Width - 4,
|
||||
monitorRect.Height - 4
|
||||
);
|
||||
|
||||
using (var screenBrush = new SolidBrush(GetMonitorScreenColor(agv.CurrentState)))
|
||||
{
|
||||
g.FillRectangle(screenBrush, screenRect);
|
||||
}
|
||||
|
||||
// 화면 패턴 (상태별)
|
||||
DrawMonitorScreen(g, screenRect, agv.CurrentState);
|
||||
|
||||
// 모니터 스탠드
|
||||
var standRect = new Rectangle(
|
||||
monitorDistance - 2,
|
||||
monitorHeight / 2 - 1,
|
||||
4,
|
||||
3
|
||||
);
|
||||
using (var standBrush = new SolidBrush(Color.Gray))
|
||||
{
|
||||
g.FillRectangle(standBrush, standRect);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 방향을 모를 때 기본 모니터 (스카이블루로)
|
||||
using (var monitorBrush = new SolidBrush(Color.SkyBlue))
|
||||
using (var borderPen = new Pen(Color.DarkGray, 2))
|
||||
{
|
||||
DrawRoundedRectangle(g, monitorBrush, borderPen, monitorRect, 2);
|
||||
}
|
||||
}
|
||||
|
||||
// Graphics 상태 복원
|
||||
g.Transform = oldTransform;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모니터 배경 색상 (스카이블루 계통으로 통일)
|
||||
/// </summary>
|
||||
private Color GetMonitorBackColor(AGVState state)
|
||||
{
|
||||
// 모든 상태에서 스카이블루 계통으로 통일하여 AGV와 구분
|
||||
return Color.SkyBlue;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모니터 화면 색상 (상태별로 구분)
|
||||
/// </summary>
|
||||
private Color GetMonitorScreenColor(AGVState state)
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case AGVState.Moving: return Color.Navy; // 이동: 네이비
|
||||
case AGVState.Charging: return Color.DarkBlue; // 충전: 다크블루
|
||||
case AGVState.Error: return Color.DarkRed; // 에러: 다크레드
|
||||
case AGVState.Docking: return Color.DarkGreen; // 도킹: 다크그린
|
||||
default: return Color.DarkSlateGray; // 대기: 다크슬레이트그레이
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모니터 화면 패턴 그리기
|
||||
/// </summary>
|
||||
private void DrawMonitorScreen(Graphics g, Rectangle screenRect, AGVState state)
|
||||
{
|
||||
using (var patternPen = new Pen(Color.White, 1))
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case AGVState.Moving:
|
||||
// 이동 중: 작은 점들 (데이터 표시)
|
||||
for (int i = 1; i < screenRect.Width; i += 3)
|
||||
{
|
||||
for (int j = 1; j < screenRect.Height; j += 3)
|
||||
{
|
||||
g.DrawRectangle(patternPen, screenRect.X + i, screenRect.Y + j, 1, 1);
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case AGVState.Charging:
|
||||
// 충전 중: 배터리 아이콘
|
||||
var batteryRect = new Rectangle(
|
||||
screenRect.X + screenRect.Width / 2 - 3,
|
||||
screenRect.Y + screenRect.Height / 2 - 2,
|
||||
6,
|
||||
4
|
||||
);
|
||||
g.DrawRectangle(patternPen, batteryRect);
|
||||
g.DrawLine(patternPen, batteryRect.Right, batteryRect.Y + 1, batteryRect.Right, batteryRect.Bottom - 1);
|
||||
break;
|
||||
|
||||
case AGVState.Error:
|
||||
// 에러: X 마크
|
||||
g.DrawLine(patternPen, screenRect.X + 2, screenRect.Y + 2,
|
||||
screenRect.Right - 2, screenRect.Bottom - 2);
|
||||
g.DrawLine(patternPen, screenRect.Right - 2, screenRect.Y + 2,
|
||||
screenRect.X + 2, screenRect.Bottom - 2);
|
||||
break;
|
||||
|
||||
case AGVState.Docking:
|
||||
// 도킹: 화살표
|
||||
var centerX = screenRect.X + screenRect.Width / 2;
|
||||
var centerY = screenRect.Y + screenRect.Height / 2;
|
||||
g.DrawLine(patternPen, centerX - 4, centerY, centerX + 2, centerY);
|
||||
g.DrawLine(patternPen, centerX, centerY - 2, centerX + 2, centerY);
|
||||
g.DrawLine(patternPen, centerX, centerY + 2, centerX + 2, centerY);
|
||||
break;
|
||||
|
||||
default:
|
||||
// 대기: 중앙 점
|
||||
g.DrawRectangle(patternPen,
|
||||
screenRect.X + screenRect.Width / 2 - 1,
|
||||
screenRect.Y + screenRect.Height / 2 - 1,
|
||||
2, 2);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void DrawUIInfo(Graphics g)
|
||||
{
|
||||
// 회사 로고
|
||||
|
||||
@@ -22,7 +22,7 @@ namespace AGVNavigationCore.Controls
|
||||
private const int GRID_SIZE = 20;
|
||||
private const float CONNECTION_WIDTH = 2.0f;
|
||||
private const int SNAP_DISTANCE = 10;
|
||||
private const int AGV_SIZE = 30;
|
||||
private const int AGV_SIZE = 40;
|
||||
private const int CONNECTION_ARROW_SIZE = 8;
|
||||
|
||||
#endregion
|
||||
@@ -64,6 +64,7 @@ namespace AGVNavigationCore.Controls
|
||||
private List<MapNode> _nodes;
|
||||
private MapNode _selectedNode;
|
||||
private MapNode _hoveredNode;
|
||||
private MapNode _destinationNode;
|
||||
|
||||
// AGV 관련
|
||||
private List<IAGV> _agvList;
|
||||
@@ -105,6 +106,7 @@ namespace AGVNavigationCore.Controls
|
||||
private Brush _chargingNodeBrush;
|
||||
private Brush _selectedNodeBrush;
|
||||
private Brush _hoveredNodeBrush;
|
||||
private Brush _destinationNodeBrush;
|
||||
private Brush _gridBrush;
|
||||
private Brush _agvBrush;
|
||||
private Brush _pathBrush;
|
||||
@@ -113,6 +115,7 @@ namespace AGVNavigationCore.Controls
|
||||
private Pen _gridPen;
|
||||
private Pen _tempConnectionPen;
|
||||
private Pen _selectedNodePen;
|
||||
private Pen _destinationNodePen;
|
||||
private Pen _pathPen;
|
||||
private Pen _agvPen;
|
||||
|
||||
@@ -239,6 +242,7 @@ namespace AGVNavigationCore.Controls
|
||||
set
|
||||
{
|
||||
_currentPath = value;
|
||||
UpdateDestinationNode();
|
||||
Invalidate();
|
||||
}
|
||||
}
|
||||
@@ -324,6 +328,7 @@ namespace AGVNavigationCore.Controls
|
||||
_chargingNodeBrush = new SolidBrush(Color.Green);
|
||||
_selectedNodeBrush = new SolidBrush(Color.Red);
|
||||
_hoveredNodeBrush = new SolidBrush(Color.LightCyan);
|
||||
_destinationNodeBrush = new SolidBrush(Color.Gold);
|
||||
|
||||
// AGV 및 경로 브러쉬
|
||||
_agvBrush = new SolidBrush(Color.Red);
|
||||
@@ -339,6 +344,7 @@ namespace AGVNavigationCore.Controls
|
||||
_gridPen = new Pen(Color.LightGray, 1);
|
||||
_tempConnectionPen = new Pen(Color.Orange, 2) { DashStyle = DashStyle.Dash };
|
||||
_selectedNodePen = new Pen(Color.Red, 3);
|
||||
_destinationNodePen = new Pen(Color.Orange, 4);
|
||||
_pathPen = new Pen(Color.Purple, 3);
|
||||
_agvPen = new Pen(Color.Red, 3);
|
||||
}
|
||||
@@ -464,6 +470,20 @@ namespace AGVNavigationCore.Controls
|
||||
Invalidate();
|
||||
}
|
||||
|
||||
private void UpdateDestinationNode()
|
||||
{
|
||||
_destinationNode = null;
|
||||
|
||||
if (_currentPath != null && _currentPath.Success && _currentPath.Path != null && _currentPath.Path.Count > 0)
|
||||
{
|
||||
// 경로의 마지막 노드가 목적지
|
||||
string destinationNodeId = _currentPath.Path[_currentPath.Path.Count - 1];
|
||||
|
||||
// 노드 목록에서 해당 노드 찾기
|
||||
_destinationNode = _nodes?.FirstOrDefault(n => n.NodeId == destinationNodeId);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Cleanup
|
||||
@@ -480,6 +500,7 @@ namespace AGVNavigationCore.Controls
|
||||
_chargingNodeBrush?.Dispose();
|
||||
_selectedNodeBrush?.Dispose();
|
||||
_hoveredNodeBrush?.Dispose();
|
||||
_destinationNodeBrush?.Dispose();
|
||||
_gridBrush?.Dispose();
|
||||
_agvBrush?.Dispose();
|
||||
_pathBrush?.Dispose();
|
||||
@@ -489,6 +510,7 @@ namespace AGVNavigationCore.Controls
|
||||
_gridPen?.Dispose();
|
||||
_tempConnectionPen?.Dispose();
|
||||
_selectedNodePen?.Dispose();
|
||||
_destinationNodePen?.Dispose();
|
||||
_pathPen?.Dispose();
|
||||
_agvPen?.Dispose();
|
||||
|
||||
@@ -517,6 +539,12 @@ namespace AGVNavigationCore.Controls
|
||||
AgvDirection CurrentDirection { get; }
|
||||
AGVState CurrentState { get; }
|
||||
float BatteryLevel { get; }
|
||||
|
||||
// 이동 경로 정보 추가
|
||||
Point? TargetPosition { get; }
|
||||
string CurrentNodeId { get; }
|
||||
string TargetNodeId { get; }
|
||||
DockingDirection DockingDirection { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
@@ -24,6 +25,11 @@ namespace AGVNavigationCore.PathFinding
|
||||
/// </summary>
|
||||
public List<AgvDirection> Commands { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보 목록
|
||||
/// </summary>
|
||||
public List<NodeMotorInfo> NodeMotorInfos { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 총 거리
|
||||
/// </summary>
|
||||
@@ -57,6 +63,7 @@ namespace AGVNavigationCore.PathFinding
|
||||
Success = false;
|
||||
Path = new List<string>();
|
||||
Commands = new List<AgvDirection>();
|
||||
NodeMotorInfos = new List<NodeMotorInfo>();
|
||||
TotalDistance = 0;
|
||||
CalculationTimeMs = 0;
|
||||
EstimatedTimeSeconds = 0;
|
||||
@@ -87,6 +94,31 @@ namespace AGVNavigationCore.PathFinding
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 성공 결과 생성 (노드별 모터방향 정보 포함)
|
||||
/// </summary>
|
||||
/// <param name="path">경로</param>
|
||||
/// <param name="commands">AGV 명령어 목록</param>
|
||||
/// <param name="nodeMotorInfos">노드별 모터방향 정보</param>
|
||||
/// <param name="totalDistance">총 거리</param>
|
||||
/// <param name="calculationTimeMs">계산 시간</param>
|
||||
/// <returns>성공 결과</returns>
|
||||
public static AGVPathResult CreateSuccess(List<string> path, List<AgvDirection> commands, List<NodeMotorInfo> nodeMotorInfos, float totalDistance, long calculationTimeMs)
|
||||
{
|
||||
var result = new AGVPathResult
|
||||
{
|
||||
Success = true,
|
||||
Path = new List<string>(path),
|
||||
Commands = new List<AgvDirection>(commands),
|
||||
NodeMotorInfos = new List<NodeMotorInfo>(nodeMotorInfos),
|
||||
TotalDistance = totalDistance,
|
||||
CalculationTimeMs = calculationTimeMs
|
||||
};
|
||||
|
||||
result.CalculateMetrics();
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 실패 결과 생성
|
||||
/// </summary>
|
||||
|
||||
@@ -159,7 +159,8 @@ namespace AGVNavigationCore.PathFinding
|
||||
}
|
||||
|
||||
var agvCommands = GenerateAGVCommands(result.Path, targetDirection ?? AgvDirection.Forward);
|
||||
return AGVPathResult.CreateSuccess(result.Path, agvCommands, result.TotalDistance, stopwatch.ElapsedMilliseconds);
|
||||
var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
|
||||
return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -182,7 +183,8 @@ namespace AGVNavigationCore.PathFinding
|
||||
}
|
||||
|
||||
var agvCommands = GenerateAGVCommands(result.Path, actualTargetDirection);
|
||||
return AGVPathResult.CreateSuccess(result.Path, agvCommands, result.TotalDistance, stopwatch.ElapsedMilliseconds);
|
||||
var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
|
||||
return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@@ -264,6 +266,92 @@ namespace AGVNavigationCore.PathFinding
|
||||
return AgvDirection.Right;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보 생성
|
||||
/// </summary>
|
||||
/// <param name="path">경로 노드 목록</param>
|
||||
/// <returns>노드별 모터방향 정보 목록</returns>
|
||||
private List<NodeMotorInfo> GenerateNodeMotorInfos(List<string> path)
|
||||
{
|
||||
var nodeMotorInfos = new List<NodeMotorInfo>();
|
||||
if (path.Count < 2) return nodeMotorInfos;
|
||||
|
||||
for (int i = 0; i < path.Count; i++)
|
||||
{
|
||||
var currentNodeId = path[i];
|
||||
string nextNodeId = i < path.Count - 1 ? path[i + 1] : null;
|
||||
|
||||
AgvDirection motorDirection;
|
||||
|
||||
if (i == path.Count - 1)
|
||||
{
|
||||
// 마지막 노드: 도킹/충전 노드 타입에 따라 결정
|
||||
if (_nodeMap.ContainsKey(currentNodeId))
|
||||
{
|
||||
var currentNode = _nodeMap[currentNodeId];
|
||||
motorDirection = GetRequiredDirectionForNode(currentNode);
|
||||
}
|
||||
else
|
||||
{
|
||||
motorDirection = AgvDirection.Forward;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 중간 노드: 다음 노드와의 관계를 고려한 모터방향 결정
|
||||
motorDirection = CalculateMotorDirection(currentNodeId, nextNodeId);
|
||||
}
|
||||
|
||||
nodeMotorInfos.Add(new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId));
|
||||
}
|
||||
|
||||
return nodeMotorInfos;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산
|
||||
/// </summary>
|
||||
/// <param name="currentNodeId">현재 노드 ID</param>
|
||||
/// <param name="nextNodeId">다음 노드 ID</param>
|
||||
/// <returns>모터방향</returns>
|
||||
private AgvDirection CalculateMotorDirection(string currentNodeId, string nextNodeId)
|
||||
{
|
||||
if (!_nodeMap.ContainsKey(currentNodeId) || !_nodeMap.ContainsKey(nextNodeId))
|
||||
{
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
|
||||
var currentNode = _nodeMap[currentNodeId];
|
||||
var nextNode = _nodeMap[nextNodeId];
|
||||
|
||||
// 현재 노드와 다음 노드의 위치를 기반으로 이동 방향 계산
|
||||
var dx = nextNode.Position.X - currentNode.Position.X;
|
||||
var dy = nextNode.Position.Y - currentNode.Position.Y;
|
||||
var moveAngle = Math.Atan2(dy, dx);
|
||||
|
||||
// AGV의 구조: 리프트 ↔ AGV 몸체 ↔ 모니터
|
||||
// 전진: 모니터 방향으로 이동 (리프트에서 멀어짐)
|
||||
// 후진: 리프트 방향으로 이동 (리프트에 가까워짐)
|
||||
|
||||
// 다음 노드가 특수 노드인지 확인
|
||||
if (nextNode.Type == NodeType.Charging)
|
||||
{
|
||||
// 충전기: 전진으로 도킹
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
else if (nextNode.Type == NodeType.Docking)
|
||||
{
|
||||
// 도킹 스테이션: 후진으로 도킹
|
||||
return AgvDirection.Backward;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 일반 이동: 기본적으로 전진
|
||||
// 향후 경로 패턴 분석을 통해 더 정확한 방향 결정 가능
|
||||
return AgvDirection.Forward;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 유효성 검증
|
||||
/// </summary>
|
||||
|
||||
32
Cs_HMI/AGVNavigationCore/PathFinding/NodeMotorInfo.cs
Normal file
32
Cs_HMI/AGVNavigationCore/PathFinding/NodeMotorInfo.cs
Normal file
@@ -0,0 +1,32 @@
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding
|
||||
{
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보
|
||||
/// </summary>
|
||||
public class NodeMotorInfo
|
||||
{
|
||||
/// <summary>
|
||||
/// 노드 ID
|
||||
/// </summary>
|
||||
public string NodeId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 해당 노드에서의 모터방향
|
||||
/// </summary>
|
||||
public AgvDirection MotorDirection { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 다음 노드 ID (경로예측용)
|
||||
/// </summary>
|
||||
public string NextNodeId { get; set; }
|
||||
|
||||
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId = null)
|
||||
{
|
||||
NodeId = nodeId;
|
||||
MotorDirection = motorDirection;
|
||||
NextNodeId = nextNodeId;
|
||||
}
|
||||
}
|
||||
}
|
||||
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