Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

refactor: Move AGV development projects to separate AGVLogic folder

Browse Source 
- Reorganized AGVMapEditor, AGVNavigationCore, AGVSimulator into AGVLogic folder
- Removed deleted project files from root folder tracking
- Updated CLAUDE.md with AGVLogic-specific development guidelines
- Clean separation of independent project development from main codebase
- Projects now ready for independent development and future integration

🤖 Generated with Claude Code

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
2025-10-23 10:00:40 +09:00
parent ce78752c2c
commit dbaf647d4e
63 changed files with 1398 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
Cs_HMI/AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj Normal file
View File

@@ -0,0 +1,105 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
<PropertyGroup>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
<ProjectGuid>{C5F7A8B2-8D3E-4A1B-9C6E-7F4D5E2A9B1C}</ProjectGuid>
<OutputType>Library</OutputType>
<AppDesignerFolder>Properties</AppDesignerFolder>
<RootNamespace>AGVNavigationCore</RootNamespace>
<AssemblyName>AGVNavigationCore</AssemblyName>
<TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
<FileAlignment>512</FileAlignment>
<Deterministic>true</Deterministic>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<OutputPath>bin\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x86'">
<DebugSymbols>true</DebugSymbols>
<OutputPath>bin\x86\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants>
<DebugType>full</DebugType>
<PlatformTarget>x86</PlatformTarget>
<ErrorReport>prompt</ErrorReport>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x86'">
<OutputPath>bin\x86\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants>
<Optimize>true</Optimize>
<DebugType>pdbonly</DebugType>
<PlatformTarget>x86</PlatformTarget>
<ErrorReport>prompt</ErrorReport>
</PropertyGroup>
<ItemGroup>
<Reference Include="System" />
<Reference Include="System.Core" />
<Reference Include="System.Drawing" />
<Reference Include="System.Windows.Forms" />
<Reference Include="System.Xml.Linq" />
<Reference Include="System.Data.DataSetExtensions" />
<Reference Include="Microsoft.CSharp" />
<Reference Include="System.Data" />
<Reference Include="System.Net.Http" />
<Reference Include="System.Xml" />
<Reference Include="Newtonsoft.Json, Version=13.0.0.0, Culture=neutral, PublicKeyToken=30ad4fe6b2a6aeed, processorArchitecture=MSIL">
<HintPath>..\packages\Newtonsoft.Json.13.0.3\lib\net45\Newtonsoft.Json.dll</HintPath>
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="Controls\AGVState.cs" />
<Compile Include="Controls\IAGV.cs" />
<Compile Include="Controls\UnifiedAGVCanvas.Events.cs">
<DependentUpon>UnifiedAGVCanvas.cs</DependentUpon>
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Controls\UnifiedAGVCanvas.Mouse.cs">
<DependentUpon>UnifiedAGVCanvas.cs</DependentUpon>
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Models\Enums.cs" />
<Compile Include="Models\IMovableAGV.cs" />
<Compile Include="Models\VirtualAGV.cs" />
<Compile Include="Models\MapLoader.cs" />
<Compile Include="Models\MapNode.cs" />
<Compile Include="PathFinding\Planning\AGVPathfinder.cs" />
<Compile Include="PathFinding\Planning\DirectionChangePlanner.cs" />
<Compile Include="PathFinding\Validation\DockingValidationResult.cs" />
<Compile Include="PathFinding\Validation\PathValidationResult.cs" />
<Compile Include="PathFinding\Analysis\JunctionAnalyzer.cs" />
<Compile Include="PathFinding\Core\PathNode.cs" />
<Compile Include="PathFinding\Core\AStarPathfinder.cs" />
<Compile Include="PathFinding\Core\AGVPathResult.cs" />
<Compile Include="PathFinding\Planning\NodeMotorInfo.cs" />
<Compile Include="Controls\UnifiedAGVCanvas.cs">
<SubType>UserControl</SubType>
</Compile>
<Compile Include="Controls\UnifiedAGVCanvas.Designer.cs">
<DependentUpon>UnifiedAGVCanvas.cs</DependentUpon>
</Compile>
<Compile Include="Utils\DockingValidator.cs" />
<Compile Include="Utils\LiftCalculator.cs" />
<Compile Include="Properties\AssemblyInfo.cs" />
</ItemGroup>
<ItemGroup>
<None Include="build.bat" />
<None Include="packages.config" />
<None Include="README.md" />
</ItemGroup>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
</Project>

21
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/AGVState.cs Normal file
View File

@@ -0,0 +1,21 @@
namespace AGVNavigationCore.Controls
{
#region Interfaces
/// <summary>
/// AGV 상태 열거형
/// </summary>
public enum AGVState
{
Idle, // 대기
Moving, // 이동 중
Rotating, // 회전 중
Docking, // 도킹 중
Charging, // 충전 중
Error // 오류
}
#endregion
}

30
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/IAGV.cs Normal file
View File

@@ -0,0 +1,30 @@
using System.Drawing;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.Controls
{
#region Interfaces
/// <summary>
/// AGV 인터페이스 (가상/실제 AGV 통합)
/// </summary>
public interface IAGV
{
string AgvId { get; }
Point CurrentPosition { get; set; }
AgvDirection CurrentDirection { get; set; }
AGVState CurrentState { get; set; }
float BatteryLevel { get; }
// 이동 경로 정보 추가
Point? TargetPosition { get; }
string CurrentNodeId { get; }
string TargetNodeId { get; }
DockingDirection DockingDirection { get; }
}
#endregion
}

41
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Designer.cs generated Normal file
View File

@@ -0,0 +1,41 @@
namespace AGVNavigationCore.Controls
{
partial class UnifiedAGVCanvas
{
/// <summary>
/// 필수 디자이너 변수입니다.
/// </summary>
private System.ComponentModel.IContainer components = null;
#region 구성 요소 디자이너에서 생성한 코드
/// <summary>
/// 디자이너 지원에 필요한 메서드입니다.
/// 이 메서드의 내용을 코드 편집기로 수정하지 마세요.
/// </summary>
private void InitializeComponent()
{
this.SuspendLayout();
//
// UnifiedAGVCanvas
//
this.AutoScaleDimensions = new System.Drawing.SizeF(7F, 12F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.BackColor = System.Drawing.Color.White;
this.Name = "UnifiedAGVCanvas";
this.Size = new System.Drawing.Size(800, 600);
this.Paint += new System.Windows.Forms.PaintEventHandler(this.UnifiedAGVCanvas_Paint);
this.MouseClick += new System.Windows.Forms.MouseEventHandler(this.UnifiedAGVCanvas_MouseClick);
this.MouseDoubleClick += new System.Windows.Forms.MouseEventHandler(this.UnifiedAGVCanvas_MouseDoubleClick);
this.MouseDown += new System.Windows.Forms.MouseEventHandler(this.UnifiedAGVCanvas_MouseDown);
this.MouseMove += new System.Windows.Forms.MouseEventHandler(this.UnifiedAGVCanvas_MouseMove);
this.MouseUp += new System.Windows.Forms.MouseEventHandler(this.UnifiedAGVCanvas_MouseUp);
this.MouseWheel += new System.Windows.Forms.MouseEventHandler(this.UnifiedAGVCanvas_MouseWheel);
this.ResumeLayout(false);
}
#endregion
}
}

1646
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs Normal file
View File

File diff suppressed because it is too large Load Diff

789
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Mouse.cs Normal file
View File

@@ -0,0 +1,789 @@
using System;
using System.Drawing;
using System.Linq;
using System.Windows.Forms;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.Controls
{
public partial class UnifiedAGVCanvas
{
#region Mouse Events
private void UnifiedAGVCanvas_MouseClick(object sender, MouseEventArgs e)
{
Focus(); // 포커스 설정
var worldPoint = ScreenToWorld(e.Location);
var hitNode = GetNodeAt(worldPoint);
switch (_editMode)
{
case EditMode.Select:
HandleSelectClick(hitNode, worldPoint);
break;
case EditMode.AddNode:
HandleAddNodeClick(worldPoint);
break;
case EditMode.Connect:
HandleConnectClick(hitNode);
break;
case EditMode.Delete:
HandleDeleteClick(hitNode);
break;
case EditMode.DeleteConnection:
HandleDeleteConnectionClick(worldPoint);
break;
}
}
private void UnifiedAGVCanvas_MouseDoubleClick(object sender, MouseEventArgs e)
{
var worldPoint = ScreenToWorld(e.Location);
var hitNode = GetNodeAt(worldPoint);
if (hitNode != null)
{
// 노드 속성 편집 (이벤트 발생)
NodeSelected?.Invoke(this, hitNode);
}
}
private void UnifiedAGVCanvas_MouseDown(object sender, MouseEventArgs e)
{
var worldPoint = ScreenToWorld(e.Location);
if (e.Button == MouseButtons.Left)
{
if (_editMode == EditMode.Move)
{
var hitNode = GetNodeAt(worldPoint);
if (hitNode != null)
{
_isDragging = true;
_selectedNode = hitNode;
_dragOffset = new Point(
worldPoint.X - hitNode.Position.X,
worldPoint.Y - hitNode.Position.Y
);
Cursor = Cursors.SizeAll;
Invalidate();
return;
}
}
// 팬 시작 (우클릭 또는 중간버튼)
_isPanning = true;
_lastMousePosition = e.Location;
Cursor = Cursors.SizeAll;
}
else if (e.Button == MouseButtons.Right)
{
// 컨텍스트 메뉴 (편집 모드에서만)
if (_canvasMode == CanvasMode.Edit)
{
var hitNode = GetNodeAt(worldPoint);
ShowContextMenu(e.Location, hitNode);
}
}
}
private void UnifiedAGVCanvas_MouseMove(object sender, MouseEventArgs e)
{
var worldPoint = ScreenToWorld(e.Location);
// 호버 노드 업데이트
var newHoveredNode = GetNodeAt(worldPoint);
if (newHoveredNode != _hoveredNode)
{
_hoveredNode = newHoveredNode;
Invalidate();
}
if (_isPanning)
{
// 팬 처리
var deltaX = e.X - _lastMousePosition.X;
var deltaY = e.Y - _lastMousePosition.Y;
_panOffset.X += deltaX;
_panOffset.Y += deltaY;
_lastMousePosition = e.Location;
Invalidate();
}
else if (_isDragging && _canvasMode == CanvasMode.Edit)
{
// 노드 드래그
if (_selectedNode != null)
{
var newPosition = new Point(
worldPoint.X - _dragOffset.X,
worldPoint.Y - _dragOffset.Y
);
// 그리드 스냅
if (ModifierKeys.HasFlag(Keys.Control))
{
newPosition.X = (newPosition.X / GRID_SIZE) * GRID_SIZE;
newPosition.Y = (newPosition.Y / GRID_SIZE) * GRID_SIZE;
}
_selectedNode.Position = newPosition;
NodeMoved?.Invoke(this, _selectedNode);
MapChanged?.Invoke(this, EventArgs.Empty);
Invalidate();
}
}
else if (_isConnectionMode && _canvasMode == CanvasMode.Edit)
{
// 임시 연결선 업데이트
_connectionEndPoint = worldPoint;
Invalidate();
}
// 툴팁 표시 (호버된 노드/AGV 정보)
UpdateTooltip(worldPoint);
}
private void UnifiedAGVCanvas_MouseUp(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Left)
{
if (_isDragging && _canvasMode == CanvasMode.Edit)
{
_isDragging = false;
Cursor = GetCursorForMode(_editMode);
}
if (_isPanning)
{
_isPanning = false;
Cursor = Cursors.Default;
}
}
}
private void UnifiedAGVCanvas_MouseWheel(object sender, MouseEventArgs e)
{
// 현재 마우스 위치를 월드 좌표로 변환 (줌 전)
var mouseWorldBefore = ScreenToWorld(e.Location);
float oldZoom = _zoomFactor;
// 줌 팩터 계산 (휠 델타 기반) - 더 부드러운 줌
if (e.Delta > 0)
_zoomFactor = Math.Min(_zoomFactor * 1.15f, 5.0f); // 확대 (더 부드러움)
else
_zoomFactor = Math.Max(_zoomFactor / 1.15f, 0.1f); // 축소 (더 부드러움)
// 줌 후 마우스 위치의 월드 좌표
var mouseWorldAfter = ScreenToWorld(e.Location);
// 마우스 위치가 같은 월드 좌표를 가리키도록 팬 오프셋 조정
_panOffset.X += (int)((mouseWorldBefore.X - mouseWorldAfter.X) * _zoomFactor);
_panOffset.Y += (int)((mouseWorldBefore.Y - mouseWorldAfter.Y) * _zoomFactor);
Invalidate();
}
#endregion
#region Mouse Helper Methods
private Point ScreenToWorld(Point screenPoint)
{
// 변환 행렬 생성 (렌더링과 동일)
var transform = new System.Drawing.Drawing2D.Matrix();
transform.Scale(_zoomFactor, _zoomFactor);
transform.Translate(_panOffset.X, _panOffset.Y);
// 역변환 행렬로 화면 좌표를 월드 좌표로 변환
transform.Invert();
var points = new System.Drawing.PointF[] { new System.Drawing.PointF(screenPoint.X, screenPoint.Y) };
transform.TransformPoints(points);
return new Point((int)points[0].X, (int)points[0].Y);
}
private Point WorldToScreen(Point worldPoint)
{
return new Point(
(int)(worldPoint.X * _zoomFactor + _panOffset.X),
(int)(worldPoint.Y * _zoomFactor + _panOffset.Y)
);
}
private MapNode GetNodeAt(Point worldPoint)
{
if (_nodes == null) return null;
// 역순으로 검사하여 위에 그려진 노드부터 확인
for (int i = _nodes.Count - 1; i >= 0; i--)
{
var node = _nodes[i];
if (IsPointInNode(worldPoint, node))
return node;
}
return null;
}
private bool IsPointInNode(Point point, MapNode node)
{
switch (node.Type)
{
case NodeType.Label:
return IsPointInLabelNode(point, node);
case NodeType.Image:
return IsPointInImageNode(point, node);
default:
return IsPointInCircularNode(point, node);
}
}
private bool IsPointInCircularNode(Point point, MapNode node)
{
switch (node.Type)
{
case NodeType.Docking:
return IsPointInPentagon(point, node);
case NodeType.Charging:
return IsPointInTriangle(point, node);
default:
return IsPointInCircle(point, node);
}
}
private bool IsPointInCircle(Point point, MapNode node)
{
// 화면에서 최소 20픽셀 정도의 히트 영역을 확보하되, 노드 크기보다 작아지지 않게 함
var minHitRadiusInScreen = 20;
var hitRadius = Math.Max(NODE_RADIUS, minHitRadiusInScreen / _zoomFactor);
var distance = Math.Sqrt(
Math.Pow(node.Position.X - point.X, 2) +
Math.Pow(node.Position.Y - point.Y, 2)
);
return distance <= hitRadius;
}
private bool IsPointInPentagon(Point point, MapNode node)
{
// 화면에서 최소 20픽셀 정도의 히트 영역을 확보
var minHitRadiusInScreen = 20;
var radius = Math.Max(NODE_RADIUS, minHitRadiusInScreen / _zoomFactor);
var center = node.Position;
// 5각형 꼭짓점 계산
var points = new Point[5];
for (int i = 0; i < 5; i++)
{
var angle = (Math.PI * 2 * i / 5) - Math.PI / 2;
points[i] = new Point(
(int)(center.X + radius * Math.Cos(angle)),
(int)(center.Y + radius * Math.Sin(angle))
);
}
return IsPointInPolygon(point, points);
}
private bool IsPointInTriangle(Point point, MapNode node)
{
// 화면에서 최소 20픽셀 정도의 히트 영역을 확보하되, 노드 크기보다 작아지지 않게 함
var minHitRadiusInScreen = 20;
var radius = Math.Max(NODE_RADIUS, minHitRadiusInScreen / _zoomFactor);
var center = node.Position;
// 삼각형 꼭짓점 계산
var points = new Point[3];
for (int i = 0; i < 3; i++)
{
var angle = (Math.PI * 2 * i / 3) - Math.PI / 2;
points[i] = new Point(
(int)(center.X + radius * Math.Cos(angle)),
(int)(center.Y + radius * Math.Sin(angle))
);
}
return IsPointInPolygon(point, points);
}
private bool IsPointInPolygon(Point point, Point[] polygon)
{
// Ray casting 알고리즘 사용
bool inside = false;
int j = polygon.Length - 1;
for (int i = 0; i < polygon.Length; i++)
{
var xi = polygon[i].X;
var yi = polygon[i].Y;
var xj = polygon[j].X;
var yj = polygon[j].Y;
if (((yi > point.Y) != (yj > point.Y)) &&
(point.X < (xj - xi) * (point.Y - yi) / (yj - yi) + xi))
{
inside = !inside;
}
j = i;
}
return inside;
}
private bool IsPointInLabelNode(Point point, MapNode node)
{
var text = string.IsNullOrEmpty(node.LabelText) ? node.NodeId : node.LabelText;
// 임시 Graphics로 텍스트 크기 측정
using (var tempBitmap = new Bitmap(1, 1))
using (var tempGraphics = Graphics.FromImage(tempBitmap))
{
var font = new Font(node.FontFamily, node.FontSize, node.FontStyle);
var textSize = tempGraphics.MeasureString(text, font);
var textRect = new Rectangle(
(int)(node.Position.X - textSize.Width / 2),
(int)(node.Position.Y - textSize.Height / 2),
(int)textSize.Width,
(int)textSize.Height
);
font.Dispose();
return textRect.Contains(point);
}
}
private bool IsPointInImageNode(Point point, MapNode node)
{
var displaySize = node.GetDisplaySize();
if (displaySize.IsEmpty)
displaySize = new Size(50, 50); // 기본 크기
var imageRect = new Rectangle(
node.Position.X - displaySize.Width / 2,
node.Position.Y - displaySize.Height / 2,
displaySize.Width,
displaySize.Height
);
return imageRect.Contains(point);
}
private IAGV GetAGVAt(Point worldPoint)
{
if (_agvList == null) return null;
var hitRadius = Math.Max(AGV_SIZE / 2, 15 / _zoomFactor);
return _agvList.FirstOrDefault(agv =>
{
if (!_agvPositions.ContainsKey(agv.AgvId)) return false;
var agvPos = _agvPositions[agv.AgvId];
var distance = Math.Sqrt(
Math.Pow(agvPos.X - worldPoint.X, 2) +
Math.Pow(agvPos.Y - worldPoint.Y, 2)
);
return distance <= hitRadius;
});
}
private void HandleSelectClick(MapNode hitNode, Point worldPoint)
{
if (hitNode != null)
{
// 노드 선택
if (hitNode != _selectedNode)
{
_selectedNode = hitNode;
NodeSelected?.Invoke(this, hitNode);
Invalidate();
}
}
else
{
// 노드가 없으면 연결선 체크
var connection = GetConnectionAt(worldPoint);
if (connection != null)
{
// 연결선을 클릭했을 때 삭제 확인
var (fromNode, toNode) = connection.Value;
string fromDisplay = !string.IsNullOrEmpty(fromNode.RfidId) ? fromNode.RfidId : fromNode.NodeId;
string toDisplay = !string.IsNullOrEmpty(toNode.RfidId) ? toNode.RfidId : toNode.NodeId;
var result = MessageBox.Show(
$"연결을 삭제하시겠습니까?\n\n{fromDisplay} ↔ {toDisplay}",
"연결 삭제 확인",
MessageBoxButtons.YesNo,
MessageBoxIcon.Question);
if (result == DialogResult.Yes)
{
// 단일 연결 삭제 (어느 방향에 저장되어 있는지 확인 후 삭제)
if (fromNode.ConnectedNodes.Contains(toNode.NodeId))
{
fromNode.RemoveConnection(toNode.NodeId);
}
else if (toNode.ConnectedNodes.Contains(fromNode.NodeId))
{
toNode.RemoveConnection(fromNode.NodeId);
}
// 이벤트 발생
ConnectionDeleted?.Invoke(this, (fromNode, toNode));
MapChanged?.Invoke(this, EventArgs.Empty);
Invalidate();
}
}
else
{
// 빈 공간 클릭 시 선택 해제
if (_selectedNode != null)
{
_selectedNode = null;
NodeSelected?.Invoke(this, null);
Invalidate();
}
}
}
}
private void HandleAddNodeClick(Point worldPoint)
{
// 그리드 스냅
if (ModifierKeys.HasFlag(Keys.Control))
{
worldPoint.X = (worldPoint.X / GRID_SIZE) * GRID_SIZE;
worldPoint.Y = (worldPoint.Y / GRID_SIZE) * GRID_SIZE;
}
// 고유한 NodeId 생성
string newNodeId = GenerateUniqueNodeId();
var newNode = new MapNode
{
NodeId = newNodeId,
Position = worldPoint,
Type = NodeType.Normal
};
_nodes.Add(newNode);
NodeAdded?.Invoke(this, newNode);
MapChanged?.Invoke(this, EventArgs.Empty);
Invalidate();
}
/// <summary>
/// 중복되지 않는 고유한 NodeId 생성
/// </summary>
private string GenerateUniqueNodeId()
{
string nodeId;
int counter = _nodeCounter;
do
{
nodeId = $"N{counter:D3}";
counter++;
}
while (_nodes.Any(n => n.NodeId == nodeId));
_nodeCounter = counter;
return nodeId;
}
private void HandleConnectClick(MapNode hitNode)
{
if (hitNode == null) return;
if (!_isConnectionMode)
{
// 연결 시작
_isConnectionMode = true;
_connectionStartNode = hitNode;
_selectedNode = hitNode;
}
else
{
// 연결 완료
if (_connectionStartNode != null && _connectionStartNode != hitNode)
{
CreateConnection(_connectionStartNode, hitNode);
}
CancelConnection();
}
Invalidate();
}
private void HandleDeleteClick(MapNode hitNode)
{
if (hitNode == null) return;
// 연결된 모든 연결선도 제거
foreach (var node in _nodes)
{
node.RemoveConnection(hitNode.NodeId);
}
_nodes.Remove(hitNode);
if (_selectedNode == hitNode)
_selectedNode = null;
NodeDeleted?.Invoke(this, hitNode);
MapChanged?.Invoke(this, EventArgs.Empty);
Invalidate();
}
private void CreateConnection(MapNode fromNode, MapNode toNode)
{
// 중복 연결 체크 (양방향)
if (fromNode.ConnectedNodes.Contains(toNode.NodeId) ||
toNode.ConnectedNodes.Contains(fromNode.NodeId))
return;
// 단일 연결 생성 (사전순으로 정렬하여 일관성 유지)
if (string.Compare(fromNode.NodeId, toNode.NodeId, StringComparison.Ordinal) < 0)
{
fromNode.AddConnection(toNode.NodeId);
}
else
{
toNode.AddConnection(fromNode.NodeId);
}
MapChanged?.Invoke(this, EventArgs.Empty);
}
private float CalculateDistance(Point from, Point to)
{
var dx = to.X - from.X;
var dy = to.Y - from.Y;
return (float)Math.Sqrt(dx * dx + dy * dy);
}
private void ShowContextMenu(Point location, MapNode hitNode)
{
_contextMenu.Items.Clear();
if (hitNode != null)
{
_contextMenu.Items.Add("노드 속성...", null, (s, e) => NodeSelected?.Invoke(this, hitNode));
_contextMenu.Items.Add("노드 삭제", null, (s, e) => HandleDeleteClick(hitNode));
_contextMenu.Items.Add("-");
}
_contextMenu.Items.Add("노드 추가", null, (s, e) =>
{
var worldPoint = ScreenToWorld(location);
HandleAddNodeClick(worldPoint);
});
_contextMenu.Items.Add("전체 맞춤", null, (s, e) => FitToNodes());
_contextMenu.Items.Add("줌 리셋", null, (s, e) => ResetZoom());
_contextMenu.Show(this, location);
}
private void HandleDeleteConnectionClick(Point worldPoint)
{
// 클릭한 위치 근처의 연결선을 찾아 삭제
var connection = GetConnectionAt(worldPoint);
if (connection != null)
{
var (fromNode, toNode) = connection.Value;
// 단일 연결 삭제 (어느 방향에 저장되어 있는지 확인 후 삭제)
if (fromNode.ConnectedNodes.Contains(toNode.NodeId))
{
fromNode.RemoveConnection(toNode.NodeId);
}
else if (toNode.ConnectedNodes.Contains(fromNode.NodeId))
{
toNode.RemoveConnection(fromNode.NodeId);
}
// 이벤트 발생
ConnectionDeleted?.Invoke(this, (fromNode, toNode));
MapChanged?.Invoke(this, EventArgs.Empty);
Invalidate();
}
}
private (MapNode From, MapNode To)? GetConnectionAt(Point worldPoint)
{
const int CONNECTION_HIT_TOLERANCE = 10;
// 모든 연결선을 확인하여 클릭한 위치와 가장 가까운 연결선 찾기
foreach (var fromNode in _nodes)
{
foreach (var toNodeId in fromNode.ConnectedNodes)
{
var toNode = _nodes.FirstOrDefault(n => n.NodeId == toNodeId);
if (toNode != null)
{
// 연결선과 클릭 위치 간의 거리 계산
var distance = CalculatePointToLineDistance(worldPoint, fromNode.Position, toNode.Position);
if (distance <= CONNECTION_HIT_TOLERANCE / _zoomFactor)
{
return (fromNode, toNode);
}
}
}
}
return null;
}
private float CalculatePointToLineDistance(Point point, Point lineStart, Point lineEnd)
{
// 점에서 선분까지의 거리 계산
var A = point.X - lineStart.X;
var B = point.Y - lineStart.Y;
var C = lineEnd.X - lineStart.X;
var D = lineEnd.Y - lineStart.Y;
var dot = A * C + B * D;
var lenSq = C * C + D * D;
if (lenSq == 0) return CalculateDistance(point, lineStart);
var param = dot / lenSq;
Point xx, yy;
if (param < 0)
{
xx = lineStart;
yy = lineStart;
}
else if (param > 1)
{
xx = lineEnd;
yy = lineEnd;
}
else
{
xx = new Point((int)(lineStart.X + param * C), (int)(lineStart.Y + param * D));
yy = xx;
}
return CalculateDistance(point, xx);
}
private void UpdateTooltip(Point worldPoint)
{
string tooltipText = "";
// 노드 툴팁
var hitNode = GetNodeAt(worldPoint);
if (hitNode != null)
{
tooltipText = $"노드: {hitNode.NodeId}\n타입: {hitNode.Type}\n위치: ({hitNode.Position.X}, {hitNode.Position.Y})";
}
else
{
// AGV 툴팁
var hitAGV = GetAGVAt(worldPoint);
if (hitAGV != null)
{
var state = _agvStates.ContainsKey(hitAGV.AgvId) ? _agvStates[hitAGV.AgvId] : AGVState.Idle;
tooltipText = $"AGV: {hitAGV.AgvId}\n상태: {state}\n배터리: {hitAGV.BatteryLevel:F1}%\n위치: ({hitAGV.CurrentPosition.X}, {hitAGV.CurrentPosition.Y})";
}
}
// 툴팁 업데이트 (기존 ToolTip 컨트롤 사용)
if (!string.IsNullOrEmpty(tooltipText))
{
// ToolTip 설정 (필요시 추가 구현)
}
}
#endregion
#region View Control Methods
/// <summary>
/// 모든 노드가 보이도록 뷰 조정
/// </summary>
public void FitToNodes()
{
if (_nodes == null || _nodes.Count == 0) return;
var minX = _nodes.Min(n => n.Position.X);
var maxX = _nodes.Max(n => n.Position.X);
var minY = _nodes.Min(n => n.Position.Y);
var maxY = _nodes.Max(n => n.Position.Y);
var margin = 50;
var contentWidth = maxX - minX + margin * 2;
var contentHeight = maxY - minY + margin * 2;
var zoomX = (float)Width / contentWidth;
var zoomY = (float)Height / contentHeight;
_zoomFactor = Math.Min(zoomX, zoomY) * 0.9f;
var centerX = (minX + maxX) / 2;
var centerY = (minY + maxY) / 2;
_panOffset.X = (int)(Width / 2 - centerX * _zoomFactor);
_panOffset.Y = (int)(Height / 2 - centerY * _zoomFactor);
Invalidate();
}
/// <summary>
/// 줌 리셋
/// </summary>
public void ResetZoom()
{
_zoomFactor = 1.0f;
_panOffset = Point.Empty;
Invalidate();
}
/// <summary>
/// 특정 위치로 이동
/// </summary>
public void PanTo(Point worldPoint)
{
_panOffset.X = (int)(Width / 2 - worldPoint.X * _zoomFactor);
_panOffset.Y = (int)(Height / 2 - worldPoint.Y * _zoomFactor);
Invalidate();
}
/// <summary>
/// 특정 노드로 이동
/// </summary>
public void PanToNode(string nodeId)
{
var node = _nodes?.FirstOrDefault(n => n.NodeId == nodeId);
if (node != null)
{
PanTo(node.Position);
}
}
/// <summary>
/// 특정 AGV로 이동
/// </summary>
public void PanToAGV(string agvId)
{
if (_agvPositions.ContainsKey(agvId))
{
PanTo(_agvPositions[agvId]);
}
}
#endregion
}
}

691
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs Normal file
View File

@@ -0,0 +1,691 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Windows.Forms;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding;
using AGVNavigationCore.PathFinding.Core;
namespace AGVNavigationCore.Controls
{
/// <summary>
/// 통합 AGV 캔버스 컨트롤
/// 맵 편집, AGV 시뮬레이션, 실시간 모니터링을 모두 지원
/// </summary>
public partial class UnifiedAGVCanvas : UserControl
{
#region Constants
private const int NODE_SIZE = 24;
private const int NODE_RADIUS = NODE_SIZE / 2;
private const int GRID_SIZE = 20;
private const float CONNECTION_WIDTH = 2.0f;
private const int SNAP_DISTANCE = 10;
private const int AGV_SIZE = 40;
private const int CONNECTION_ARROW_SIZE = 8;
#endregion
#region Enums
/// <summary>
/// 캔버스 모드
/// </summary>
public enum CanvasMode
{
Edit // 편집 가능 (맵 에디터)
}
/// <summary>
/// 편집 모드 (CanvasMode.Edit일 때만 적용)
/// </summary>
public enum EditMode
{
Select, // 선택 모드
Move, // 이동 모드
AddNode, // 노드 추가 모드
Connect, // 연결 모드
Delete, // 삭제 모드
DeleteConnection, // 연결 삭제 모드
AddLabel, // 라벨 추가 모드
AddImage, // 이미지 추가 모드
}
#endregion
#region Fields
// 캔버스 모드
private CanvasMode _canvasMode = CanvasMode.Edit;
private EditMode _editMode = EditMode.Select;
// 맵 데이터
private List<MapNode> _nodes;
private MapNode _selectedNode;
private MapNode _hoveredNode;
private MapNode _destinationNode;
// AGV 관련
private List<IAGV> _agvList;
private Dictionary<string, Point> _agvPositions;
private Dictionary<string, AgvDirection> _agvDirections;
private Dictionary<string, AGVState> _agvStates;
// 경로 관련
private AGVPathResult _currentPath;
private List<AGVPathResult> _allPaths;
// 도킹 검증 관련
private Dictionary<string, bool> _dockingErrors;
// UI 요소들
private Image _companyLogo;
private string _companyLogoPath = string.Empty;
private string _measurementInfo = "스케일: 1:100\n면적: 1000㎡\n최종 수정: " + DateTime.Now.ToString("yyyy-MM-dd");
// 편집 관련 (EditMode에서만 사용)
private bool _isDragging;
private Point _dragOffset;
private Point _lastMousePosition;
private bool _isConnectionMode;
private MapNode _connectionStartNode;
private Point _connectionEndPoint;
// 그리드 및 줌 관련
private bool _showGrid = true;
private float _zoomFactor = 1.0f;
private Point _panOffset = Point.Empty;
private bool _isPanning;
// 자동 증가 카운터
private int _nodeCounter = 1;
// 강조 연결
private (string FromNodeId, string ToNodeId)? _highlightedConnection = null;
// RFID 중복 검사
private HashSet<string> _duplicateRfidNodes = new HashSet<string>();
// 브러쉬 및 펜
private Brush _normalNodeBrush;
private Brush _rotationNodeBrush;
private Brush _dockingNodeBrush;
private Brush _chargingNodeBrush;
private Brush _selectedNodeBrush;
private Brush _hoveredNodeBrush;
private Brush _destinationNodeBrush;
private Brush _gridBrush;
private Brush _agvBrush;
private Brush _pathBrush;
private Pen _connectionPen;
private Pen _gridPen;
private Pen _tempConnectionPen;
private Pen _selectedNodePen;
private Pen _destinationNodePen;
private Pen _pathPen;
private Pen _agvPen;
private Pen _highlightedConnectionPen;
// 컨텍스트 메뉴
private ContextMenuStrip _contextMenu;
#endregion
#region Events
// 맵 편집 이벤트
public event EventHandler<MapNode> NodeAdded;
public event EventHandler<MapNode> NodeSelected;
public event EventHandler<MapNode> NodeDeleted;
public event EventHandler<MapNode> NodeMoved;
public event EventHandler<(MapNode From, MapNode To)> ConnectionDeleted;
public event EventHandler MapChanged;
// AGV 이벤트
public event EventHandler<IAGV> AGVSelected;
public event EventHandler<IAGV> AGVStateChanged;
#endregion
#region Properties
/// <summary>
/// 캔버스 모드
/// </summary>
public CanvasMode Mode
{
get => _canvasMode;
set
{
_canvasMode = value;
UpdateModeUI();
Invalidate();
}
}
/// <summary>
/// 편집 모드 (CanvasMode.Edit일 때만 적용)
/// </summary>
public EditMode CurrentEditMode
{
get => _editMode;
set
{
if (_canvasMode != CanvasMode.Edit) return;
_editMode = value;
if (_editMode != EditMode.Connect)
{
CancelConnection();
}
Cursor = GetCursorForMode(_editMode);
Invalidate();
}
}
/// <summary>
/// 그리드 표시 여부
/// </summary>
public bool ShowGrid
{
get => _showGrid;
set
{
_showGrid = value;
Invalidate();
}
}
/// <summary>
/// 줌 팩터
/// </summary>
public float ZoomFactor
{
get => _zoomFactor;
set
{
_zoomFactor = Math.Max(0.1f, Math.Min(5.0f, value));
Invalidate();
}
}
/// <summary>
/// 선택된 노드
/// </summary>
public MapNode SelectedNode => _selectedNode;
/// <summary>
/// 노드 목록
/// </summary>
public List<MapNode> Nodes
{
get => _nodes ?? new List<MapNode>();
set
{
_nodes = value ?? new List<MapNode>();
// 기존 노드들의 최대 번호를 찾아서 _nodeCounter 설정
UpdateNodeCounter();
// RFID 중복값 검사
DetectDuplicateRfidNodes();
Invalidate();
}
}
/// <summary>
/// AGV 목록
/// </summary>
public List<IAGV> AGVList
{
get => _agvList ?? new List<IAGV>();
set
{
_agvList = value ?? new List<IAGV>();
UpdateAGVData();
Invalidate();
}
}
/// <summary>
/// 현재 표시할 경로
/// </summary>
public AGVPathResult CurrentPath
{
get => _currentPath;
set
{
_currentPath = value;
UpdateDestinationNode();
Invalidate();
}
}
/// <summary>
/// 모든 경로 목록 (다중 AGV 경로 표시용)
/// </summary>
public List<AGVPathResult> AllPaths
{
get => _allPaths ?? new List<AGVPathResult>();
set
{
_allPaths = value ?? new List<AGVPathResult>();
Invalidate();
}
}
/// <summary>
/// 회사 로고 이미지
/// </summary>
public Image CompanyLogo
{
get => _companyLogo;
set
{
_companyLogo = value;
Invalidate();
}
}
/// <summary>
/// 측정 정보 텍스트
/// </summary>
public string MeasurementInfo
{
get => _measurementInfo;
set
{
_measurementInfo = value;
Invalidate();
}
}
#endregion
#region Connection Highlighting
/// <summary>
/// 특정 연결을 강조 표시
/// </summary>
/// <param name="fromNodeId">시작 노드 ID</param>
/// <param name="toNodeId">끝 노드 ID</param>
public void HighlightConnection(string fromNodeId, string toNodeId)
{
if (string.IsNullOrEmpty(fromNodeId) || string.IsNullOrEmpty(toNodeId))
{
_highlightedConnection = null;
}
else
{
// 사전순으로 정렬하여 저장 (연결이 단일 방향으로 저장되므로)
if (string.Compare(fromNodeId, toNodeId, StringComparison.Ordinal) <= 0)
{
_highlightedConnection = (fromNodeId, toNodeId);
}
else
{
_highlightedConnection = (toNodeId, fromNodeId);
}
}
Invalidate();
}
/// <summary>
/// 연결 강조 표시 해제
/// </summary>
public void ClearHighlightedConnection()
{
_highlightedConnection = null;
Invalidate();
}
#endregion
#region Constructor
public UnifiedAGVCanvas()
{
InitializeComponent();
InitializeCanvas();
}
#endregion
#region Initialization
private void InitializeCanvas()
{
SetStyle(ControlStyles.AllPaintingInWmPaint |
ControlStyles.UserPaint |
ControlStyles.DoubleBuffer |
ControlStyles.ResizeRedraw, true);
_nodes = new List<MapNode>();
_agvList = new List<IAGV>();
_agvPositions = new Dictionary<string, Point>();
_agvDirections = new Dictionary<string, AgvDirection>();
_agvStates = new Dictionary<string, AGVState>();
_allPaths = new List<AGVPathResult>();
_dockingErrors = new Dictionary<string, bool>();
InitializeBrushesAndPens();
CreateContextMenu();
}
private void InitializeBrushesAndPens()
{
// 노드 브러쉬
_normalNodeBrush = new SolidBrush(Color.LightBlue);
_rotationNodeBrush = new SolidBrush(Color.Yellow);
_dockingNodeBrush = new SolidBrush(Color.Orange);
_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);
_pathBrush = new SolidBrush(Color.Purple);
// 그리드 브러쉬
_gridBrush = new SolidBrush(Color.LightGray);
// 펜
_connectionPen = new Pen(Color.DarkBlue, CONNECTION_WIDTH);
_connectionPen.EndCap = LineCap.ArrowAnchor;
_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);
_highlightedConnectionPen = new Pen(Color.Red, 4) { DashStyle = DashStyle.Solid };
}
private void CreateContextMenu()
{
_contextMenu = new ContextMenuStrip();
// 컨텍스트 메뉴는 EditMode에서만 사용
}
private void UpdateModeUI()
{
// 모드에 따른 UI 업데이트
_contextMenu.Enabled = true;
Cursor = GetCursorForMode(_editMode);
}
#endregion
#region AGV Management
/// <summary>
/// AGV 위치 업데이트
/// </summary>
public void UpdateAGVPosition(string agvId, Point position)
{
if (_agvPositions.ContainsKey(agvId))
_agvPositions[agvId] = position;
else
_agvPositions.Add(agvId, position);
Invalidate();
}
/// <summary>
/// AGV 방향 업데이트
/// </summary>
public void UpdateAGVDirection(string agvId, AgvDirection direction)
{
if (_agvDirections.ContainsKey(agvId))
_agvDirections[agvId] = direction;
else
_agvDirections.Add(agvId, direction);
Invalidate();
}
/// <summary>
/// AGV 상태 업데이트
/// </summary>
public void UpdateAGVState(string agvId, AGVState state)
{
if (_agvStates.ContainsKey(agvId))
_agvStates[agvId] = state;
else
_agvStates.Add(agvId, state);
Invalidate();
}
/// <summary>
/// AGV 위치 설정 (시뮬레이터용)
/// </summary>
/// <param name="agvId">AGV ID</param>
/// <param name="position">새로운 위치</param>
public void SetAGVPosition(string agvId, Point position)
{
UpdateAGVPosition(agvId, position);
}
/// <summary>
/// AGV 데이터 동기화
/// </summary>
private void UpdateAGVData()
{
if (_agvList == null) return;
foreach (var agv in _agvList)
{
UpdateAGVPosition(agv.AgvId, agv.CurrentPosition);
UpdateAGVDirection(agv.AgvId, agv.CurrentDirection);
UpdateAGVState(agv.AgvId, agv.CurrentState);
}
}
#endregion
#region Helper Methods
private Cursor GetCursorForMode(EditMode mode)
{
if (_canvasMode != CanvasMode.Edit)
return Cursors.Default;
switch (mode)
{
case EditMode.AddNode:
return Cursors.Cross;
case EditMode.Move:
return Cursors.SizeAll;
case EditMode.Connect:
return Cursors.Hand;
case EditMode.Delete:
return Cursors.No;
default:
return Cursors.Default;
}
}
private void CancelConnection()
{
_isConnectionMode = false;
_connectionStartNode = null;
_connectionEndPoint = Point.Empty;
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
protected override void Dispose(bool disposing)
{
if (disposing)
{
// 브러쉬 정리
_normalNodeBrush?.Dispose();
_rotationNodeBrush?.Dispose();
_dockingNodeBrush?.Dispose();
_chargingNodeBrush?.Dispose();
_selectedNodeBrush?.Dispose();
_hoveredNodeBrush?.Dispose();
_destinationNodeBrush?.Dispose();
_gridBrush?.Dispose();
_agvBrush?.Dispose();
_pathBrush?.Dispose();
// 펜 정리
_connectionPen?.Dispose();
_gridPen?.Dispose();
_tempConnectionPen?.Dispose();
_selectedNodePen?.Dispose();
_destinationNodePen?.Dispose();
_pathPen?.Dispose();
_agvPen?.Dispose();
_highlightedConnectionPen?.Dispose();
// 컨텍스트 메뉴 정리
_contextMenu?.Dispose();
// 이미지 정리
_companyLogo?.Dispose();
}
base.Dispose(disposing);
}
#endregion
/// <summary>
/// RFID 중복값을 가진 노드들을 감지하고 표시
/// 나중에 추가된 노드(인덱스가 더 큰)를 중복으로 간주
/// </summary>
private void DetectDuplicateRfidNodes()
{
_duplicateRfidNodes.Clear();
if (_nodes == null || _nodes.Count == 0)
return;
// RFID값과 해당 노드의 인덱스를 저장
var rfidToNodeIndex = new Dictionary<string, List<int>>();
// 모든 노드의 RFID값 수집
for (int i = 0; i < _nodes.Count; i++)
{
var node = _nodes[i];
if (!string.IsNullOrEmpty(node.RfidId))
{
if (!rfidToNodeIndex.ContainsKey(node.RfidId))
{
rfidToNodeIndex[node.RfidId] = new List<int>();
}
rfidToNodeIndex[node.RfidId].Add(i);
}
}
// 중복된 RFID를 가진 노드들을 찾아서 나중에 추가된 것들을 표시
foreach (var kvp in rfidToNodeIndex)
{
if (kvp.Value.Count > 1)
{
// 첫 번째 노드는 원본으로 유지, 나머지는 중복으로 표시
for (int i = 1; i < kvp.Value.Count; i++)
{
int duplicateNodeIndex = kvp.Value[i];
_duplicateRfidNodes.Add(_nodes[duplicateNodeIndex].NodeId);
}
}
}
}
/// <summary>
/// 기존 노드들의 최대 번호를 찾아서 _nodeCounter를 업데이트
/// </summary>
private void UpdateNodeCounter()
{
if (_nodes == null || _nodes.Count == 0)
{
_nodeCounter = 1;
return;
}
int maxNumber = 0;
foreach (var node in _nodes)
{
// NodeId에서 숫자 부분 추출 (예: "N001" -> 1)
if (node.NodeId.StartsWith("N") && int.TryParse(node.NodeId.Substring(1), out int number))
{
maxNumber = Math.Max(maxNumber, number);
}
}
_nodeCounter = maxNumber + 1;
}
/// <summary>
/// 특정 노드에 도킹 오류 표시를 설정/해제합니다.
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <param name="hasError">오류 여부</param>
public void SetDockingError(string nodeId, bool hasError)
{
if (string.IsNullOrEmpty(nodeId))
return;
if (hasError)
{
_dockingErrors[nodeId] = true;
}
else
{
_dockingErrors.Remove(nodeId);
}
Invalidate(); // 화면 다시 그리기
}
/// <summary>
/// 특정 노드에 도킹 오류가 있는지 확인합니다.
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <returns>도킹 오류 여부</returns>
public bool HasDockingError(string nodeId)
{
return _dockingErrors.ContainsKey(nodeId) && _dockingErrors[nodeId];
}
/// <summary>
/// 모든 도킹 오류를 초기화합니다.
/// </summary>
public void ClearDockingErrors()
{
_dockingErrors.Clear();
Invalidate();
}
}
}

72
Cs_HMI/AGVLogic/AGVNavigationCore/Models/Enums.cs Normal file
View File

@@ -0,0 +1,72 @@
using System;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 노드 타입 열거형
/// </summary>
public enum NodeType
{
/// <summary>일반 경로 노드</summary>
Normal,
/// <summary>회전 가능 지점</summary>
Rotation,
/// <summary>도킹 스테이션</summary>
Docking,
/// <summary>충전 스테이션</summary>
Charging,
/// <summary>라벨 (UI 요소)</summary>
Label,
/// <summary>이미지 (UI 요소)</summary>
Image
}
/// <summary>
/// 도킹 방향 열거형
/// </summary>
public enum DockingDirection
{
/// <summary>도킹 방향 상관없음 (일반 경로 노드)</summary>
DontCare,
/// <summary>전진 도킹 (충전기)</summary>
Forward,
/// <summary>후진 도킹 (로더, 클리너, 오프로더, 버퍼)</summary>
Backward
}
/// <summary>
/// AGV 이동 방향 열거형
/// </summary>
public enum AgvDirection
{
/// <summary>전진 (모니터 방향)</summary>
Forward,
/// <summary>후진 (리프트 방향)</summary>
Backward,
/// <summary>좌회전</summary>
Left,
/// <summary>우회전</summary>
Right,
/// <summary>정지</summary>
Stop
}
/// <summary>
/// 장비 타입 열거형
/// </summary>
public enum StationType
{
/// <summary>로더</summary>
Loader,
/// <summary>클리너</summary>
Cleaner,
/// <summary>오프로더</summary>
Offloader,
/// <summary>버퍼</summary>
Buffer,
/// <summary>충전기</summary>
Charger
}
}

215
Cs_HMI/AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs Normal file
View File

@@ -0,0 +1,215 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using AGVNavigationCore.Controls;
using AGVNavigationCore.PathFinding;
using AGVNavigationCore.PathFinding.Core;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 이동 가능한 AGV 인터페이스
/// 실제 AGV와 시뮬레이션 AGV 모두 구현해야 하는 기본 인터페이스
/// </summary>
public interface IMovableAGV
{
#region Events
/// <summary>
/// AGV 상태 변경 이벤트
/// </summary>
event EventHandler<AGVState> StateChanged;
/// <summary>
/// 위치 변경 이벤트
/// </summary>
event EventHandler<(Point, AgvDirection, MapNode)> PositionChanged;
/// <summary>
/// RFID 감지 이벤트
/// </summary>
event EventHandler<string> RfidDetected;
/// <summary>
/// 경로 완료 이벤트
/// </summary>
event EventHandler<AGVPathResult> PathCompleted;
/// <summary>
/// 오류 발생 이벤트
/// </summary>
event EventHandler<string> ErrorOccurred;
#endregion
#region Properties
/// <summary>
/// AGV ID
/// </summary>
string AgvId { get; }
/// <summary>
/// 현재 위치
/// </summary>
Point CurrentPosition { get; set; }
/// <summary>
/// 현재 방향 (모터 방향)
/// </summary>
AgvDirection CurrentDirection { get; set; }
/// <summary>
/// 현재 상태
/// </summary>
AGVState CurrentState { get; set; }
/// <summary>
/// 현재 속도
/// </summary>
float CurrentSpeed { get; }
/// <summary>
/// 배터리 레벨 (0-100%)
/// </summary>
float BatteryLevel { get; set; }
/// <summary>
/// 현재 경로
/// </summary>
AGVPathResult CurrentPath { get; }
/// <summary>
/// 현재 노드 ID
/// </summary>
string CurrentNodeId { get; }
/// <summary>
/// 목표 위치
/// </summary>
Point? TargetPosition { get; }
/// <summary>
/// 목표 노드 ID
/// </summary>
string TargetNodeId { get; }
/// <summary>
/// 도킹 방향
/// </summary>
DockingDirection DockingDirection { get; }
#endregion
#region Sensor Input Methods (실제 AGV에서 호출)
/// <summary>
/// 현재 위치 설정 (실제 위치 센서에서)
/// </summary>
void SetCurrentPosition(Point position);
/// <summary>
/// 감지된 RFID 설정 (실제 RFID 센서에서)
/// </summary>
void SetDetectedRfid(string rfidId);
/// <summary>
/// 모터 방향 설정 (모터 컨트롤러에서)
/// </summary>
void SetMotorDirection(AgvDirection direction);
/// <summary>
/// 배터리 레벨 설정 (BMS에서)
/// </summary>
void SetBatteryLevel(float percentage);
#endregion
#region State Query Methods
/// <summary>
/// 현재 위치 조회
/// </summary>
Point GetCurrentPosition();
/// <summary>
/// 현재 상태 조회
/// </summary>
AGVState GetCurrentState();
/// <summary>
/// 현재 노드 ID 조회
/// </summary>
string GetCurrentNodeId();
/// <summary>
/// AGV 상태 정보 문자열 조회
/// </summary>
string GetStatus();
#endregion
#region Path Execution Methods
/// <summary>
/// 경로 실행
/// </summary>
void ExecutePath(AGVPathResult path, List<MapNode> mapNodes);
/// <summary>
/// 경로 정지
/// </summary>
void StopPath();
/// <summary>
/// 긴급 정지
/// </summary>
void EmergencyStop();
#endregion
#region Update Method
/// <summary>
/// 프레임 업데이트 (외부에서 주기적으로 호출)
/// 이 방식으로 타이머에 의존하지 않고 외부에서 제어 가능
/// </summary>
/// <param name="deltaTimeMs">마지막 업데이트 이후 경과 시간 (밀리초)</param>
void Update(float deltaTimeMs);
#endregion
#region Manual Control Methods (테스트용)
/// <summary>
/// 수동 이동
/// </summary>
void MoveTo(Point targetPosition);
/// <summary>
/// 수동 회전
/// </summary>
void Rotate(AgvDirection direction);
/// <summary>
/// 충전 시작
/// </summary>
void StartCharging();
/// <summary>
/// 충전 종료
/// </summary>
void StopCharging();
#endregion
#region Cleanup
/// <summary>
/// 리소스 정리
/// </summary>
void Dispose();
#endregion
}
}

355
Cs_HMI/AGVLogic/AGVNavigationCore/Models/MapLoader.cs Normal file
View File

@@ -0,0 +1,355 @@
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using Newtonsoft.Json;
namespace AGVNavigationCore.Models
{
/// <summary>
/// AGV 맵 파일 로딩/저장을 위한 공용 유틸리티 클래스
/// AGVMapEditor와 AGVSimulator에서 공통으로 사용
/// </summary>
public static class MapLoader
{
/// <summary>
/// 맵 파일 로딩 결과
/// </summary>
public class MapLoadResult
{
public bool Success { get; set; }
public List<MapNode> Nodes { get; set; } = new List<MapNode>();
public string ErrorMessage { get; set; } = string.Empty;
public string Version { get; set; } = string.Empty;
public DateTime CreatedDate { get; set; }
}
/// <summary>
/// 맵 파일 저장용 데이터 구조
/// </summary>
public class MapFileData
{
public List<MapNode> Nodes { get; set; } = new List<MapNode>();
public DateTime CreatedDate { get; set; }
public string Version { get; set; } = "1.0";
}
/// <summary>
/// 맵 파일을 로드하여 노드를 반환
/// </summary>
/// <param name="filePath">맵 파일 경로</param>
/// <returns>로딩 결과</returns>
public static MapLoadResult LoadMapFromFile(string filePath)
{
var result = new MapLoadResult();
try
{
if (!File.Exists(filePath))
{
result.ErrorMessage = $"파일을 찾을 수 없습니다: {filePath}";
return result;
}
var json = File.ReadAllText(filePath);
// JSON 역직렬화 설정: 누락된 속성 무시, 안전한 처리
var settings = new JsonSerializerSettings
{
MissingMemberHandling = MissingMemberHandling.Ignore,
NullValueHandling = NullValueHandling.Ignore,
DefaultValueHandling = DefaultValueHandling.Populate
};
var mapData = JsonConvert.DeserializeObject<MapFileData>(json, settings);
if (mapData != null)
{
result.Nodes = mapData.Nodes ?? new List<MapNode>();
result.Version = mapData.Version ?? "1.0";
result.CreatedDate = mapData.CreatedDate;
// 기존 Description 데이터를 Name으로 마이그레이션
MigrateDescriptionToName(result.Nodes);
// DockingDirection 마이그레이션 (기존 NodeType 기반으로 설정)
MigrateDockingDirection(result.Nodes);
// 중복된 NodeId 정리
FixDuplicateNodeIds(result.Nodes);
// 중복 연결 정리 (양방향 중복 제거)
CleanupDuplicateConnections(result.Nodes);
// 이미지 노드들의 이미지 로드
LoadImageNodes(result.Nodes);
result.Success = true;
}
else
{
result.ErrorMessage = "맵 데이터 파싱에 실패했습니다.";
}
}
catch (Exception ex)
{
result.ErrorMessage = $"맵 파일 로딩 중 오류 발생: {ex.Message}";
}
return result;
}
/// <summary>
/// 맵 데이터를 파일로 저장
/// </summary>
/// <param name="filePath">저장할 파일 경로</param>
/// <param name="nodes">맵 노드 목록</param>
/// <returns>저장 성공 여부</returns>
public static bool SaveMapToFile(string filePath, List<MapNode> nodes)
{
try
{
var mapData = new MapFileData
{
Nodes = nodes,
CreatedDate = DateTime.Now,
Version = "1.0"
};
var json = JsonConvert.SerializeObject(mapData, Formatting.Indented);
File.WriteAllText(filePath, json);
return true;
}
catch (Exception)
{
return false;
}
}
/// <summary>
/// 이미지 노드들의 이미지 로드
/// </summary>
/// <param name="nodes">노드 목록</param>
private static void LoadImageNodes(List<MapNode> nodes)
{
foreach (var node in nodes)
{
if (node.Type == NodeType.Image)
{
node.LoadImage();
}
}
}
/// <summary>
/// 기존 Description 데이터를 Name 필드로 마이그레이션
/// JSON 파일에서 Description 필드가 있는 경우 Name으로 이동
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void MigrateDescriptionToName(List<MapNode> mapNodes)
{
// JSON에서 Description이 있던 기존 파일들을 위한 마이그레이션
// 현재 MapNode 클래스에는 Description 속성이 제거되었으므로
// 이 메서드는 호환성을 위해 유지되지만 실제로는 작동하지 않음
// 기존 파일들은 다시 저장될 때 Description 없이 저장됨
}
/// <summary>
/// 기존 맵 파일의 DockingDirection을 NodeType 기반으로 마이그레이션
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void MigrateDockingDirection(List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0) return;
foreach (var node in mapNodes)
{
// 기존 파일에서 DockingDirection이 기본값(DontCare)인 경우에만 마이그레이션
if (node.DockDirection == DockingDirection.DontCare)
{
switch (node.Type)
{
case NodeType.Charging:
node.DockDirection = DockingDirection.Forward;
break;
case NodeType.Docking:
node.DockDirection = DockingDirection.Backward;
break;
default:
// Normal, Rotation, Label, Image는 DontCare 유지
node.DockDirection = DockingDirection.DontCare;
break;
}
}
}
}
/// <summary>
/// 중복된 NodeId를 가진 노드들을 고유한 NodeId로 수정
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void FixDuplicateNodeIds(List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0) return;
var usedIds = new HashSet<string>();
var duplicateNodes = new List<MapNode>();
// 첫 번째 패스: 중복된 노드들 식별
foreach (var node in mapNodes)
{
if (usedIds.Contains(node.NodeId))
{
duplicateNodes.Add(node);
}
else
{
usedIds.Add(node.NodeId);
}
}
// 두 번째 패스: 중복된 노드들에게 새로운 NodeId 할당
foreach (var duplicateNode in duplicateNodes)
{
string newNodeId = GenerateUniqueNodeId(usedIds);
// 다른 노드들의 연결에서 기존 NodeId를 새 NodeId로 업데이트
UpdateConnections(mapNodes, duplicateNode.NodeId, newNodeId);
duplicateNode.NodeId = newNodeId;
usedIds.Add(newNodeId);
}
}
/// <summary>
/// 사용되지 않는 고유한 NodeId 생성
/// </summary>
/// <param name="usedIds">이미 사용된 NodeId 목록</param>
/// <returns>고유한 NodeId</returns>
private static string GenerateUniqueNodeId(HashSet<string> usedIds)
{
int counter = 1;
string nodeId;
do
{
nodeId = $"N{counter:D3}";
counter++;
}
while (usedIds.Contains(nodeId));
return nodeId;
}
/// <summary>
/// 노드 연결에서 NodeId 변경사항 반영
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
/// <param name="oldNodeId">기존 NodeId</param>
/// <param name="newNodeId">새로운 NodeId</param>
private static void UpdateConnections(List<MapNode> mapNodes, string oldNodeId, string newNodeId)
{
foreach (var node in mapNodes)
{
if (node.ConnectedNodes != null)
{
for (int i = 0; i < node.ConnectedNodes.Count; i++)
{
if (node.ConnectedNodes[i] == oldNodeId)
{
node.ConnectedNodes[i] = newNodeId;
}
}
}
}
}
/// <summary>
/// 중복 연결을 정리합니다. 양방향 중복 연결을 단일 연결로 통합합니다.
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void CleanupDuplicateConnections(List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0) return;
var processedPairs = new HashSet<string>();
foreach (var node in mapNodes)
{
var connectionsToRemove = new List<string>();
foreach (var connectedNodeId in node.ConnectedNodes.ToList())
{
var connectedNode = mapNodes.FirstOrDefault(n => n.NodeId == connectedNodeId);
if (connectedNode == null) continue;
// 연결 쌍의 키 생성 (사전순 정렬)
string pairKey = string.Compare(node.NodeId, connectedNodeId, StringComparison.Ordinal) < 0
? $"{node.NodeId}-{connectedNodeId}"
: $"{connectedNodeId}-{node.NodeId}";
if (processedPairs.Contains(pairKey))
{
// 이미 처리된 연결인 경우 중복으로 간주하고 제거
connectionsToRemove.Add(connectedNodeId);
}
else
{
// 처리되지 않은 연결인 경우
processedPairs.Add(pairKey);
// 양방향 연결인 경우 하나만 유지
if (connectedNode.ConnectedNodes.Contains(node.NodeId))
{
// 사전순으로 더 작은 노드에만 연결을 유지
if (string.Compare(node.NodeId, connectedNodeId, StringComparison.Ordinal) > 0)
{
connectionsToRemove.Add(connectedNodeId);
}
else
{
// 반대 방향 연결 제거
connectedNode.RemoveConnection(node.NodeId);
}
}
}
}
// 중복 연결 제거
foreach (var connectionToRemove in connectionsToRemove)
{
node.RemoveConnection(connectionToRemove);
}
}
}
/// <summary>
/// MapNode 목록에서 RFID가 없는 노드들에 자동으로 RFID ID를 할당합니다.
/// *** 에디터와 시뮬레이터 데이터 불일치 방지를 위해 비활성화됨 ***
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
[Obsolete("RFID 자동 할당은 에디터와 시뮬레이터 간 데이터 불일치를 야기하므로 사용하지 않음")]
public static void AssignAutoRfidIds(List<MapNode> mapNodes)
{
// 에디터에서 설정한 RFID 값을 그대로 사용하기 위해 자동 할당 기능 비활성화
// 에디터와 시뮬레이터 간 데이터 일관성 유지를 위함
return;
/*
foreach (var node in mapNodes)
{
// 네비게이션 가능한 노드이면서 RFID가 없는 경우에만 자동 할당
if (node.IsNavigationNode() && !node.HasRfid())
{
// 기본 RFID ID 생성 (N001 -> 001)
var rfidId = node.NodeId.Replace("N", "").PadLeft(3, '0');
node.SetRfidInfo(rfidId, "", "정상");
}
}
*/
}
}
}

481
Cs_HMI/AGVLogic/AGVNavigationCore/Models/MapNode.cs Normal file
View File

@@ -0,0 +1,481 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Drawing2D;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 맵 노드 정보를 관리하는 클래스
/// 논리적 노드로서 실제 맵의 위치와 속성을 정의
/// </summary>
public class MapNode
{
/// <summary>
/// 논리적 노드 ID (맵 에디터에서 관리하는 고유 ID)
/// 예: "N001", "N002", "LOADER1", "CHARGER1"
/// </summary>
public string NodeId { get; set; } = string.Empty;
/// <summary>
/// 노드 표시 이름 (사용자 친화적)
/// 예: "로더1", "충전기1", "교차점A", "회전지점1"
/// </summary>
public string Name { get; set; } = string.Empty;
/// <summary>
/// 맵 상의 위치 좌표 (픽셀 단위)
/// </summary>
public Point Position { get; set; } = Point.Empty;
/// <summary>
/// 노드 타입
/// </summary>
public NodeType Type { get; set; } = NodeType.Normal;
/// <summary>
/// 도킹 방향 (도킹/충전 노드인 경우에만 Forward/Backward, 일반 노드는 DontCare)
/// </summary>
public DockingDirection DockDirection { get; set; } = DockingDirection.DontCare;
/// <summary>
/// 연결된 노드 ID 목록 (경로 정보)
/// </summary>
public List<string> ConnectedNodes { get; set; } = new List<string>();
/// <summary>
/// 회전 가능 여부 (180도 회전 가능한 지점)
/// </summary>
public bool CanRotate { get; set; } = false;
/// <summary>
/// 장비 ID (도킹/충전 스테이션인 경우)
/// 예: "LOADER1", "CLEANER1", "BUFFER1", "CHARGER1"
/// </summary>
public string StationId { get; set; } = string.Empty;
/// <summary>
/// 장비 타입 (도킹/충전 스테이션인 경우)
/// </summary>
public StationType? StationType { get; set; } = null;
/// <summary>
/// 노드 생성 일자
/// </summary>
public DateTime CreatedDate { get; set; } = DateTime.Now;
/// <summary>
/// 노드 수정 일자
/// </summary>
public DateTime ModifiedDate { get; set; } = DateTime.Now;
/// <summary>
/// 노드 활성화 여부
/// </summary>
public bool IsActive { get; set; } = true;
/// <summary>
/// 노드 색상 (맵 에디터 표시용)
/// </summary>
public Color DisplayColor { get; set; } = Color.Blue;
/// <summary>
/// RFID 태그 ID (이 노드에 매핑된 RFID)
/// </summary>
public string RfidId { get; set; } = string.Empty;
/// <summary>
/// RFID 상태 (정상, 손상, 교체예정 등)
/// </summary>
public string RfidStatus { get; set; } = "정상";
/// <summary>
/// RFID 설치 위치 설명 (현장 작업자용)
/// 예: "로더1번 앞", "충전기2번 입구", "복도 교차점" 등
/// </summary>
public string RfidDescription { get; set; } = string.Empty;
/// <summary>
/// 라벨 텍스트 (NodeType.Label인 경우 사용)
/// </summary>
public string LabelText { get; set; } = string.Empty;
/// <summary>
/// 라벨 폰트 패밀리 (NodeType.Label인 경우 사용)
/// </summary>
public string FontFamily { get; set; } = "Arial";
/// <summary>
/// 라벨 폰트 크기 (NodeType.Label인 경우 사용)
/// </summary>
public float FontSize { get; set; } = 12.0f;
/// <summary>
/// 라벨 폰트 스타일 (NodeType.Label인 경우 사용)
/// </summary>
public FontStyle FontStyle { get; set; } = FontStyle.Regular;
/// <summary>
/// 라벨 전경색 (NodeType.Label인 경우 사용)
/// </summary>
public Color ForeColor { get; set; } = Color.Black;
/// <summary>
/// 라벨 배경색 (NodeType.Label인 경우 사용)
/// </summary>
public Color BackColor { get; set; } = Color.Transparent;
/// <summary>
/// 라벨 배경 표시 여부 (NodeType.Label인 경우 사용)
/// </summary>
public bool ShowBackground { get; set; } = false;
/// <summary>
/// 이미지 파일 경로 (NodeType.Image인 경우 사용)
/// </summary>
public string ImagePath { get; set; } = string.Empty;
/// <summary>
/// 이미지 크기 배율 (NodeType.Image인 경우 사용)
/// </summary>
public SizeF Scale { get; set; } = new SizeF(1.0f, 1.0f);
/// <summary>
/// 이미지 투명도 (NodeType.Image인 경우 사용, 0.0~1.0)
/// </summary>
public float Opacity { get; set; } = 1.0f;
/// <summary>
/// 이미지 회전 각도 (NodeType.Image인 경우 사용, 도 단위)
/// </summary>
public float Rotation { get; set; } = 0.0f;
/// <summary>
/// 로딩된 이미지 (런타임에서만 사용, JSON 직렬화 제외)
/// </summary>
[Newtonsoft.Json.JsonIgnore]
public Image LoadedImage { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public MapNode()
{
}
/// <summary>
/// 매개변수 생성자
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <param name="name">노드 이름</param>
/// <param name="position">위치</param>
/// <param name="type">노드 타입</param>
public MapNode(string nodeId, string name, Point position, NodeType type)
{
NodeId = nodeId;
Name = name;
Position = position;
Type = type;
CreatedDate = DateTime.Now;
ModifiedDate = DateTime.Now;
// 타입별 기본 색상 설정
SetDefaultColorByType(type);
}
/// <summary>
/// 노드 타입에 따른 기본 색상 설정
/// </summary>
/// <param name="type">노드 타입</param>
public void SetDefaultColorByType(NodeType type)
{
switch (type)
{
case NodeType.Normal:
DisplayColor = Color.Blue;
break;
case NodeType.Rotation:
DisplayColor = Color.Orange;
break;
case NodeType.Docking:
DisplayColor = Color.Green;
break;
case NodeType.Charging:
DisplayColor = Color.Red;
break;
case NodeType.Label:
DisplayColor = Color.Purple;
break;
case NodeType.Image:
DisplayColor = Color.Brown;
break;
}
}
/// <summary>
/// 다른 노드와의 연결 추가
/// </summary>
/// <param name="nodeId">연결할 노드 ID</param>
public void AddConnection(string nodeId)
{
if (!ConnectedNodes.Contains(nodeId))
{
ConnectedNodes.Add(nodeId);
ModifiedDate = DateTime.Now;
}
}
/// <summary>
/// 다른 노드와의 연결 제거
/// </summary>
/// <param name="nodeId">연결 해제할 노드 ID</param>
public void RemoveConnection(string nodeId)
{
if (ConnectedNodes.Remove(nodeId))
{
ModifiedDate = DateTime.Now;
}
}
/// <summary>
/// 도킹 스테이션 설정
/// </summary>
/// <param name="stationId">장비 ID</param>
/// <param name="stationType">장비 타입</param>
/// <param name="dockDirection">도킹 방향</param>
public void SetDockingStation(string stationId, StationType stationType, DockingDirection dockDirection)
{
Type = NodeType.Docking;
StationId = stationId;
StationType = stationType;
DockDirection = dockDirection;
SetDefaultColorByType(NodeType.Docking);
ModifiedDate = DateTime.Now;
}
/// <summary>
/// 충전 스테이션 설정
/// </summary>
/// <param name="stationId">충전기 ID</param>
public void SetChargingStation(string stationId)
{
Type = NodeType.Charging;
StationId = stationId;
StationType = Models.StationType.Charger;
DockDirection = DockingDirection.Forward; // 충전기는 항상 전진 도킹
SetDefaultColorByType(NodeType.Charging);
ModifiedDate = DateTime.Now;
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
return $"{RfidId}({NodeId}): {Name} ({Type}) at ({Position.X}, {Position.Y})";
}
/// <summary>
/// 리스트박스 표시용 텍스트 (노드ID - 설명 - RFID 순서)
/// </summary>
public string DisplayText
{
get
{
var displayText = NodeId;
if (!string.IsNullOrEmpty(Name))
{
displayText += $" - {Name}";
}
if (!string.IsNullOrEmpty(RfidId))
{
displayText += $" - [{RfidId}]";
}
return displayText;
}
}
/// <summary>
/// 노드 복사
/// </summary>
/// <returns>복사된 노드</returns>
public MapNode Clone()
{
var clone = new MapNode
{
NodeId = NodeId,
Name = Name,
Position = Position,
Type = Type,
DockDirection = DockDirection,
ConnectedNodes = new List<string>(ConnectedNodes),
CanRotate = CanRotate,
StationId = StationId,
StationType = StationType,
CreatedDate = CreatedDate,
ModifiedDate = ModifiedDate,
IsActive = IsActive,
DisplayColor = DisplayColor,
RfidId = RfidId,
RfidStatus = RfidStatus,
RfidDescription = RfidDescription,
LabelText = LabelText,
FontFamily = FontFamily,
FontSize = FontSize,
FontStyle = FontStyle,
ForeColor = ForeColor,
BackColor = BackColor,
ShowBackground = ShowBackground,
ImagePath = ImagePath,
Scale = Scale,
Opacity = Opacity,
Rotation = Rotation
};
return clone;
}
/// <summary>
/// 이미지 로드 (256x256 이상일 경우 자동 리사이즈)
/// </summary>
/// <returns>로드 성공 여부</returns>
public bool LoadImage()
{
if (Type != NodeType.Image) return false;
try
{
if (!string.IsNullOrEmpty(ImagePath) && System.IO.File.Exists(ImagePath))
{
LoadedImage?.Dispose();
var originalImage = Image.FromFile(ImagePath);
// 이미지 크기 체크 및 리사이즈
if (originalImage.Width > 256 || originalImage.Height > 256)
{
LoadedImage = ResizeImage(originalImage, 256, 256);
originalImage.Dispose();
}
else
{
LoadedImage = originalImage;
}
return true;
}
}
catch (Exception)
{
// 이미지 로드 실패
}
return false;
}
/// <summary>
/// 이미지 리사이즈 (비율 유지)
/// </summary>
/// <param name="image">원본 이미지</param>
/// <param name="maxWidth">최대 너비</param>
/// <param name="maxHeight">최대 높이</param>
/// <returns>리사이즈된 이미지</returns>
private Image ResizeImage(Image image, int maxWidth, int maxHeight)
{
// 비율 계산
double ratioX = (double)maxWidth / image.Width;
double ratioY = (double)maxHeight / image.Height;
double ratio = Math.Min(ratioX, ratioY);
// 새로운 크기 계산
int newWidth = (int)(image.Width * ratio);
int newHeight = (int)(image.Height * ratio);
// 리사이즈된 이미지 생성
var resizedImage = new Bitmap(newWidth, newHeight);
using (var graphics = Graphics.FromImage(resizedImage))
{
graphics.CompositingQuality = CompositingQuality.HighQuality;
graphics.InterpolationMode = InterpolationMode.HighQualityBicubic;
graphics.SmoothingMode = SmoothingMode.HighQuality;
graphics.DrawImage(image, 0, 0, newWidth, newHeight);
}
return resizedImage;
}
/// <summary>
/// 실제 표시될 크기 계산 (이미지 노드인 경우)
/// </summary>
/// <returns>실제 크기</returns>
public Size GetDisplaySize()
{
if (Type != NodeType.Image || LoadedImage == null) return Size.Empty;
return new Size(
(int)(LoadedImage.Width * Scale.Width),
(int)(LoadedImage.Height * Scale.Height)
);
}
/// <summary>
/// 리소스 정리
/// </summary>
public void Dispose()
{
LoadedImage?.Dispose();
LoadedImage = null;
}
/// <summary>
/// 경로 찾기에 사용 가능한 노드인지 확인
/// (라벨, 이미지 노드는 경로 찾기에서 제외)
/// </summary>
public bool IsNavigationNode()
{
return Type != NodeType.Label && Type != NodeType.Image && IsActive;
}
/// <summary>
/// RFID가 할당되어 있는지 확인
/// </summary>
public bool HasRfid()
{
return !string.IsNullOrEmpty(RfidId);
}
/// <summary>
/// RFID 정보 설정
/// </summary>
/// <param name="rfidId">RFID ID</param>
/// <param name="rfidDescription">설치 위치 설명</param>
/// <param name="rfidStatus">RFID 상태</param>
public void SetRfidInfo(string rfidId, string rfidDescription = "", string rfidStatus = "정상")
{
RfidId = rfidId;
RfidDescription = rfidDescription;
RfidStatus = rfidStatus;
ModifiedDate = DateTime.Now;
}
/// <summary>
/// RFID 정보 삭제
/// </summary>
public void ClearRfidInfo()
{
RfidId = string.Empty;
RfidDescription = string.Empty;
RfidStatus = "정상";
ModifiedDate = DateTime.Now;
}
/// <summary>
/// RFID 기반 표시 텍스트 (RFID ID 우선, 없으면 노드ID)
/// </summary>
public string GetRfidDisplayText()
{
return HasRfid() ? RfidId : NodeId;
}
}
}

613
Cs_HMI/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs Normal file
View File

@@ -0,0 +1,613 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Controls;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding;
using AGVNavigationCore.PathFinding.Core;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 가상 AGV 클래스 (코어 비즈니스 로직)
/// 실제 AGV와 시뮬레이터 모두에서 사용 가능한 공용 로직
/// 시뮬레이션과 실제 동작이 동일하게 동작하도록 설계됨
/// </summary>
public class VirtualAGV : IMovableAGV, IAGV
{
#region Events
/// <summary>
/// AGV 상태 변경 이벤트
/// </summary>
public event EventHandler<AGVState> StateChanged;
/// <summary>
/// 위치 변경 이벤트
/// </summary>
public event EventHandler<(Point, AgvDirection, MapNode)> PositionChanged;
/// <summary>
/// RFID 감지 이벤트
/// </summary>
public event EventHandler<string> RfidDetected;
/// <summary>
/// 경로 완료 이벤트
/// </summary>
public event EventHandler<AGVPathResult> PathCompleted;
/// <summary>
/// 오류 발생 이벤트
/// </summary>
public event EventHandler<string> ErrorOccurred;
#endregion
#region Fields
private string _agvId;
private Point _currentPosition;
private Point _targetPosition;
private string _targetId;
private string _currentId;
private AgvDirection _currentDirection;
private AgvDirection _targetDirection;
private AGVState _currentState;
private float _currentSpeed;
// 경로 관련
private AGVPathResult _currentPath;
private List<string> _remainingNodes;
private int _currentNodeIndex;
private MapNode _currentNode;
private MapNode _targetNode;
// 이동 관련
private DateTime _lastUpdateTime;
private Point _moveStartPosition;
private Point _moveTargetPosition;
private float _moveProgress;
// 도킹 관련
private DockingDirection _dockingDirection;
// 시뮬레이션 설정
private readonly float _moveSpeed = 50.0f; // 픽셀/초
private readonly float _rotationSpeed = 90.0f; // 도/초
private bool _isMoving;
#endregion
#region Properties
/// <summary>
/// AGV ID
/// </summary>
public string AgvId => _agvId;
/// <summary>
/// 현재 위치
/// </summary>
public Point CurrentPosition
{
get => _currentPosition;
set => _currentPosition = value;
}
/// <summary>
/// 현재 방향
/// 모터의 동작 방향
/// </summary>
public AgvDirection CurrentDirection
{
get => _currentDirection;
set => _currentDirection = value;
}
/// <summary>
/// 현재 상태
/// </summary>
public AGVState CurrentState
{
get => _currentState;
set => _currentState = value;
}
/// <summary>
/// 현재 속도
/// </summary>
public float CurrentSpeed => _currentSpeed;
/// <summary>
/// 현재 경로
/// </summary>
public AGVPathResult CurrentPath => _currentPath;
/// <summary>
/// 현재 노드 ID
/// </summary>
public string CurrentNodeId => _currentNode?.NodeId;
/// <summary>
/// 목표 위치
/// </summary>
public Point? TargetPosition => _targetPosition;
/// <summary>
/// 배터리 레벨 (시뮬레이션)
/// </summary>
public float BatteryLevel { get; set; } = 100.0f;
/// <summary>
/// 목표 노드 ID
/// </summary>
public string TargetNodeId => _targetNode?.NodeId;
/// <summary>
/// 도킹 방향
/// </summary>
public DockingDirection DockingDirection => _dockingDirection;
#endregion
#region Constructor
/// <summary>
/// 생성자
/// </summary>
/// <param name="agvId">AGV ID</param>
/// <param name="startPosition">시작 위치</param>
/// <param name="startDirection">시작 방향</param>
public VirtualAGV(string agvId, Point startPosition, AgvDirection startDirection = AgvDirection.Forward)
{
_agvId = agvId;
_currentPosition = startPosition;
_currentDirection = startDirection;
_currentState = AGVState.Idle;
_currentSpeed = 0;
_dockingDirection = DockingDirection.Forward; // 기본값: 전진 도킹
_currentNode = null;
_targetNode = null;
_isMoving = false;
_lastUpdateTime = DateTime.Now;
}
#endregion
#region Public Methods - 센서/RFID 입력 (실제 AGV에서 호출)
/// <summary>
/// 현재 위치 설정 (실제 AGV 센서에서)
/// </summary>
public void SetCurrentPosition(Point position)
{
_currentPosition = position;
}
/// <summary>
/// 감지된 RFID 설정 (실제 RFID 센서에서)
/// </summary>
public void SetDetectedRfid(string rfidId)
{
RfidDetected?.Invoke(this, rfidId);
}
/// <summary>
/// 모터 방향 설정 (실제 모터 컨트롤러에서)
/// </summary>
public void SetMotorDirection(AgvDirection direction)
{
_currentDirection = direction;
}
/// <summary>
/// 배터리 레벨 설정 (실제 BMS에서)
/// </summary>
public void SetBatteryLevel(float percentage)
{
BatteryLevel = Math.Max(0, Math.Min(100, percentage));
// 배터리 부족 경고
if (BatteryLevel < 20.0f && _currentState != AGVState.Charging)
{
OnError($"배터리 부족: {BatteryLevel:F1}%");
}
}
#endregion
#region Public Methods - 상태 조회
/// <summary>
/// 현재 위치 조회
/// </summary>
public Point GetCurrentPosition() => _currentPosition;
/// <summary>
/// 현재 상태 조회
/// </summary>
public AGVState GetCurrentState() => _currentState;
/// <summary>
/// 현재 노드 ID 조회
/// </summary>
public string GetCurrentNodeId() => _currentNode?.NodeId;
/// <summary>
/// AGV 정보 조회
/// </summary>
public string GetStatus()
{
return $"AGV[{_agvId}] 위치:({_currentPosition.X},{_currentPosition.Y}) " +
$"방향:{_currentDirection} 상태:{_currentState} " +
$"속도:{_currentSpeed:F1} 배터리:{BatteryLevel:F1}%";
}
#endregion
#region Public Methods - 경로 실행
/// <summary>
/// 경로 실행 시작
/// </summary>
/// <param name="path">실행할 경로</param>
/// <param name="mapNodes">맵 노드 목록</param>
public void ExecutePath(AGVPathResult path, List<MapNode> mapNodes)
{
if (path == null || !path.Success)
{
OnError("유효하지 않은 경로입니다.");
return;
}
_currentPath = path;
_remainingNodes = new List<string>(path.Path);
_currentNodeIndex = 0;
// 시작 노드와 목표 노드 설정
if (_remainingNodes.Count > 0)
{
var startNode = mapNodes.FirstOrDefault(n => n.NodeId == _remainingNodes[0]);
if (startNode != null)
{
_currentNode = startNode;
// 목표 노드 설정 (경로의 마지막 노드)
if (_remainingNodes.Count > 1)
{
var targetNodeId = _remainingNodes[_remainingNodes.Count - 1];
var targetNode = mapNodes.FirstOrDefault(n => n.NodeId == targetNodeId);
// 목표 노드의 타입에 따라 도킹 방향 결정
if (targetNode != null)
{
_dockingDirection = GetDockingDirection(targetNode.Type);
_targetNode = targetNode;
}
}
StartMovement();
}
else
{
OnError($"시작 노드를 찾을 수 없습니다: {_remainingNodes[0]}");
}
}
}
/// <summary>
/// 간단한 경로 실행 (경로 객체 없이 노드만)
/// </summary>
public void StartPath(AGVPathResult path, List<MapNode> mapNodes)
{
ExecutePath(path, mapNodes);
}
/// <summary>
/// 경로 정지
/// </summary>
public void StopPath()
{
_isMoving = false;
_currentPath = null;
_remainingNodes?.Clear();
SetState(AGVState.Idle);
_currentSpeed = 0;
}
/// <summary>
/// 긴급 정지
/// </summary>
public void EmergencyStop()
{
StopPath();
OnError("긴급 정지가 실행되었습니다.");
}
#endregion
#region Public Methods - 프레임 업데이트 (외부에서 정기적으로 호출)
/// <summary>
/// 프레임 업데이트 (외부에서 주기적으로 호출)
/// 이 방식으로 타이머에 의존하지 않고 외부에서 제어 가능
/// </summary>
/// <param name="deltaTimeMs">마지막 업데이트 이후 경과 시간 (밀리초)</param>
public void Update(float deltaTimeMs)
{
var deltaTime = deltaTimeMs / 1000.0f; // 초 단위로 변환
UpdateMovement(deltaTime);
UpdateBattery(deltaTime);
// 위치 변경 이벤트 발생
PositionChanged?.Invoke(this, (_currentPosition, _currentDirection, _currentNode));
}
#endregion
#region Public Methods - 수동 제어 (테스트용)
/// <summary>
/// 수동 이동 (테스트용)
/// </summary>
/// <param name="targetPosition">목표 위치</param>
public void MoveTo(Point targetPosition)
{
_targetPosition = targetPosition;
_moveStartPosition = _currentPosition;
_moveTargetPosition = targetPosition;
_moveProgress = 0;
SetState(AGVState.Moving);
_isMoving = true;
}
/// <summary>
/// 수동 회전 (테스트용)
/// </summary>
/// <param name="direction">회전 방향</param>
public void Rotate(AgvDirection direction)
{
if (_currentState != AGVState.Idle)
return;
SetState(AGVState.Rotating);
_currentDirection = direction;
SetState(AGVState.Idle);
}
/// <summary>
/// 충전 시작
/// </summary>
public void StartCharging()
{
if (_currentState == AGVState.Idle)
{
SetState(AGVState.Charging);
}
}
/// <summary>
/// 충전 종료
/// </summary>
public void StopCharging()
{
if (_currentState == AGVState.Charging)
{
SetState(AGVState.Idle);
}
}
#endregion
#region Public Methods - AGV 위치 설정 (시뮬레이터용)
/// <summary>
/// AGV 위치 직접 설정
/// TargetPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
/// </summary>
/// <param name="node">현재 노드</param>
/// <param name="newPosition">새로운 위치</param>
/// <param name="motorDirection">모터이동방향</param>
public void SetPosition(MapNode node, Point newPosition, AgvDirection motorDirection)
{
// 현재 위치를 이전 위치로 저장 (리프트 방향 계산용)
if (_currentPosition != Point.Empty)
{
_targetPosition = _currentPosition; // 이전 위치
_targetDirection = _currentDirection;
_targetNode = node;
}
// 새로운 위치 설정
_currentPosition = newPosition;
_currentDirection = motorDirection;
_currentNode = node;
// 위치 변경 이벤트 발생
PositionChanged?.Invoke(this, (_currentPosition, _currentDirection, _currentNode));
}
/// <summary>
/// 현재 RFID 시뮬레이션 (현재 위치 기준)
/// </summary>
public string SimulateRfidReading(List<MapNode> mapNodes)
{
var closestNode = FindClosestNode(_currentPosition, mapNodes);
if (closestNode == null)
return null;
return closestNode.HasRfid() ? closestNode.RfidId : null;
}
#endregion
#region Private Methods
private void StartMovement()
{
SetState(AGVState.Moving);
_isMoving = true;
_lastUpdateTime = DateTime.Now;
}
private void UpdateMovement(float deltaTime)
{
if (_currentState != AGVState.Moving || !_isMoving)
return;
// 목표 위치까지의 거리 계산
var distance = CalculateDistance(_currentPosition, _moveTargetPosition);
if (distance < 5.0f) // 도달 임계값
{
// 목표 도달
_currentPosition = _moveTargetPosition;
_currentSpeed = 0;
// 다음 노드로 이동
ProcessNextNode();
}
else
{
// 계속 이동
var moveDistance = _moveSpeed * deltaTime;
var direction = new PointF(
_moveTargetPosition.X - _currentPosition.X,
_moveTargetPosition.Y - _currentPosition.Y
);
// 정규화
var length = (float)Math.Sqrt(direction.X * direction.X + direction.Y * direction.Y);
if (length > 0)
{
direction.X /= length;
direction.Y /= length;
}
// 새 위치 계산
_currentPosition = new Point(
(int)(_currentPosition.X + direction.X * moveDistance),
(int)(_currentPosition.Y + direction.Y * moveDistance)
);
_currentSpeed = _moveSpeed;
}
}
private void UpdateBattery(float deltaTime)
{
// 배터리 소모 시뮬레이션
if (_currentState == AGVState.Moving)
{
BatteryLevel -= 0.1f * deltaTime; // 이동시 소모
}
else if (_currentState == AGVState.Charging)
{
BatteryLevel += 5.0f * deltaTime; // 충전
BatteryLevel = Math.Min(100.0f, BatteryLevel);
}
BatteryLevel = Math.Max(0, BatteryLevel);
}
private void ProcessNextNode()
{
if (_remainingNodes == null || _currentNodeIndex >= _remainingNodes.Count - 1)
{
// 경로 완료
_isMoving = false;
SetState(AGVState.Idle);
PathCompleted?.Invoke(this, _currentPath);
return;
}
// 다음 노드로 이동
_currentNodeIndex++;
var nextNodeId = _remainingNodes[_currentNodeIndex];
// RFID 감지 시뮬레이션
RfidDetected?.Invoke(this, $"RFID_{nextNodeId}");
// 다음 목표 위치 설정 (실제로는 맵에서 좌표 가져와야 함)
var random = new Random();
_moveTargetPosition = new Point(
_currentPosition.X + random.Next(-100, 100),
_currentPosition.Y + random.Next(-100, 100)
);
}
private MapNode FindClosestNode(Point position, List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0)
return null;
MapNode closestNode = null;
float closestDistance = float.MaxValue;
foreach (var node in mapNodes)
{
var distance = CalculateDistance(position, node.Position);
if (distance < closestDistance)
{
closestDistance = distance;
closestNode = node;
}
}
return closestDistance < 50.0f ? closestNode : null;
}
private float CalculateDistance(Point from, Point to)
{
var dx = to.X - from.X;
var dy = to.Y - from.Y;
return (float)Math.Sqrt(dx * dx + dy * dy);
}
private void SetState(AGVState newState)
{
if (_currentState != newState)
{
_currentState = newState;
StateChanged?.Invoke(this, newState);
}
}
private DockingDirection GetDockingDirection(NodeType nodeType)
{
switch (nodeType)
{
case NodeType.Charging:
return DockingDirection.Forward;
case NodeType.Docking:
return DockingDirection.Backward;
default:
return DockingDirection.Forward;
}
}
private void OnError(string message)
{
SetState(AGVState.Error);
ErrorOccurred?.Invoke(this, message);
}
#endregion
#region Cleanup
/// <summary>
/// 리소스 정리
/// </summary>
public void Dispose()
{
StopPath();
}
#endregion
}
}

305
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Analysis/JunctionAnalyzer.cs Normal file
View File

@@ -0,0 +1,305 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.PathFinding.Analysis
{
/// <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;
}
}
}

371
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs Normal file
View File

@@ -0,0 +1,371 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// AGV 경로 계산 결과 (방향성 및 명령어 포함)
/// </summary>
public class AGVPathResult
{
/// <summary>
/// 경로 찾기 성공 여부
/// </summary>
public bool Success { get; set; }
/// <summary>
/// 경로 노드 ID 목록 (시작 → 목적지 순서)
/// </summary>
public List<string> Path { get; set; }
/// <summary>
/// AGV 명령어 목록 (이동 방향 시퀀스)
/// </summary>
public List<AgvDirection> Commands { get; set; }
/// <summary>
/// 노드별 모터방향 정보 목록
/// </summary>
public List<NodeMotorInfo> NodeMotorInfos { get; set; }
/// <summary>
/// 총 거리
/// </summary>
public float TotalDistance { get; set; }
/// <summary>
/// 계산 소요 시간 (밀리초)
/// </summary>
public long CalculationTimeMs { get; set; }
/// <summary>
/// 탐색된 노드 수
/// </summary>
public int ExploredNodeCount { get; set; }
/// <summary>
/// 탐색된 노드 수 (호환성용)
/// </summary>
public int ExploredNodes
{
get => ExploredNodeCount;
set => ExploredNodeCount = value;
}
/// <summary>
/// 예상 소요 시간 (초)
/// </summary>
public float EstimatedTimeSeconds { get; set; }
/// <summary>
/// 회전 횟수
/// </summary>
public int RotationCount { get; set; }
/// <summary>
/// 오류 메시지 (실패시)
/// </summary>
public string ErrorMessage { get; set; }
/// <summary>
/// 도킹 검증 결과
/// </summary>
public DockingValidationResult DockingValidation { get; set; }
/// <summary>
/// 상세 경로 정보 (NodeMotorInfo 목록)
/// </summary>
public List<NodeMotorInfo> DetailedPath { get; set; }
/// <summary>
/// 계획 설명
/// </summary>
public string PlanDescription { get; set; }
/// <summary>
/// 방향 전환 필요 여부
/// </summary>
public bool RequiredDirectionChange { get; set; }
/// <summary>
/// 방향 전환 노드 ID
/// </summary>
public string DirectionChangeNode { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public AGVPathResult()
{
Success = false;
Path = new List<string>();
Commands = new List<AgvDirection>();
NodeMotorInfos = new List<NodeMotorInfo>();
DetailedPath = new List<NodeMotorInfo>();
TotalDistance = 0;
CalculationTimeMs = 0;
ExploredNodes = 0;
EstimatedTimeSeconds = 0;
RotationCount = 0;
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
RequiredDirectionChange = false;
DirectionChangeNode = string.Empty;
DockingValidation = DockingValidationResult.CreateNotRequired();
}
/// <summary>
/// 성공 결과 생성
/// </summary>
/// <param name="path">경로</param>
/// <param name="commands">AGV 명령어 목록</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<string> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<string>(path),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs
};
result.CalculateMetrics();
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>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <returns>실패 결과</returns>
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs)
{
return new AGVPathResult
{
Success = false,
ErrorMessage = errorMessage,
CalculationTimeMs = calculationTimeMs
};
}
/// <summary>
/// 실패 결과 생성 (확장)
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <returns>실패 결과</returns>
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs, int exploredNodes)
{
return new AGVPathResult
{
Success = false,
ErrorMessage = errorMessage,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes
};
}
/// <summary>
/// 성공 결과 생성 (상세 경로용)
/// </summary>
/// <param name="detailedPath">상세 경로</param>
/// <param name="totalDistance">총 거리</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <param name="planDescription">계획 설명</param>
/// <param name="directionChange">방향 전환 여부</param>
/// <param name="changeNode">방향 전환 노드</param>
/// <returns>성공 결과</returns>
public static AGVPathResult CreateSuccess(List<NodeMotorInfo> detailedPath, float totalDistance, long calculationTimeMs, int exploredNodes, string planDescription, bool directionChange = false, string changeNode = null)
{
var path = detailedPath?.Select(n => n.NodeId).ToList() ?? new List<string>();
var result = new AGVPathResult
{
Success = true,
Path = path,
DetailedPath = detailedPath ?? new List<NodeMotorInfo>(),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes,
PlanDescription = planDescription ?? string.Empty,
RequiredDirectionChange = directionChange,
DirectionChangeNode = changeNode ?? string.Empty
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 경로 메트릭 계산
/// </summary>
private void CalculateMetrics()
{
RotationCount = CountRotations();
EstimatedTimeSeconds = CalculateEstimatedTime();
}
/// <summary>
/// 회전 횟수 계산
/// </summary>
private int CountRotations()
{
int count = 0;
foreach (var command in Commands)
{
if (command == AgvDirection.Left || command == AgvDirection.Right)
{
count++;
}
}
return count;
}
/// <summary>
/// 예상 소요 시간 계산
/// </summary>
/// <param name="agvSpeed">AGV 속도 (픽셀/초, 기본값: 100)</param>
/// <param name="rotationTime">회전 시간 (초, 기본값: 3)</param>
/// <returns>예상 소요 시간 (초)</returns>
private float CalculateEstimatedTime(float agvSpeed = 100.0f, float rotationTime = 3.0f)
{
float moveTime = TotalDistance / agvSpeed;
float totalRotationTime = RotationCount * rotationTime;
return moveTime + totalRotationTime;
}
/// <summary>
/// 명령어 요약 생성
/// </summary>
/// <returns>명령어 요약 문자열</returns>
public string GetCommandSummary()
{
if (!Success) return "실패";
var summary = new List<string>();
var currentCommand = AgvDirection.Stop;
var count = 0;
foreach (var command in Commands)
{
if (command == currentCommand)
{
count++;
}
else
{
if (count > 0)
{
summary.Add($"{GetCommandText(currentCommand)}×{count}");
}
currentCommand = command;
count = 1;
}
}
if (count > 0)
{
summary.Add($"{GetCommandText(currentCommand)}×{count}");
}
return string.Join(" → ", summary);
}
/// <summary>
/// 명령어 텍스트 반환
/// </summary>
private string GetCommandText(AgvDirection command)
{
switch (command)
{
case AgvDirection.Forward: return "전진";
case AgvDirection.Backward: return "후진";
case AgvDirection.Left: return "좌회전";
case AgvDirection.Right: return "우회전";
case AgvDirection.Stop: return "정지";
default: return command.ToString();
}
}
/// <summary>
/// 상세 경로 정보 반환
/// </summary>
/// <returns>상세 정보 문자열</returns>
public string GetDetailedInfo()
{
if (!Success)
{
return $"경로 계산 실패: {ErrorMessage} (계산시간: {CalculationTimeMs}ms)";
}
return $"경로: {Path.Count}개 노드, 거리: {TotalDistance:F1}px, " +
$"회전: {RotationCount}회, 예상시간: {EstimatedTimeSeconds:F1}초, " +
$"계산시간: {CalculationTimeMs}ms";
}
/// <summary>
/// 단순 경로 목록 반환 (호환성용)
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetSimplePath()
{
if (DetailedPath != null && DetailedPath.Count > 0)
{
return DetailedPath.Select(n => n.NodeId).ToList();
}
return Path ?? new List<string>();
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
if (Success)
{
return $"Success: {Path.Count} nodes, {TotalDistance:F1}px, {RotationCount} rotations, {EstimatedTimeSeconds:F1}s";
}
else
{
return $"Failed: {ErrorMessage}";
}
}
}
}

290
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs Normal file
View File

@@ -0,0 +1,290 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// A* 알고리즘 기반 경로 탐색기
/// </summary>
public class AStarPathfinder
{
private Dictionary<string, PathNode> _nodeMap;
private List<MapNode> _mapNodes;
/// <summary>
/// 휴리스틱 가중치 (기본값: 1.0)
/// 값이 클수록 목적지 방향을 우선시하나 최적 경로를 놓칠 수 있음
/// </summary>
public float HeuristicWeight { get; set; } = 1.0f;
/// <summary>
/// 최대 탐색 노드 수 (무한 루프 방지)
/// </summary>
public int MaxSearchNodes { get; set; } = 1000;
/// <summary>
/// 생성자
/// </summary>
public AStarPathfinder()
{
_nodeMap = new Dictionary<string, PathNode>();
_mapNodes = new List<MapNode>();
}
/// <summary>
/// 맵 노드 설정
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
public void SetMapNodes(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_nodeMap.Clear();
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode())
{
var pathNode = new PathNode(mapNode.NodeId, mapNode.Position);
_nodeMap[mapNode.NodeId] = pathNode;
}
}
// 단일 연결을 양방향으로 확장
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.NodeId))
{
var pathNode = _nodeMap[mapNode.NodeId];
foreach (var connectedNodeId in mapNode.ConnectedNodes)
{
if (_nodeMap.ContainsKey(connectedNodeId))
{
// 양방향 연결 생성 (단일 연결이 양방향을 의미)
if (!pathNode.ConnectedNodes.Contains(connectedNodeId))
{
pathNode.ConnectedNodes.Add(connectedNodeId);
}
var connectedPathNode = _nodeMap[connectedNodeId];
if (!connectedPathNode.ConnectedNodes.Contains(mapNode.NodeId))
{
connectedPathNode.ConnectedNodes.Add(mapNode.NodeId);
}
}
}
}
}
}
/// <summary>
/// 경로 찾기 (A* 알고리즘)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindPath(string startNodeId, string endNodeId)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(startNodeId))
{
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(endNodeId))
{
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
}
if (startNodeId == endNodeId)
{
var singlePath = new List<string> { startNodeId };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
var startNode = _nodeMap[startNodeId];
var endNode = _nodeMap[endNodeId];
var openSet = new List<PathNode>();
var closedSet = new HashSet<string>();
var exploredCount = 0;
startNode.GCost = 0;
startNode.HCost = CalculateHeuristic(startNode, endNode);
startNode.Parent = null;
openSet.Add(startNode);
while (openSet.Count > 0 && exploredCount < MaxSearchNodes)
{
var currentNode = GetLowestFCostNode(openSet);
openSet.Remove(currentNode);
closedSet.Add(currentNode.NodeId);
exploredCount++;
if (currentNode.NodeId == endNodeId)
{
var path = ReconstructPath(currentNode);
var totalDistance = CalculatePathDistance(path);
return AGVPathResult.CreateSuccess(path, new List<AgvDirection>(), totalDistance, stopwatch.ElapsedMilliseconds);
}
foreach (var neighborId in currentNode.ConnectedNodes)
{
if (closedSet.Contains(neighborId) || !_nodeMap.ContainsKey(neighborId))
continue;
var neighbor = _nodeMap[neighborId];
var tentativeGCost = currentNode.GCost + currentNode.DistanceTo(neighbor);
if (!openSet.Contains(neighbor))
{
neighbor.Parent = currentNode;
neighbor.GCost = tentativeGCost;
neighbor.HCost = CalculateHeuristic(neighbor, endNode);
openSet.Add(neighbor);
}
else if (tentativeGCost < neighbor.GCost)
{
neighbor.Parent = currentNode;
neighbor.GCost = tentativeGCost;
}
}
}
return AGVPathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
/// <summary>
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
{
if (targetNodeIds == null || targetNodeIds.Count == 0)
{
return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
}
AGVPathResult bestResult = null;
foreach (var targetId in targetNodeIds)
{
var result = FindPath(startNodeId, targetId);
if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
{
bestResult = result;
}
}
return bestResult ?? AGVPathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
}
/// <summary>
/// 휴리스틱 거리 계산 (유클리드 거리)
/// </summary>
private float CalculateHeuristic(PathNode from, PathNode to)
{
return from.DistanceTo(to) * HeuristicWeight;
}
/// <summary>
/// F cost가 가장 낮은 노드 선택
/// </summary>
private PathNode GetLowestFCostNode(List<PathNode> nodes)
{
PathNode lowest = nodes[0];
foreach (var node in nodes)
{
if (node.FCost < lowest.FCost ||
(Math.Abs(node.FCost - lowest.FCost) < 0.001f && node.HCost < lowest.HCost))
{
lowest = node;
}
}
return lowest;
}
/// <summary>
/// 경로 재구성 (부모 노드를 따라 역추적)
/// </summary>
private List<string> ReconstructPath(PathNode endNode)
{
var path = new List<string>();
var current = endNode;
while (current != null)
{
path.Add(current.NodeId);
current = current.Parent;
}
path.Reverse();
return path;
}
/// <summary>
/// 경로의 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<string> path)
{
if (path.Count < 2) return 0;
float totalDistance = 0;
for (int i = 0; i < path.Count - 1; i++)
{
if (_nodeMap.ContainsKey(path[i]) && _nodeMap.ContainsKey(path[i + 1]))
{
totalDistance += _nodeMap[path[i]].DistanceTo(_nodeMap[path[i + 1]]);
}
}
return totalDistance;
}
/// <summary>
/// 두 노드가 연결되어 있는지 확인
/// </summary>
/// <param name="nodeId1">노드 1 ID</param>
/// <param name="nodeId2">노드 2 ID</param>
/// <returns>연결 여부</returns>
public bool AreNodesConnected(string nodeId1, string nodeId2)
{
if (!_nodeMap.ContainsKey(nodeId1) || !_nodeMap.ContainsKey(nodeId2))
return false;
return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
}
/// <summary>
/// 네비게이션 가능한 노드 목록 반환
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetNavigationNodes()
{
return _nodeMap.Keys.ToList();
}
/// <summary>
/// 노드 정보 반환
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <returns>노드 정보 또는 null</returns>
public PathNode GetNode(string nodeId)
{
return _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
}
}
}

101
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/PathNode.cs Normal file
View File

@@ -0,0 +1,101 @@
using System;
using System.Drawing;
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// A* 알고리즘에서 사용하는 경로 노드
/// </summary>
public class PathNode
{
/// <summary>
/// 노드 ID
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// 노드 위치
/// </summary>
public Point Position { get; set; }
/// <summary>
/// 시작점으로부터의 실제 거리 (G cost)
/// </summary>
public float GCost { get; set; }
/// <summary>
/// 목적지까지의 추정 거리 (H cost - 휴리스틱)
/// </summary>
public float HCost { get; set; }
/// <summary>
/// 총 비용 (F cost = G cost + H cost)
/// </summary>
public float FCost => GCost + HCost;
/// <summary>
/// 부모 노드 (경로 추적용)
/// </summary>
public PathNode Parent { get; set; }
/// <summary>
/// 연결된 노드 ID 목록
/// </summary>
public System.Collections.Generic.List<string> ConnectedNodes { get; set; }
/// <summary>
/// 생성자
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <param name="position">위치</param>
public PathNode(string nodeId, Point position)
{
NodeId = nodeId;
Position = position;
GCost = 0;
HCost = 0;
Parent = null;
ConnectedNodes = new System.Collections.Generic.List<string>();
}
/// <summary>
/// 다른 노드까지의 유클리드 거리 계산
/// </summary>
/// <param name="other">대상 노드</param>
/// <returns>거리</returns>
public float DistanceTo(PathNode other)
{
float dx = Position.X - other.Position.X;
float dy = Position.Y - other.Position.Y;
return (float)Math.Sqrt(dx * dx + dy * dy);
}
/// <summary>
/// 문자열 표현
/// </summary>
public override string ToString()
{
return $"{NodeId} - F:{FCost:F1} G:{GCost:F1} H:{HCost:F1}";
}
/// <summary>
/// 같음 비교 (NodeId 기준)
/// </summary>
public override bool Equals(object obj)
{
if (obj is PathNode other)
{
return NodeId == other.NodeId;
}
return false;
}
/// <summary>
/// 해시코드 (NodeId 기준)
/// </summary>
public override int GetHashCode()
{
return NodeId?.GetHashCode() ?? 0;
}
}
}

377
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs Normal file
View File

@@ -0,0 +1,377 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.Utils;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 경로 계획기
/// 물리적 제약사항과 마그넷 센서를 고려한 실제 AGV 경로 생성
/// </summary>
public class AGVPathfinder
{
private readonly List<MapNode> _mapNodes;
private readonly AStarPathfinder _basicPathfinder;
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly DirectionChangePlanner _directionChangePlanner;
public AGVPathfinder(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_basicPathfinder = new AStarPathfinder();
_basicPathfinder.SetMapNodes(_mapNodes);
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
_directionChangePlanner = new DirectionChangePlanner(_mapNodes);
}
/// <summary>
/// AGV 경로 계산
/// </summary>
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode,
MapNode prevNode, AgvDirection currentDirection = AgvDirection.Forward)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
// 입력 검증
if (startNode == null)
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
if (targetNode == null)
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
if (prevNode == null)
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
// 1. 목적지 도킹 방향 요구사항 확인 (노드의 도킹방향 속성에서 확인)
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
// 통합된 경로 계획 함수 사용
AGVPathResult result = PlanPath(startNode, targetNode, prevNode, requiredDirection, currentDirection);
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
// 도킹 검증 수행
if (result.Success && _mapNodes != null)
{
result.DockingValidation = DockingValidator.ValidateDockingDirection(result, _mapNodes, currentDirection);
}
return result;
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
/// <summary>
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
/// </summary>
private AgvDirection? GetRequiredDockingDirection(DockingDirection dockDirection)
{
switch (dockDirection)
{
case DockingDirection.Forward:
return AgvDirection.Forward; // 전진 도킹
case DockingDirection.Backward:
return AgvDirection.Backward; // 후진 도킹
case DockingDirection.DontCare:
default:
return null; // 도킹 방향 상관없음
}
}
/// <summary>
/// 통합 경로 계획 (직접 경로 또는 방향 전환 경로)
/// </summary>
private AGVPathResult PlanPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection? requiredDirection = null, AgvDirection currentDirection = AgvDirection.Forward)
{
bool needDirectionChange = requiredDirection.HasValue && (currentDirection != requiredDirection.Value);
//현재 위치에서 목적지까지의 최단 거리 모록을 찾는다.
var DirectPathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
//이전 위치에서 목적지까지의 최단 거리를 모록을 찾는다.
var DirectPathResultP = _basicPathfinder.FindPath(prevNode.NodeId, targetNode.NodeId);
//
if (DirectPathResultP.Path.Contains(startNode.NodeId))
{
}
if (needDirectionChange)
{
// 방향 전환 경로 계획
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(
startNode.NodeId, targetNode.NodeId, currentDirection, requiredDirection.Value);
if (!directionChangePlan.Success)
{
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
}
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection.Value);
float totalDistance = CalculatePathDistance(detailedPath);
return AGVPathResult.CreateSuccess(
detailedPath,
totalDistance,
0,
0,
directionChangePlan.PlanDescription,
true,
directionChangePlan.DirectionChangeNode
);
}
else
{
// 직접 경로 계획
var basicResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
if (!basicResult.Success)
{
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
}
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
return AGVPathResult.CreateSuccess(
detailedPath,
basicResult.TotalDistance,
basicResult.CalculationTimeMs,
basicResult.ExploredNodeCount,
"직접 경로 - 방향 전환 불필요"
);
}
}
/// <summary>
/// 기본 경로를 상세 경로로 변환
/// </summary>
private List<NodeMotorInfo> ConvertToDetailedPath(List<string> simplePath, AgvDirection initialDirection)
{
var detailedPath = new List<NodeMotorInfo>();
var currentDirection = initialDirection;
for (int i = 0; i < simplePath.Count; i++)
{
string currentNodeId = simplePath[i];
string nextNodeId = (i + 1 < simplePath.Count) ? simplePath[i + 1] : null;
// 마그넷 방향 계산
MagnetDirection magnetDirection = MagnetDirection.Straight;
if (i > 0 && nextNodeId != null)
{
string prevNodeId = simplePath[i - 1];
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
}
// 노드 정보 생성
var nodeMotorInfo = new NodeMotorInfo(
currentNodeId,
currentDirection,
nextNodeId,
magnetDirection
);
// 회전 가능 노드 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
if (mapNode != null)
{
nodeMotorInfo.CanRotate = mapNode.CanRotate;
}
detailedPath.Add(nodeMotorInfo);
}
return detailedPath;
}
/// <summary>
/// 방향 전환 경로를 상세 경로로 변환
/// </summary>
private List<NodeMotorInfo> ConvertDirectionChangePath(DirectionChangePlanner.DirectionChangePlan plan, AgvDirection startDirection, AgvDirection endDirection)
{
var detailedPath = new List<NodeMotorInfo>();
var currentDirection = startDirection;
for (int i = 0; i < plan.DirectionChangePath.Count; i++)
{
string currentNodeId = plan.DirectionChangePath[i];
string nextNodeId = (i + 1 < plan.DirectionChangePath.Count) ? plan.DirectionChangePath[i + 1] : null;
// 방향 전환 노드에서 방향 변경
if (currentNodeId == plan.DirectionChangeNode && currentDirection != endDirection)
{
currentDirection = endDirection;
}
// 마그넷 방향 계산
MagnetDirection magnetDirection = MagnetDirection.Straight;
if (i > 0 && nextNodeId != null)
{
string prevNodeId = plan.DirectionChangePath[i - 1];
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
}
// 특수 동작 확인
bool requiresSpecialAction = false;
string specialActionDescription = "";
if (currentNodeId == plan.DirectionChangeNode)
{
requiresSpecialAction = true;
specialActionDescription = $"방향전환: {startDirection} → {endDirection}";
}
// 노드 정보 생성
var nodeMotorInfo = new NodeMotorInfo(
currentNodeId,
currentDirection,
nextNodeId,
true, // 방향 전환 경로의 경우 회전 가능으로 설정
currentNodeId == plan.DirectionChangeNode,
magnetDirection,
requiresSpecialAction,
specialActionDescription
);
detailedPath.Add(nodeMotorInfo);
}
return detailedPath;
}
/// <summary>
/// 경로 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<NodeMotorInfo> detailedPath)
{
float totalDistance = 0;
for (int i = 0; i < detailedPath.Count - 1; i++)
{
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i].NodeId);
var nextNode = _mapNodes.FirstOrDefault(n => n.NodeId == detailedPath[i + 1].NodeId);
if (currentNode != null && nextNode != null)
{
float dx = nextNode.Position.X - currentNode.Position.X;
float dy = nextNode.Position.Y - currentNode.Position.Y;
totalDistance += (float)Math.Sqrt(dx * dx + dy * dy);
}
}
return totalDistance;
}
/// <summary>
/// 경로 유효성 검증
/// </summary>
public bool ValidatePath(List<NodeMotorInfo> detailedPath)
{
if (detailedPath == null || detailedPath.Count == 0)
return false;
// 1. 모든 노드가 존재하는지 확인
foreach (var nodeInfo in detailedPath)
{
if (!_mapNodes.Any(n => n.NodeId == nodeInfo.NodeId))
return false;
}
// 2. 연결성 확인
for (int i = 0; i < detailedPath.Count - 1; i++)
{
string currentId = detailedPath[i].NodeId;
string nextId = detailedPath[i + 1].NodeId;
if (!_basicPathfinder.AreNodesConnected(currentId, nextId))
return false;
}
// 3. 물리적 제약사항 확인
return ValidatePhysicalConstraints(detailedPath);
}
/// <summary>
/// 물리적 제약사항 검증
/// </summary>
private bool ValidatePhysicalConstraints(List<NodeMotorInfo> detailedPath)
{
for (int i = 1; i < detailedPath.Count; i++)
{
var prevNode = detailedPath[i - 1];
var currentNode = detailedPath[i];
// 급작스러운 방향 전환 검증
if (prevNode.MotorDirection != currentNode.MotorDirection)
{
// 방향 전환은 반드시 회전 가능 노드에서만
if (!currentNode.CanRotate && !currentNode.IsDirectionChangePoint)
{
return false;
}
}
}
return true;
}
/// <summary>
/// 경로 최적화
/// </summary>
public AGVPathResult OptimizePath(AGVPathResult originalResult)
{
if (!originalResult.Success)
return originalResult;
// TODO: 경로 최적화 로직 구현
// - 불필요한 중간 노드 제거
// - 마그넷 방향 최적화
// - 방향 전환 최소화
return originalResult;
}
/// <summary>
/// 디버깅용 경로 정보
/// </summary>
public string GetPathSummary(AGVPathResult result)
{
if (!result.Success)
return $"경로 계산 실패: {result.ErrorMessage}";
var summary = new List<string>
{
$"=== AGV 고급 경로 계획 결과 ===",
$"총 노드 수: {result.DetailedPath.Count}",
$"총 거리: {result.TotalDistance:F1}px",
$"계산 시간: {result.CalculationTimeMs}ms",
$"방향 전환: {(result.RequiredDirectionChange ? $"필요 (노드: {result.DirectionChangeNode})" : "불필요")}",
$"설명: {result.PlanDescription}",
"",
"=== 상세 경로 ===",
};
foreach (var nodeInfo in result.DetailedPath)
{
summary.Add(nodeInfo.ToString());
}
return string.Join("\n", summary);
}
}
}

768
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/DirectionChangePlanner.cs Normal file
View File

@@ -0,0 +1,768 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Analysis;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 방향 전환 경로 계획 시스템
/// 물리적 제약사항을 고려한 방향 전환 경로 생성
/// </summary>
public class DirectionChangePlanner
{
/// <summary>
/// 방향 전환 계획 결과
/// </summary>
public class DirectionChangePlan
{
public bool Success { get; set; }
public List<string> DirectionChangePath { get; set; }
public string DirectionChangeNode { get; set; }
public string ErrorMessage { get; set; }
public string PlanDescription { get; set; }
public DirectionChangePlan()
{
DirectionChangePath = new List<string>();
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
}
public static DirectionChangePlan CreateSuccess(List<string> path, string changeNode, string description)
{
return new DirectionChangePlan
{
Success = true,
DirectionChangePath = path,
DirectionChangeNode = changeNode,
PlanDescription = description
};
}
public static DirectionChangePlan CreateFailure(string error)
{
return new DirectionChangePlan
{
Success = false,
ErrorMessage = error
};
}
}
private readonly List<MapNode> _mapNodes;
private readonly JunctionAnalyzer _junctionAnalyzer;
private readonly AStarPathfinder _pathfinder;
public DirectionChangePlanner(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_junctionAnalyzer = new JunctionAnalyzer(_mapNodes);
_pathfinder = new AStarPathfinder();
_pathfinder.SetMapNodes(_mapNodes);
}
/// <summary>
/// 방향 전환이 필요한 경로 계획
/// </summary>
public DirectionChangePlan PlanDirectionChange(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
// 방향이 같으면 직접 경로 계산
if (currentDirection == requiredDirection)
{
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath.Success)
{
return DirectionChangePlan.CreateSuccess(
directPath.Path,
null,
"방향 전환 불필요 - 직접 경로 사용"
);
}
}
// 방향 전환이 필요한 경우 - 먼저 간단한 직접 경로 확인
var directPath2 = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath2.Success)
{
// 직접 경로에 갈림길이 포함된 경우 그 갈림길에서 방향 전환
foreach (var nodeId in directPath2.Path.Skip(1).Take(directPath2.Path.Count - 2)) // 시작과 끝 제외
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 간단한 방향 전환: 직접 경로 사용하되 방향 전환 노드 표시
return DirectionChangePlan.CreateSuccess(
directPath2.Path,
nodeId,
$"갈림길 {nodeId}에서 방향 전환: {currentDirection} → {requiredDirection}"
);
}
}
}
// 복잡한 방향 전환이 필요한 경우
return PlanDirectionChangeRoute(startNodeId, targetNodeId, currentDirection, requiredDirection);
}
/// <summary>
/// 방향 전환 경로 계획
/// </summary>
private DirectionChangePlan PlanDirectionChangeRoute(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
// 1. 방향 전환 가능한 갈림길 찾기
var changeJunctions = FindSuitableChangeJunctions(startNodeId, targetNodeId, currentDirection, requiredDirection);
if (changeJunctions.Count == 0)
{
return DirectionChangePlan.CreateFailure("방향 전환 가능한 갈림길을 찾을 수 없습니다.");
}
// 2. 각 갈림길에 대해 경로 계획 시도
foreach (var junction in changeJunctions)
{
var plan = TryDirectionChangeAtJunction(startNodeId, targetNodeId, junction, currentDirection, requiredDirection);
if (plan.Success)
{
return plan;
}
}
return DirectionChangePlan.CreateFailure("모든 갈림길에서 방향 전환 경로 계획이 실패했습니다.");
}
/// <summary>
/// 방향 전환에 적합한 갈림길 검색 (인근 우회 경로 우선)
/// </summary>
private List<string> FindSuitableChangeJunctions(string startNodeId, string targetNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var suitableJunctions = new List<string>();
// 1. 시작점 인근의 갈림길들을 우선 검색 (경로 진행 중 우회용)
var nearbyJunctions = FindNearbyJunctions(startNodeId, 2); // 2단계 내의 갈림길
foreach (var junction in nearbyJunctions)
{
if (junction == startNodeId) continue; // 시작점 제외
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junction);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 이 갈림길을 통해 목적지로 갈 수 있는지 확인
if (CanReachTargetViaJunction(junction, targetNodeId) &&
HasSuitableDetourOptions(junction, startNodeId))
{
suitableJunctions.Add(junction);
}
}
}
// 2. 직진 경로상의 갈림길들도 검색 (단, 되돌아가기 방지)
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
if (directPath.Success)
{
foreach (var nodeId in directPath.Path.Skip(2)) // 시작점과 다음 노드는 제외
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(nodeId);
if (junctionInfo != null && junctionInfo.IsJunction)
{
// 직진 경로상에서는 더 엄격한 조건 적용
if (!suitableJunctions.Contains(nodeId) &&
HasMultipleExitOptions(nodeId))
{
suitableJunctions.Add(nodeId);
}
}
}
}
// 거리순으로 정렬 (가까운 갈림길 우선 - 인근 우회용)
return SortJunctionsByDistance(startNodeId, suitableJunctions);
}
/// <summary>
/// 특정 노드 주변의 갈림길 검색
/// </summary>
private List<string> FindNearbyJunctions(string nodeId, int maxSteps)
{
var junctions = new List<string>();
var visited = new HashSet<string>();
var queue = new Queue<(string NodeId, int Steps)>();
queue.Enqueue((nodeId, 0));
visited.Add(nodeId);
while (queue.Count > 0)
{
var (currentNodeId, steps) = queue.Dequeue();
if (steps > maxSteps) continue;
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(currentNodeId);
if (junctionInfo != null && junctionInfo.IsJunction && currentNodeId != nodeId)
{
junctions.Add(currentNodeId);
}
// 연결된 노드들을 큐에 추가
var connectedNodes = GetAllConnectedNodes(currentNodeId);
foreach (var connectedId in connectedNodes)
{
if (!visited.Contains(connectedId))
{
visited.Add(connectedId);
queue.Enqueue((connectedId, steps + 1));
}
}
}
return junctions;
}
/// <summary>
/// 양방향 연결을 고려한 연결 노드 검색
/// </summary>
private List<string> GetAllConnectedNodes(string nodeId)
{
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (node == null) return new List<string>();
var connected = new HashSet<string>();
// 직접 연결
foreach (var connectedId in node.ConnectedNodes)
{
connected.Add(connectedId);
}
// 역방향 연결
foreach (var otherNode in _mapNodes)
{
if (otherNode.NodeId != nodeId && otherNode.ConnectedNodes.Contains(nodeId))
{
connected.Add(otherNode.NodeId);
}
}
return connected.ToList();
}
/// <summary>
/// 갈림길을 거리순으로 정렬
/// </summary>
private List<string> SortJunctionsByDistance(string startNodeId, List<string> junctions)
{
var distances = new List<(string NodeId, double Distance)>();
foreach (var junction in junctions)
{
var path = _pathfinder.FindPath(startNodeId, junction);
double distance = path.Success ? path.TotalDistance : double.MaxValue;
distances.Add((junction, distance));
}
return distances.OrderBy(d => d.Distance).Select(d => d.NodeId).ToList();
}
/// <summary>
/// 특정 갈림길에서 방향 전환 시도
/// </summary>
private DirectionChangePlan TryDirectionChangeAtJunction(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
try
{
// 방향 전환 경로 생성
var changePath = GenerateDirectionChangePath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
if (changePath.Count > 0)
{
// **VALIDATION**: 되돌아가기 패턴 검증
var validationResult = ValidateDirectionChangePath(changePath, startNodeId, junctionNodeId);
if (!validationResult.IsValid)
{
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ❌ 갈림길 {junctionNodeId} 경로 검증 실패: {validationResult.ValidationError}");
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId} 검증 실패: {validationResult.ValidationError}");
}
// 실제 방향 전환 노드 찾기 (우회 노드)
string actualDirectionChangeNode = FindActualDirectionChangeNode(changePath, junctionNodeId);
string description = $"갈림길 {GetDisplayName(junctionNodeId)}를 통해 {GetDisplayName(actualDirectionChangeNode)}에서 방향 전환: {currentDirection} → {requiredDirection}";
System.Diagnostics.Debug.WriteLine($"[DirectionChangePlanner] ✅ 유효한 방향전환 경로: {string.Join(" → ", changePath)}");
return DirectionChangePlan.CreateSuccess(changePath, actualDirectionChangeNode, description);
}
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 방향 전환 경로 생성 실패");
}
catch (Exception ex)
{
return DirectionChangePlan.CreateFailure($"갈림길 {junctionNodeId}에서 오류: {ex.Message}");
}
}
/// <summary>
/// 방향 전환 경로 생성 (인근 갈림길 우회 방식)
/// </summary>
private List<string> GenerateDirectionChangePath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
// 2. 인근 갈림길을 통한 우회인지, 직진 경로상 갈림길인지 판단
var directPath = _pathfinder.FindPath(startNodeId, targetNodeId);
bool isNearbyDetour = !directPath.Success || !directPath.Path.Contains(junctionNodeId);
if (isNearbyDetour)
{
// 인근 갈림길 우회: 직진하다가 마그넷으로 방향 전환
return GenerateNearbyDetourPath(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
}
else
{
// 직진 경로상 갈림길: 기존 방식으로 처리 (단, 되돌아가기 방지)
return GenerateDirectPathChangeRoute(startNodeId, targetNodeId, junctionNodeId, currentDirection, requiredDirection);
}
}
/// <summary>
/// 인근 갈림길을 통한 우회 경로 생성 (예: 012 → 013 → 마그넷으로 016 방향)
/// </summary>
private List<string> GenerateNearbyDetourPath(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
// 1. 시작점에서 갈림길까지 직진 (현재 방향 유지)
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
fullPath.AddRange(toJunctionPath.Path);
// 2. 갈림길에서 방향 전환 후 목적지로
// 이때 마그넷 센서를 이용해 목적지 방향으로 진입
var fromJunctionPath = _pathfinder.FindPath(junctionNodeId, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1)); // 중복 노드 제거
}
return fullPath;
}
/// <summary>
/// 직진 경로상 갈림길에서 방향 전환 경로 생성 (기존 방식 개선)
/// </summary>
private List<string> GenerateDirectPathChangeRoute(string startNodeId, string targetNodeId, string junctionNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var fullPath = new List<string>();
// 1. 시작점에서 갈림길까지의 경로
var toJunctionPath = _pathfinder.FindPath(startNodeId, junctionNodeId);
if (!toJunctionPath.Success)
return fullPath;
fullPath.AddRange(toJunctionPath.Path);
// 2. 갈림길에서 방향 전환 처리 (되돌아가기 방지)
if (currentDirection != requiredDirection)
{
string fromNodeId = toJunctionPath.Path.Count >= 2 ?
toJunctionPath.Path[toJunctionPath.Path.Count - 2] : startNodeId;
var changeSequence = GenerateDirectionChangeSequence(junctionNodeId, fromNodeId, currentDirection, requiredDirection);
if (changeSequence.Count > 1)
{
fullPath.AddRange(changeSequence.Skip(1));
}
}
// 3. 갈림길에서 목표점까지의 경로
string lastNode = fullPath.LastOrDefault() ?? junctionNodeId;
var fromJunctionPath = _pathfinder.FindPath(lastNode, targetNodeId);
if (fromJunctionPath.Success && fromJunctionPath.Path.Count > 1)
{
fullPath.AddRange(fromJunctionPath.Path.Skip(1));
}
return fullPath;
}
/// <summary>
/// 갈림길에서 방향 전환 시퀀스 생성
/// 물리적으로 실현 가능한 방향 전환 경로 생성
/// </summary>
private List<string> GenerateDirectionChangeSequence(string junctionNodeId, string fromNodeId, AgvDirection currentDirection, AgvDirection requiredDirection)
{
var sequence = new List<string> { junctionNodeId };
// 방향이 같으면 변경 불필요
if (currentDirection == requiredDirection)
return sequence;
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return sequence;
// 물리적으로 실현 가능한 방향 전환 시퀀스 생성
// 핵심 원리: AGV는 RFID 태그를 읽자마자 바로 방향전환하면 안됨
// 왔던 길로 되돌아가지 않도록 다른 노드로 우회한 후 방향전환
var connectedNodes = junctionInfo.ConnectedNodes;
// 왔던 노드(fromNodeId)를 제외한 연결 노드들만 후보로 선택
// 이렇게 해야 AGV가 되돌아가는 것을 방지할 수 있음
var availableNodes = connectedNodes.Where(nodeId => nodeId != fromNodeId).ToList();
if (availableNodes.Count > 0)
{
// 방향 전환을 위한 우회 경로 생성
// 예시: 003→004(전진) 상태에서 후진 필요한 경우
// 잘못된 방법: 004→003 (왔던 길로 되돌아감)
// 올바른 방법: 004→005→004 (005로 우회하여 방향전환)
// 가장 적합한 우회 노드 선택 (직진 방향 우선, 각도 변화 최소)
string detourNode = FindBestDetourNode(junctionNodeId, availableNodes, fromNodeId);
if (!string.IsNullOrEmpty(detourNode))
{
// 1단계: 갈림길에서 우회 노드로 이동 (현재 방향 유지)
// AGV는 계속 전진하여 한 태그 더 지나감
sequence.Add(detourNode);
// 2단계: 우회 노드에서 갈림길로 다시 돌아옴 (요구 방향으로 변경)
// 이때 AGV는 안전한 위치에서 방향을 전환할 수 있음
sequence.Add(junctionNodeId);
}
}
else
{
// 사용 가능한 우회 노드가 없는 경우 (2갈래 길목)
// 이 경우 물리적으로 방향 전환이 불가능할 수 있음
// 별도의 처리 로직이 필요할 수 있음
return sequence;
}
return sequence;
}
/// <summary>
/// 방향 전환을 위한 최적의 우회 노드 선택
/// AGV의 물리적 특성을 고려한 각도 기반 선택
/// </summary>
private string FindBestDetourNode(string junctionNodeId, List<string> availableNodes, string excludeNodeId)
{
// 왔던 길(excludeNodeId)를 제외한 노드 중에서 최적의 우회 노드 선택
// 우선순위: 1) 막다른 길이 아닌 노드 (우회 후 복귀 가능) 2) 직진방향 3) 목적지 방향
var junctionNode = _mapNodes.FirstOrDefault(n => n.NodeId == junctionNodeId);
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == excludeNodeId);
if (junctionNode == null || fromNode == null)
return availableNodes.FirstOrDefault();
string bestNode = null;
double minAngleChange = double.MaxValue;
bool foundNonDeadEnd = false;
// AGV가 들어온 방향 벡터 계산 (fromNode → junctionNode)
double incomingAngle = CalculateAngle(fromNode.Position, junctionNode.Position);
foreach (var nodeId in availableNodes)
{
if (nodeId == excludeNodeId) continue; // 왔던 길 제외
var candidateNode = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (candidateNode == null) continue;
// 갈림길에서 후보 노드로의 방향 벡터 계산 (junctionNode → candidateNode)
double outgoingAngle = CalculateAngle(junctionNode.Position, candidateNode.Position);
// 방향 변화 각도 계산 (0도가 직진, 180도가 유턴)
double angleChange = CalculateAngleChange(incomingAngle, outgoingAngle);
// 막다른 길 여부 확인
var nodeConnections = GetAllConnectedNodes(nodeId);
bool isDeadEnd = nodeConnections.Count <= 1;
// 최적 노드 선택 로직
bool shouldUpdate = false;
if (!foundNonDeadEnd && !isDeadEnd)
{
// 첫 번째 막다른 길이 아닌 노드 발견
shouldUpdate = true;
foundNonDeadEnd = true;
}
else if (foundNonDeadEnd && isDeadEnd)
{
// 이미 막다른 길이 아닌 노드를 찾았으므로 막다른 길은 제외
continue;
}
else if (foundNonDeadEnd == isDeadEnd)
{
// 같은 조건(둘 다 막다른길 or 둘 다 아님)에서는 각도가 작은 것 선택
shouldUpdate = angleChange < minAngleChange;
}
if (shouldUpdate)
{
minAngleChange = angleChange;
bestNode = nodeId;
}
}
return bestNode ?? availableNodes.FirstOrDefault(n => n != excludeNodeId);
}
/// <summary>
/// 두 점 사이의 각도 계산 (라디안 단위)
/// </summary>
private double CalculateAngle(System.Drawing.Point from, System.Drawing.Point to)
{
double dx = to.X - from.X;
double dy = to.Y - from.Y;
return Math.Atan2(dy, dx);
}
/// <summary>
/// 두 방향 사이의 각도 변화량 계산 (0~180도 범위)
/// 0도에 가까울수록 직진, 180도에 가까울수록 유턴
/// </summary>
private double CalculateAngleChange(double fromAngle, double toAngle)
{
// 각도 차이 계산
double angleDiff = Math.Abs(toAngle - fromAngle);
// 0~π 범위로 정규화 (0~180도)
if (angleDiff > Math.PI)
{
angleDiff = 2 * Math.PI - angleDiff;
}
return angleDiff;
}
/// <summary>
/// 실제 방향 전환이 일어나는 노드 찾기
/// </summary>
private string FindActualDirectionChangeNode(List<string> changePath, string junctionNodeId)
{
// 방향전환 경로 구조: [start...junction, detourNode, junction...target]
// 실제 방향전환은 detourNode에서 일어남 (AGV가 한 태그 더 지나간 후)
if (changePath.Count < 3)
return junctionNodeId; // 기본값으로 갈림길 반환
// 갈림길이 두 번 나타나는 위치 찾기
int firstJunctionIndex = changePath.IndexOf(junctionNodeId);
int lastJunctionIndex = changePath.LastIndexOf(junctionNodeId);
// 갈림길이 두 번 나타나고, 그 사이에 노드가 있는 경우
if (firstJunctionIndex != lastJunctionIndex && lastJunctionIndex - firstJunctionIndex == 2)
{
// 첫 번째와 두 번째 갈림길 사이에 있는 노드가 실제 방향전환 노드
string detourNode = changePath[firstJunctionIndex + 1];
return detourNode;
}
// 방향전환 구조를 찾지 못한 경우 기본값 반환
return junctionNodeId;
}
/// <summary>
/// 갈림길에서 적절한 우회 옵션이 있는지 확인
/// </summary>
private bool HasSuitableDetourOptions(string junctionNodeId, string excludeNodeId)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return false;
// 제외할 노드(직전 노드)를 뺀 연결된 노드가 2개 이상이어야 적절한 우회 가능
var availableConnections = junctionInfo.ConnectedNodes
.Where(nodeId => nodeId != excludeNodeId)
.ToList();
// 최소 2개의 우회 옵션이 있어야 함 (갈림길에서 방향전환 후 다시 나갈 수 있어야 함)
return availableConnections.Count >= 2;
}
/// <summary>
/// 갈림길을 통해 목적지에 도달할 수 있는지 확인
/// </summary>
private bool CanReachTargetViaJunction(string junctionNodeId, string targetNodeId)
{
// 갈림길에서 목적지까지의 경로가 존재하는지 확인
var pathToTarget = _pathfinder.FindPath(junctionNodeId, targetNodeId);
return pathToTarget.Success;
}
/// <summary>
/// 갈림길에서 여러 출구 옵션이 있는지 확인 (직진 경로상 갈림길용)
/// </summary>
private bool HasMultipleExitOptions(string junctionNodeId)
{
var junctionInfo = _junctionAnalyzer.GetJunctionInfo(junctionNodeId);
if (junctionInfo == null || !junctionInfo.IsJunction)
return false;
// 최소 3개 이상의 연결 노드가 있어야 적절한 방향전환 가능
return junctionInfo.ConnectedNodes.Count >= 3;
}
/// <summary>
/// 방향전환 경로 검증 - 되돌아가기 패턴 및 물리적 실현성 검증
/// </summary>
private PathValidationResult ValidateDirectionChangePath(List<string> path, string startNodeId, string junctionNodeId)
{
if (path == null || path.Count == 0)
{
return PathValidationResult.CreateInvalid(startNodeId, "", "경로가 비어있습니다.");
}
// 1. 되돌아가기 패턴 검증 (A → B → A)
var backtrackingPatterns = DetectBacktrackingPatterns(path);
if (backtrackingPatterns.Count > 0)
{
var issues = new List<string>();
foreach (var pattern in backtrackingPatterns)
{
issues.Add($"되돌아가기 패턴 발견: {pattern}");
}
string errorMessage = $"되돌아가기 패턴 검출 ({backtrackingPatterns.Count}개): {string.Join(", ", issues)}";
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 경로: {string.Join(" → ", path)}");
System.Diagnostics.Debug.WriteLine($"[PathValidation] ❌ 되돌아가기 패턴: {errorMessage}");
return PathValidationResult.CreateInvalidWithBacktracking(
path, backtrackingPatterns, startNodeId, "", junctionNodeId, errorMessage);
}
// 2. 연속된 중복 노드 검증
var duplicates = DetectConsecutiveDuplicates(path);
if (duplicates.Count > 0)
{
string errorMessage = $"연속된 중복 노드 발견: {string.Join(", ", duplicates)}";
return PathValidationResult.CreateInvalid(startNodeId, "", errorMessage);
}
// 3. 경로 연결성 검증
var connectivity = ValidatePathConnectivity(path);
if (!connectivity.IsValid)
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"경로 연결성 오류: {connectivity.ValidationError}");
}
// 4. 갈림길 포함 여부 검증
if (!path.Contains(junctionNodeId))
{
return PathValidationResult.CreateInvalid(startNodeId, "", $"갈림길 {junctionNodeId}이 경로에 포함되지 않음");
}
System.Diagnostics.Debug.WriteLine($"[PathValidation] ✅ 유효한 경로: {string.Join(" → ", path)}");
return PathValidationResult.CreateValid(path, startNodeId, "", junctionNodeId);
}
/// <summary>
/// 되돌아가기 패턴 검출 (A → B → A)
/// </summary>
private List<BacktrackingPattern> DetectBacktrackingPatterns(List<string> path)
{
var patterns = new List<BacktrackingPattern>();
for (int i = 0; i < path.Count - 2; i++)
{
string nodeA = path[i];
string nodeB = path[i + 1];
string nodeC = path[i + 2];
// A → B → A 패턴 검출
if (nodeA == nodeC && nodeA != nodeB)
{
var pattern = BacktrackingPattern.Create(nodeA, nodeB, nodeA, i, i + 2);
patterns.Add(pattern);
}
}
return patterns;
}
/// <summary>
/// 연속된 중복 노드 검출
/// </summary>
private List<string> DetectConsecutiveDuplicates(List<string> path)
{
var duplicates = new List<string>();
for (int i = 0; i < path.Count - 1; i++)
{
if (path[i] == path[i + 1])
{
duplicates.Add(path[i]);
}
}
return duplicates;
}
/// <summary>
/// 경로 연결성 검증
/// </summary>
private PathValidationResult ValidatePathConnectivity(List<string> path)
{
for (int i = 0; i < path.Count - 1; i++)
{
string currentNode = path[i];
string nextNode = path[i + 1];
// 두 노드간 직접 연결성 확인 (맵 노드의 ConnectedNodes 리스트 사용)
var currentMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNode);
if (currentMapNode == null || !currentMapNode.ConnectedNodes.Contains(nextNode))
{
return PathValidationResult.CreateInvalid(currentNode, nextNode, $"노드 {currentNode}와 {nextNode} 사이에 연결이 없음");
}
}
return PathValidationResult.CreateNotRequired();
}
/// <summary>
/// 두 점 사이의 거리 계산
/// </summary>
private float CalculateDistance(System.Drawing.Point p1, System.Drawing.Point p2)
{
float dx = p2.X - p1.X;
float dy = p2.Y - p1.Y;
return (float)Math.Sqrt(dx * dx + dy * dy);
}
/// <summary>
/// 경로 계획 요약 정보
/// </summary>
public string GetPlanSummary()
{
var junctions = _junctionAnalyzer.GetJunctionSummary();
return string.Join("\n", junctions);
}
/// <summary>
/// 노드의 표시명 가져오기 (RFID 우선, 없으면 (NodeID) 형태)
/// </summary>
/// <param name="nodeId">노드 ID</param>
/// <returns>표시할 이름</returns>
private string GetDisplayName(string nodeId)
{
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
if (node != null && !string.IsNullOrEmpty(node.RfidId))
{
return node.RfidId;
}
return $"({nodeId})";
}
}
}

121
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs Normal file
View File

@@ -0,0 +1,121 @@
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// AGV 마그넷 센서 방향 제어
/// </summary>
public enum MagnetDirection
{
/// <summary>
/// 직진 - 기본 마그넷 라인 추종
/// </summary>
Straight = 0,
/// <summary>
/// 좌측 - 마그넷 센서 가중치를 좌측으로 조정
/// </summary>
Left = 1,
/// <summary>
/// 우측 - 마그넷 센서 가중치를 우측으로 조정
/// </summary>
Right = 2
}
/// <summary>
/// 노드별 모터방향 정보 (방향 전환 지원 포함)
/// </summary>
public class NodeMotorInfo
{
/// <summary>
/// 노드 ID
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// 해당 노드에서의 모터방향
/// </summary>
public AgvDirection MotorDirection { get; set; }
/// <summary>
/// 마그넷 센서 방향 제어 (갈림길 처리용)
/// </summary>
public MagnetDirection MagnetDirection { get; set; }
/// <summary>
/// 다음 노드 ID (경로예측용)
/// </summary>
public string NextNodeId { get; set; }
/// <summary>
/// 회전 가능 노드 여부
/// </summary>
public bool CanRotate { get; set; }
/// <summary>
/// 방향 전환이 발생하는 노드 여부
/// </summary>
public bool IsDirectionChangePoint { get; set; }
/// <summary>
/// 특수 동작이 필요한 노드 여부 (갈림길 전진/후진 반복)
/// </summary>
public bool RequiresSpecialAction { get; set; }
/// <summary>
/// 특수 동작 설명
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
{
NodeId = nodeId;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNodeId = nextNodeId;
CanRotate = false;
IsDirectionChangePoint = false;
RequiresSpecialAction = false;
SpecialActionDescription = string.Empty;
}
/// <summary>
/// 방향 전환 정보를 포함한 생성자
/// </summary>
public NodeMotorInfo(string nodeId, AgvDirection motorDirection, string nextNodeId, bool canRotate, bool isDirectionChangePoint, MagnetDirection magnetDirection = MagnetDirection.Straight, bool requiresSpecialAction = false, string specialActionDescription = "")
{
NodeId = nodeId;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNodeId = nextNodeId;
CanRotate = canRotate;
IsDirectionChangePoint = isDirectionChangePoint;
RequiresSpecialAction = requiresSpecialAction;
SpecialActionDescription = specialActionDescription ?? string.Empty;
}
/// <summary>
/// 디버깅용 문자열 표현
/// </summary>
public override string ToString()
{
var result = $"{NodeId}:{MotorDirection}";
// 마그넷 방향이 직진이 아닌 경우 표시
if (MagnetDirection != MagnetDirection.Straight)
result += $"({MagnetDirection})";
if (IsDirectionChangePoint)
result += " [방향전환]";
if (CanRotate)
result += " [회전가능]";
if (RequiresSpecialAction)
result += $" [특수동작:{SpecialActionDescription}]";
return result;
}
}
}

103
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Validation/DockingValidationResult.cs Normal file
View File

@@ -0,0 +1,103 @@
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Validation
{
/// <summary>
/// 도킹 검증 결과
/// </summary>
public class DockingValidationResult
{
/// <summary>
/// 도킹 검증이 필요한지 여부 (목적지가 도킹 대상인 경우)
/// </summary>
public bool IsValidationRequired { get; set; }
/// <summary>
/// 도킹 검증 통과 여부
/// </summary>
public bool IsValid { get; set; }
/// <summary>
/// 목적지 노드 ID
/// </summary>
public string TargetNodeId { get; set; }
/// <summary>
/// 목적지 노드 타입
/// </summary>
public NodeType TargetNodeType { get; set; }
/// <summary>
/// 필요한 도킹 방향
/// </summary>
public AgvDirection RequiredDockingDirection { get; set; }
/// <summary>
/// 계산된 경로의 마지막 방향
/// </summary>
public AgvDirection CalculatedFinalDirection { get; set; }
/// <summary>
/// 검증 오류 메시지 (실패시)
/// </summary>
public string ValidationError { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public DockingValidationResult()
{
IsValidationRequired = false;
IsValid = true;
TargetNodeId = string.Empty;
RequiredDockingDirection = AgvDirection.Forward;
CalculatedFinalDirection = AgvDirection.Forward;
ValidationError = string.Empty;
}
/// <summary>
/// 검증 불필요한 경우 생성
/// </summary>
public static DockingValidationResult CreateNotRequired()
{
return new DockingValidationResult
{
IsValidationRequired = false,
IsValid = true
};
}
/// <summary>
/// 검증 성공 결과 생성
/// </summary>
public static DockingValidationResult CreateValid(string targetNodeId, NodeType nodeType, AgvDirection requiredDirection, AgvDirection calculatedDirection)
{
return new DockingValidationResult
{
IsValidationRequired = true,
IsValid = true,
TargetNodeId = targetNodeId,
TargetNodeType = nodeType,
RequiredDockingDirection = requiredDirection,
CalculatedFinalDirection = calculatedDirection
};
}
/// <summary>
/// 검증 실패 결과 생성
/// </summary>
public static DockingValidationResult CreateInvalid(string targetNodeId, NodeType nodeType, AgvDirection requiredDirection, AgvDirection calculatedDirection, string error)
{
return new DockingValidationResult
{
IsValidationRequired = true,
IsValid = false,
TargetNodeId = targetNodeId,
TargetNodeType = nodeType,
RequiredDockingDirection = requiredDirection,
CalculatedFinalDirection = calculatedDirection,
ValidationError = error
};
}
}
}

205
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Validation/PathValidationResult.cs Normal file
View File

@@ -0,0 +1,205 @@
using System.Collections.Generic;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Validation
{
/// <summary>
/// 경로 검증 결과 (되돌아가기 패턴 검증 포함)
/// </summary>
public class PathValidationResult
{
/// <summary>
/// 경로 검증이 필요한지 여부
/// </summary>
public bool IsValidationRequired { get; set; }
/// <summary>
/// 경로 검증 통과 여부
/// </summary>
public bool IsValid { get; set; }
/// <summary>
/// 검증된 경로
/// </summary>
public List<string> ValidatedPath { get; set; }
/// <summary>
/// 검출된 되돌아가기 패턴 목록 (A → B → A 형태)
/// </summary>
public List<BacktrackingPattern> BacktrackingPatterns { get; set; }
/// <summary>
/// 갈림길 노드 목록
/// </summary>
public List<string> JunctionNodes { get; set; }
/// <summary>
/// 시작 노드 ID
/// </summary>
public string StartNodeId { get; set; }
/// <summary>
/// 목표 노드 ID
/// </summary>
public string TargetNodeId { get; set; }
/// <summary>
/// 갈림길 노드 ID (방향 전환용)
/// </summary>
public string JunctionNodeId { get; set; }
/// <summary>
/// 검증 오류 메시지 (실패시)
/// </summary>
public string ValidationError { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public PathValidationResult()
{
IsValidationRequired = false;
IsValid = true;
ValidatedPath = new List<string>();
BacktrackingPatterns = new List<BacktrackingPattern>();
JunctionNodes = new List<string>();
StartNodeId = string.Empty;
TargetNodeId = string.Empty;
JunctionNodeId = string.Empty;
ValidationError = string.Empty;
}
/// <summary>
/// 검증 불필요한 경우 생성
/// </summary>
public static PathValidationResult CreateNotRequired()
{
return new PathValidationResult
{
IsValidationRequired = false,
IsValid = true
};
}
/// <summary>
/// 검증 성공 결과 생성
/// </summary>
public static PathValidationResult CreateValid(List<string> path, string startNodeId, string targetNodeId, string junctionNodeId = "")
{
return new PathValidationResult
{
IsValidationRequired = true,
IsValid = true,
ValidatedPath = new List<string>(path),
StartNodeId = startNodeId,
TargetNodeId = targetNodeId,
JunctionNodeId = junctionNodeId
};
}
/// <summary>
/// 검증 실패 결과 생성 (되돌아가기 패턴 검출)
/// </summary>
public static PathValidationResult CreateInvalidWithBacktracking(
List<string> path,
List<BacktrackingPattern> backtrackingPatterns,
string startNodeId,
string targetNodeId,
string junctionNodeId,
string error)
{
return new PathValidationResult
{
IsValidationRequired = true,
IsValid = false,
ValidatedPath = new List<string>(path),
BacktrackingPatterns = new List<BacktrackingPattern>(backtrackingPatterns),
StartNodeId = startNodeId,
TargetNodeId = targetNodeId,
JunctionNodeId = junctionNodeId,
ValidationError = error
};
}
/// <summary>
/// 일반 검증 실패 결과 생성
/// </summary>
public static PathValidationResult CreateInvalid(string startNodeId, string targetNodeId, string error)
{
return new PathValidationResult
{
IsValidationRequired = true,
IsValid = false,
StartNodeId = startNodeId,
TargetNodeId = targetNodeId,
ValidationError = error
};
}
}
/// <summary>
/// 되돌아가기 패턴 정보 (A → B → A)
/// </summary>
public class BacktrackingPattern
{
/// <summary>
/// 시작 노드 (A)
/// </summary>
public string StartNode { get; set; }
/// <summary>
/// 중간 노드 (B)
/// </summary>
public string MiddleNode { get; set; }
/// <summary>
/// 되돌아간 노드 (다시 A)
/// </summary>
public string ReturnNode { get; set; }
/// <summary>
/// 경로에서의 시작 인덱스
/// </summary>
public int StartIndex { get; set; }
/// <summary>
/// 경로에서의 종료 인덱스
/// </summary>
public int EndIndex { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public BacktrackingPattern()
{
StartNode = string.Empty;
MiddleNode = string.Empty;
ReturnNode = string.Empty;
StartIndex = -1;
EndIndex = -1;
}
/// <summary>
/// 되돌아가기 패턴 생성
/// </summary>
public static BacktrackingPattern Create(string startNode, string middleNode, string returnNode, int startIndex, int endIndex)
{
return new BacktrackingPattern
{
StartNode = startNode,
MiddleNode = middleNode,
ReturnNode = returnNode,
StartIndex = startIndex,
EndIndex = endIndex
};
}
/// <summary>
/// 패턴 설명 문자열
/// </summary>
public override string ToString()
{
return $"{StartNode} → {MiddleNode} → {ReturnNode} (인덱스: {StartIndex}-{EndIndex})";
}
}
}

36
Cs_HMI/AGVLogic/AGVNavigationCore/Properties/AssemblyInfo.cs Normal file
View File

@@ -0,0 +1,36 @@
using System.Reflection;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
// General Information about an assembly is controlled through the following
// set of attributes. Change these attribute values to modify the information
// associated with an assembly.
[assembly: AssemblyTitle("AGVNavigationCore")]
[assembly: AssemblyDescription("AGV Navigation and Pathfinding Core Library")]
[assembly: AssemblyConfiguration("")]
[assembly: AssemblyCompany("ENIG")]
[assembly: AssemblyProduct("AGV Navigation System")]
[assembly: AssemblyCopyright("Copyright © ENIG 2024")]
[assembly: AssemblyTrademark("")]
[assembly: AssemblyCulture("")]
// Setting ComVisible to false makes the types in this assembly not visible
// to COM components. If you need to access a type in this assembly from
// COM, set the ComVisible attribute to true on that type.
[assembly: ComVisible(false)]
// The following GUID is for the ID of the typelib if this project is exposed to COM
[assembly: Guid("c5f7a8b2-8d3e-4a1b-9c6e-7f4d5e2a9b1c")]
// Version information for an assembly consists of the following four values:
//
// Major Version
// Minor Version
// Build Number
// Revision
//
// You can specify all the values or you can default the Build and Revision Numbers
// by using the '*' as shown below:
// [assembly: AssemblyVersion("1.0.*")]
[assembly: AssemblyVersion("1.0.0.0")]
[assembly: AssemblyFileVersion("1.0.0.0")]

155
Cs_HMI/AGVLogic/AGVNavigationCore/README.md Normal file
View File

@@ -0,0 +1,155 @@
# AGVNavigationCore
ENIG AGV 시스템을 위한 핵심 네비게이션 및 경로 탐색 라이브러리
## 📋 개요
AGVNavigationCore는 자동 유도 차량(AGV) 시스템의 경로 계획, 맵 편집, 시뮬레이션, 실시간 모니터링 기능을 제공하는 .NET Framework 4.8 라이브러리입니다.
## 🏗️ 프로젝트 구조
### 📁 Controls/
**AGV 관련 사용자 인터페이스 컨트롤 및 AGV 추상화 계층**
- **AGVState.cs** - AGV 상태 열거형 (Idle, Moving, Rotating, Docking, Charging, Error)
- **IAGV.cs** - AGV 인터페이스 정의 (가상/실제 AGV 통합)
- **UnifiedAGVCanvas.cs** - 통합 AGV 캔버스 컨트롤 메인 클래스
- **UnifiedAGVCanvas.Events.cs** - 그리기 및 렌더링 로직 (AGV, 노드, 경로 시각화)
- **UnifiedAGVCanvas.Mouse.cs** - 마우스 이벤트 처리 (클릭, 드래그, 줌, 팬)
### 📁 Models/
**데이터 모델 및 핵심 비즈니스 엔티티 정의**
- **Enums.cs** - 핵심 열거형 정의 (NodeType, DockingDirection, AgvDirection, StationType)
- **MapNode.cs** - 맵 노드 엔티티 클래스 (논리적 노드 ID, 위치, 타입, 연결 정보, RFID 정보)
- **MapLoader.cs** - 맵 파일 로딩/저장 유틸리티 (JSON 직렬화, 데이터 마이그레이션, 검증)
### 📁 PathFinding/
**AGV 경로 탐색 및 계산 알고리즘**
#### 🟢 활발히 사용되는 클래스
- **AGVPathfinder.cs** - 메인 AGV 경로 계획기 (물리적 제약사항 고려)
- **AGVPathResult.cs** - 경로 계산 결과 데이터 클래스
- **DockingValidationResult.cs** - 도킹 검증 결과 데이터 클래스
#### 🟡 내부 구현 클래스
- **AStarPathfinder.cs** - A* 알고리즘 기반 기본 경로 탐색
- **DirectionChangePlanner.cs** - AGV 방향 전환 경로 계획 시스템
- **JunctionAnalyzer.cs** - 교차점 분석 및 마그넷 센서 방향 계산
- **NodeMotorInfo.cs** - 노드별 모터방향 정보 (방향 전환 지원 포함)
- **PathNode.cs** - A* 알고리즘용 경로 노드
### 📁 Utils/
**유틸리티 및 계산 헬퍼 클래스**
- **DockingValidator.cs** - AGV 도킹 방향 검증 유틸리티
- **LiftCalculator.cs** - AGV 리프트 방향 계산 유틸리티
### 📁 Properties/
- **AssemblyInfo.cs** - 어셈블리 정보 및 버전 관리
## 🎯 주요 기능
### 🗺️ 맵 관리
- **논리적 노드 시스템**: 물리적 RFID와 분리된 논리적 노드 ID 관리
- **노드 타입**: Normal, Rotation, Docking, Charging 등 다양한 노드 타입 지원
- **연결 관리**: 노드 간 방향성 연결 관리
- **JSON 저장/로드**: 표준 JSON 형식으로 맵 데이터 관리
### 🧭 경로 탐색
- **A* 알고리즘**: 효율적인 최단 경로 탐색
- **AGV 물리적 제약**: 전진/후진 모터 방향, 회전 제약 고려
- **방향 전환 계획**: 마그넷 센서 위치에서의 방향 전환 최적화
- **도킹 검증**: 목적지 타입에 따른 도킹 방향 검증
### 🎮 시각화 및 편집
- **통합 캔버스**: 맵 편집, 시뮬레이션, 모니터링 모드 지원
- **실시간 렌더링**: AGV 위치, 경로, 상태 실시간 표시
- **인터랙티브 편집**: 드래그앤드롭 노드 편집, 연결 관리
- **줌/팬**: 대형 맵 탐색을 위한 줌/팬 기능
## 🔧 아키텍처 특징
### ✅ 장점
- **계층화 아키텍처**: Models → Utils → PathFinding → Controls 의존성 구조
- **관심사 분리**: 각 폴더별 명확한 책임 분담
- **인터페이스 기반**: IAGV 인터페이스로 가상/실제 AGV 통합
- **확장성**: 새로운 알고리즘, AGV 타입 추가 용이
### ⚠️ 개선 영역
- **코드 크기**: 일부 클래스가 과도하게 큼 (UnifiedAGVCanvas.Events.cs: 1,699행)
- **복잡도**: DirectionChangePlanner 등 복잡한 로직 포함
- **분할 필요**: UnifiedAGVCanvas의 다중 책임 분리 필요
## 🚀 사용 방법
### 기본 맵 로딩
```csharp
var mapLoader = new MapLoader();
var mapNodes = mapLoader.LoadMap("path/to/map.json");
```
### 경로 계산
```csharp
var pathfinder = new AGVPathfinder();
pathfinder.SetMapNodes(mapNodes);
var result = pathfinder.FindPath("START_NODE", "TARGET_NODE", AgvDirection.Forward);
if (result.Success)
{
Console.WriteLine($"경로: {string.Join(" -> ", result.Path)}");
Console.WriteLine($"거리: {result.TotalDistance:F1}px");
}
```
### 캔버스 사용
```csharp
var canvas = new UnifiedAGVCanvas();
canvas.Nodes = mapNodes;
canvas.CurrentPath = result;
canvas.CurrentEditMode = UnifiedAGVCanvas.EditMode.Select;
```
## 📈 최근 업데이트 (2024.12)
### ✅ 완료된 개선사항
- **중복 코드 정리**: PathResult, RfidPathResult 등 중복 클래스 제거
- **아키텍처 통합**: AdvancedAGVPathfinder → AGVPathfinder 통합
- **좌표 정확성**: 줌/팬 시 노드 선택 정확도 개선
- **미사용 코드 제거**: PathfindingOptions 등 미사용 클래스 삭제
### 🔄 진행 중인 개선사항
- **방향 계산 최적화**: 리프트 방향 계산 로직 개선
- **도킹 검증**: 도킹 방향 검증 시스템 강화
- **성능 최적화**: 대형 맵 처리 성능 개선
## 🏃‍♂️ 향후 계획
### 우선순위 1 (즉시)
- UnifiedAGVCanvas 분할 (Rendering, Editing, Simulation 분리)
- [완료] PathFinding 폴더 세분화 (Core, Validation, Planning, Analysis)
### 우선순위 2 (중기)
- 인터페이스 표준화 (I접두사 통일)
- Utils 폴더 확장 (Calculations, Validators, Converters)
### 우선순위 3 (장기)
- 의존성 주입 도입
- 성능 모니터링 시스템
- 단위 테스트 확충
## 📦 의존성
- .NET Framework 4.8
- Newtonsoft.Json 13.0.3
- System.Drawing
- System.Windows.Forms
## 🔗 관련 프로젝트
- **AGVMapEditor**: 맵 편집 전용 애플리케이션
- **AGVSimulator**: AGV 시뮬레이션 애플리케이션
- **AGVCSharp**: 메인 AGV 제어 시스템
## 📞 연락처
ENIG AGV 개발팀 - 2024년 12월 업데이트

217
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs Normal file
View File

@@ -0,0 +1,217 @@
using System;
using System.Collections.Generic;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Core;
using AGVNavigationCore.PathFinding.Validation;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// AGV 도킹 방향 검증 유틸리티
/// 경로 계산 후 마지막 도킹 방향이 올바른지 검증
/// </summary>
public static class DockingValidator
{
/// <summary>
/// 경로의 도킹 방향 검증
/// </summary>
/// <param name="pathResult">경로 계산 결과</param>
/// <param name="mapNodes">맵 노드 목록</param>
/// <param name="currentDirection">AGV 현재 방향</param>
/// <returns>도킹 검증 결과</returns>
public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes, AgvDirection currentDirection)
{
// 경로가 없거나 실패한 경우
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 경로 없음");
return DockingValidationResult.CreateNotRequired();
}
// 목적지 노드 찾기
string targetNodeId = pathResult.Path[pathResult.Path.Count - 1];
var targetNode = mapNodes?.FirstOrDefault(n => n.NodeId == targetNodeId);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {targetNodeId}");
if (targetNode == null)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드 찾을 수 없음: {targetNodeId}");
return DockingValidationResult.CreateNotRequired();
}
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드 타입: {targetNode.Type} ({(int)targetNode.Type})");
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
if (!IsDockingRequired(targetNode.DockDirection))
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {targetNode.DockDirection}");
return DockingValidationResult.CreateNotRequired();
}
// 필요한 도킹 방향 확인
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 필요한 도킹 방향: {requiredDirection}");
// 경로 기반 최종 방향 계산
var calculatedDirection = CalculateFinalDirection(pathResult.Path, mapNodes, currentDirection);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 계산된 최종 방향: {calculatedDirection}");
System.Diagnostics.Debug.WriteLine($"[DockingValidator] AGV 현재 방향: {currentDirection}");
// 검증 수행
if (calculatedDirection == requiredDirection)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ✅ 도킹 검증 성공");
return DockingValidationResult.CreateValid(
targetNodeId,
targetNode.Type,
requiredDirection,
calculatedDirection);
}
else
{
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(calculatedDirection)}";
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
return DockingValidationResult.CreateInvalid(
targetNodeId,
targetNode.Type,
requiredDirection,
calculatedDirection,
error);
}
}
/// <summary>
/// 도킹이 필요한 노드인지 확인 (도킹방향이 DontCare가 아닌 경우)
/// </summary>
private static bool IsDockingRequired(DockingDirection dockDirection)
{
return dockDirection != DockingDirection.DontCare;
}
/// <summary>
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
/// </summary>
private static AgvDirection GetRequiredDockingDirection(DockingDirection dockDirection)
{
switch (dockDirection)
{
case DockingDirection.Forward:
return AgvDirection.Forward; // 전진 도킹
case DockingDirection.Backward:
return AgvDirection.Backward; // 후진 도킹
case DockingDirection.DontCare:
default:
return AgvDirection.Forward; // 기본값 (사실상 사용되지 않음)
}
}
/// <summary>
/// 경로 기반 최종 방향 계산
/// 개선된 구현: 경로 진행 방향과 목적지 노드 타입을 고려
/// </summary>
private static AgvDirection CalculateFinalDirection(List<string> path, List<MapNode> mapNodes, AgvDirection currentDirection)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 입력 - 경로 수: {path?.Count}, 현재 방향: {currentDirection}");
// 경로가 1개 이하면 현재 방향 유지
if (path.Count < 2)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 경로가 짧음, 현재 방향 유지: {currentDirection}");
return currentDirection;
}
// 목적지 노드 확인
var lastNodeId = path[path.Count - 1];
var lastNode = mapNodes?.FirstOrDefault(n => n.NodeId == lastNodeId);
if (lastNode == null)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 목적지 노드 찾을 수 없음: {lastNodeId}");
return currentDirection;
}
// 도킹 노드인 경우, 필요한 도킹 방향으로 설정
if (IsDockingRequired(lastNode.DockDirection))
{
var requiredDockingDirection = GetRequiredDockingDirection(lastNode.DockDirection);
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 도킹 노드(DockDirection={lastNode.DockDirection}) 감지, 필요 방향: {requiredDockingDirection}");
// 현재 방향이 필요한 도킹 방향과 다르면 경고 로그
if (currentDirection != requiredDockingDirection)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] ⚠️ 현재 방향({currentDirection})과 필요 도킹 방향({requiredDockingDirection}) 불일치");
}
// 도킹 노드의 경우 항상 필요한 도킹 방향 반환
return requiredDockingDirection;
}
// 일반 노드인 경우 마지막 구간의 이동 방향 분석
var secondLastNodeId = path[path.Count - 2];
var secondLastNode = mapNodes?.FirstOrDefault(n => n.NodeId == secondLastNodeId);
if (secondLastNode == null)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이전 노드 찾을 수 없음: {secondLastNodeId}");
return currentDirection;
}
// 마지막 구간의 이동 벡터 계산
var deltaX = lastNode.Position.X - secondLastNode.Position.X;
var deltaY = lastNode.Position.Y - secondLastNode.Position.Y;
var distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 마지막 구간: {secondLastNodeId} → {lastNodeId}, 벡터: ({deltaX}, {deltaY}), 거리: {distance:F2}");
// 이동 거리가 매우 작으면 현재 방향 유지
if (distance < 1.0)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이동 거리 너무 짧음, 현재 방향 유지: {currentDirection}");
return currentDirection;
}
// 일반 노드의 경우 현재 방향 유지 (방향 전환은 회전 노드에서만 발생)
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 일반 노드, 현재 방향 유지: {currentDirection}");
return currentDirection;
}
/// <summary>
/// 방향을 텍스트로 변환
/// </summary>
private static string GetDirectionText(AgvDirection direction)
{
switch (direction)
{
case AgvDirection.Forward:
return "전진";
case AgvDirection.Backward:
return "후진";
default:
return direction.ToString();
}
}
/// <summary>
/// 도킹 검증 결과를 문자열로 변환 (디버깅용)
/// </summary>
public static string GetValidationSummary(DockingValidationResult validation)
{
if (validation == null)
return "검증 결과 없음";
if (!validation.IsValidationRequired)
return "도킹 검증 불필요";
if (validation.IsValid)
{
return $"도킹 검증 통과: {validation.TargetNodeId}({validation.TargetNodeType}) - {GetDirectionText(validation.RequiredDockingDirection)} 도킹";
}
else
{
return $"도킹 검증 실패: {validation.TargetNodeId}({validation.TargetNodeType}) - {validation.ValidationError}";
}
}
}
}

281
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/LiftCalculator.cs Normal file
View File

@@ -0,0 +1,281 @@
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>
/// 경로 예측 기반 리프트 방향 계산
/// 현재 노드에서 연결된 다음 노드들을 분석하여 리프트 방향 결정
/// </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>
/// 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가 리프트 쪽으로 이동하므로 리프트는 AGV 이동 방향에 위치
// 007→006 후진시: 리프트는 006방향(이동방향)을 향해야 함 (타겟→현재 반대방향)
var dx = currentPos.X - targetPos.X;
var dy = currentPos.Y - targetPos.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="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;
}
}
}

29
Cs_HMI/AGVLogic/AGVNavigationCore/build.bat Normal file
View File

@@ -0,0 +1,29 @@
@echo off
echo Building V2GDecoder VC++ Project...
REM Check if Visual Studio 2022 is installed (Professional or Community)
set MSBUILD_PRO="C:\Program Files\Microsoft Visual Studio\2022\Professional\MSBuild\Current\Bin\MSBuild.exe"
set MSBUILD_COM="C:\Program Files\Microsoft Visual Studio\2022\Community\MSBuild\Current\Bin\MSBuild.exe"
set MSBUILD_BT="F:\(VHD) Program Files\Microsoft Visual Studio\2022\MSBuild\Current\Bin\MSBuild.exe"
if exist %MSBUILD_PRO% (
echo "Found Visual Studio 2022 Professional"
set MSBUILD=%MSBUILD_PRO%
) else if exist %MSBUILD_COM% (
echo "Found Visual Studio 2022 Community"
set MSBUILD=%MSBUILD_COM%
) else if exist %MSBUILD_BT% (
echo "Found Visual Studio 2022 BuildTools"
set MSBUILD=%MSBUILD_BT%
) else (
echo "Visual Studio 2022 (Professional or Community) not found!"
echo "Please install Visual Studio 2022 or update the MSBuild path."
pause
exit /b 1
)
REM Build Debug x64 configuration
echo Building Debug x64 configuration...
%MSBUILD% AGVNavigationCore.csproj
pause

4
Cs_HMI/AGVLogic/AGVNavigationCore/packages.config Normal file
View File

@@ -0,0 +1,4 @@
<?xml version="1.0" encoding="utf-8"?>
<packages>
<package id="Newtonsoft.Json" version="13.0.3" targetFramework="net48" />
</packages>