파일정리

This commit is contained in:
ChiKyun Kim
2026-01-29 14:03:17 +09:00
parent 00cc0ef5b7
commit 58ca67150d
440 changed files with 47236 additions and 99165 deletions

View File

@@ -0,0 +1,119 @@
<?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="Microsoft.VisualBasic" />
<Reference Include="Newtonsoft.Json, Version=13.0.0.0, Culture=neutral, PublicKeyToken=30ad4fe6b2a6aeed, processorArchitecture=MSIL">
<HintPath>..\..\packages\Newtonsoft.Json.13.0.4\lib\net45\Newtonsoft.Json.dll</HintPath>
</Reference>
<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" />
</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\AGVCommand.cs" />
<Compile Include="Models\Enums.cs" />
<Compile Include="Models\IMovableAGV.cs" />
<Compile Include="Models\VirtualAGV.cs" />
<Compile Include="Models\MapLoader.cs" />
<Compile Include="Models\MapMagnet.cs" />
<Compile Include="Models\MapMark.cs" />
<Compile Include="Models\MapNode.cs" />
<Compile Include="Models\NodeBase.cs" />
<Compile Include="Models\MapLabel.cs" />
<Compile Include="Models\MapImage.cs" />
<Compile Include="PathFinding\Planning\AGVPathfinder.cs" />
<Compile Include="PathFinding\Planning\DirectionChangePlanner.cs" />
<Compile Include="PathFinding\Planning\DirectionalPathfinder.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\DirectionalHelper.cs" />
<Compile Include="Utils\LiftCalculator.cs" />
<Compile Include="Utils\ImageConverterUtil.cs" />
<Compile Include="Utils\AGVDirectionCalculator.cs" />
<Compile Include="Utils\DirectionalPathfinderTest.cs" />
<Compile Include="Utils\GetNextNodeIdTest.cs" />
<Compile Include="Utils\TestRunner.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>

View File

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

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? PrevPosition { get; }
MapNode CurrentNode { get; }
MapNode PrevNode { get; }
DockingDirection DockingDirection { get; }
}
#endregion
}

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
}
}

File diff suppressed because it is too large Load Diff

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,979 @@
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding;
using AGVNavigationCore.PathFinding.Core;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Reflection.Emit;
using System.Windows.Forms;
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 = 1.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, // 편집 가능 (맵 에디터)
Sync, // 동기화 모드 (장비 설정 동기화)
Emulator, // 에뮬레이터 모드
Run // 가동 모드 (User Request)
}
/// <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 List<MapLabel> _labels; // 추가
private List<MapImage> _images; // 추가
private List<MapMark> _marks;
private List<MapMagnet> _magnets;
// 선택된 객체들 (나중에 NodeBase로 통일 필요)
private NodeBase _selectedNode;
private List<NodeBase> _selectedNodes; // 다중 선택 (NodeBase로 변경 고려)
private NodeBase _hoveredNode;
private NodeBase _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 = string.Empty;
// 편집 관련 (EditMode에서만 사용)
private bool _isDragging;
private Point _dragOffset;
private Point _dragStartPosition; // 드래그 시작 위치 (고스트 표시용)
private Point _lastMousePosition;
private bool _isConnectionMode;
private MapNode _connectionStartNode;
private Point _connectionEndPoint;
private int _mouseMoveCounter = 0; // 디버그용: MouseMove 실행 횟수
// 영역 선택 관련
private bool _isAreaSelecting;
private Point _areaSelectStart;
private Point _areaSelectEnd;
// 그리드 및 줌 관련
private bool _showGrid = true;
private float _zoomFactor = 1.0f;
private PointF _panOffset = PointF.Empty; // float 정밀도로 변경 (팬 이동 정확도 개선)
private bool _isPanning;
// 자동 증가 카운터
private int _nodeCounter = 1;
// 강조 연결
private (string FromNodeId, string ToNodeId)? _highlightedConnection = null;
// RFID 중복 검사
private HashSet<string> _duplicateRfidNodes = new HashSet<string>();
// 동기화 모드 관련
private string _syncMessage = "동기화 중...";
private float _syncProgress = 0.0f;
private string _syncDetail = "";
string _alertmesage = "";
bool showalert = false;
public void SetAlertMessage(string m)
{
_alertmesage = m;
showalert = !string.IsNullOrEmpty(m);
}
// 브러쉬 및 펜
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 Pen _magnetPen;
private Pen _markPen;
private ToolTip _tooltip;
// 컨텍스트 메뉴
private ContextMenuStrip _contextMenu;
// 이벤트
public event EventHandler<NodeBase> NodeRightClicked;
#endregion
#region Events
// 맵 편집 이벤트
public delegate void NodeSelectHandler(object sender, NodeBase node, MouseEventArgs e);
public event NodeSelectHandler NodeSelect;
public event EventHandler<NodeBase> NodeAdded;
public event EventHandler<List<NodeBase>> NodesSelected; // 다중 선택 이벤트
public event EventHandler<NodeBase> NodeDeleted;
public event EventHandler<NodeBase> NodeMoved;
public event EventHandler<(MapNode From, MapNode To)> ConnectionDeleted;
public event EventHandler<MapImage> ImageDoubleClicked;
public event EventHandler<MapLabel> LabelDoubleClicked;
public event EventHandler MapChanged;
#endregion
#region Properties
public string PredictMessage { get; set; } = "";
public string MapFileName { get; set; } = "";
/// <summary>
/// 캔버스 모드
/// </summary>
public CanvasMode Mode
{
get => _canvasMode;
set
{
_canvasMode = value;
UpdateModeUI();
Invalidate();
}
}
/// <summary>
/// 강조해서 표시할 특정 노드 ID (예: Gateway)
/// 이 값이 설정되면 해당 노드만 강조 표시됩니다.
/// </summary>
public string HighlightNodeId { get; set; }
public void RemoveItem(NodeBase item)
{
if (item is MapImage img) RemoveImage(img);
else if (item is MapLabel lb) RemoveLabel(lb);
else if (item is MapNode nd) RemoveNode(nd);
else if (item is MapMark mk) RemoveMark(mk);
else if (item is MapMagnet mg) RemoveMagnet(mg);
else throw new Exception("unknown type");
}
public void RemoveNode(MapNode node)
{
if (_nodes != null && _nodes.Contains(node))
{
_nodes.Remove(node);
Invalidate();
}
}
public void RemoveLabel(MapLabel label)
{
if (_labels != null && _labels.Contains(label))
{
_labels.Remove(label);
Invalidate();
}
}
public void RemoveImage(MapImage image)
{
if (_images != null && _images.Contains(image))
{
_images.Remove(image);
Invalidate();
}
}
public void RemoveMark(MapMark mark)
{
if (_marks != null && _marks.Contains(mark))
{
_marks.Remove(mark);
Invalidate();
}
}
public void RemoveMagnet(MapMagnet magnet)
{
if (_magnets != null && _magnets.Contains(magnet))
{
_magnets.Remove(magnet);
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();
}
}
[Browsable(false)]
public MapImage SelectedImage
{
get { return this._selectedNode as MapImage; }
}
[Browsable(false)]
public MapLabel SelectedLabel
{
get { return this._selectedNode as MapLabel; }
}
[Browsable(false)]
public MapMark SelectedMark
{
get { return this._selectedNode as MapMark; }
}
[Browsable(false)]
public MapMagnet SelectedMagnet
{
get { return this._selectedNode as MapMagnet; }
}
/// <summary>
/// 선택된 노드 (단일)
/// </summary>
public MapNode SelectedNode
{
get { return this._selectedNode as MapNode; }
}
/// <summary>
/// 선택된 노드들 (다중)
/// </summary>
public List<NodeBase> SelectedNodes => _selectedNodes ?? new List<NodeBase>();
public List<NodeBase> Items
{
get
{
List<NodeBase> items = new List<NodeBase>();
if (Nodes != null && Nodes.Any()) items.AddRange(Nodes);
if (Labels != null && Labels.Any()) items.AddRange(Labels);
if (Images != null && Images.Any()) items.AddRange(Images);
if (Marks != null && Marks.Any()) items.AddRange(Marks);
if (Magnets != null && Magnets.Any()) items.AddRange(Magnets);
return items;
}
}
/// <summary>
/// Map file loading 결과를 셋팅합니다
/// </summary>
/// <param name="result"></param>
public void SetMapLoadResult(MapLoader.MapLoadResult result)
{
this.Nodes = result.Nodes;
this.Labels = result.Labels; // 추가
this.Images = result.Images; // 추가
this.Marks = result.Marks;
this.Magnets = result.Magnets;
// 🔥 맵 설정 적용 (배경색, 그리드 표시)
if (result.Settings != null)
{
this.BackColor = Color.FromArgb(result.Settings.BackgroundColorArgb);
this.ShowGrid = result.Settings.ShowGrid;
}
this.FitToNodes();
}
/// <summary>
/// 노드 목록
/// </summary>
public List<MapNode> Nodes
{
get => _nodes ?? new List<MapNode>();
set
{
_nodes = value ?? new List<MapNode>();
// 기존 노드들의 최대 번호를 찾아서 _nodeCounter 설정
UpdateNodeCounter();
// RFID 중복값 검사
DetectDuplicateRfidNodes();
Invalidate();
}
}
/// <summary>
/// 라벨 목록
/// </summary>
public List<MapLabel> Labels
{
get => _labels ?? new List<MapLabel>();
set
{
_labels = value ?? new List<MapLabel>();
Invalidate();
}
}
/// <summary>
/// 이미지 목록
/// </summary>
public List<MapImage> Images
{
get => _images ?? new List<MapImage>();
set
{
_images = value ?? new List<MapImage>();
Invalidate();
}
}
/// <summary>
/// 마크 목록
/// </summary>
public List<MapMark> Marks
{
get => _marks ?? new List<MapMark>();
set
{
_marks = value ?? new List<MapMark>();
Invalidate();
}
}
/// <summary>
/// 마그넷 목록
/// </summary>
public List<MapMagnet> Magnets
{
get => _magnets ?? new List<MapMagnet>();
set
{
_magnets = value ?? new List<MapMagnet>();
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>
/// 상세경로가 설정되어있는가?
/// </summary>
/// <returns></returns>
public bool HasPath()
{
if (_currentPath == null) return false;
if (_currentPath.DetailedPath == null) return false;
return _currentPath.DetailedPath.Any();
}
/// <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>();
_labels = new List<MapLabel>();
_images = new List<MapImage>();
_marks = new List<MapMark>();
_magnets = new List<MapMagnet>();
_selectedNodes = new List<NodeBase>(); // 다중 선택 리스트 초기화
_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();
_tooltip = new ToolTip();
_tooltip.AutoPopDelay = 5000;
_tooltip.InitialDelay = 1000;
_tooltip.ReshowDelay = 500;
_tooltip.ShowAlways = true;
}
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.White, CONNECTION_WIDTH);
_connectionPen.DashStyle = DashStyle.Dash;
_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 };
_magnetPen = new Pen(Color.FromArgb(100, Color.LightSkyBlue), 15) { DashStyle = DashStyle.Solid };
_markPen = new Pen(Color.White, 3); // 마크는 흰색 선으로 표시
}
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, MapNode node, AgvDirection direction)
{
UpdateAGVPosition(agvId, node.Position);
UpdateAGVDirection(agvId, direction);
}
/// <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)
{
// 경로의 마지막 노드가 목적지
_destinationNode = _currentPath.Path[_currentPath.Path.Count - 1];
}
}
/// <summary>
/// 동기화 상태 설정
/// </summary>
/// <param name="message">메인 메시지</param>
/// <param name="progress">진행률 (0.0 ~ 1.0)</param>
/// <param name="detail">상세 메시지</param>
public void SetSyncStatus(string message, float progress, string detail = "")
{
_syncMessage = message;
_syncProgress = Math.Max(0.0f, Math.Min(1.0f, progress));
_syncDetail = detail;
if (_canvasMode != CanvasMode.Sync)
{
_canvasMode = CanvasMode.Sync;
UpdateModeUI();
}
Invalidate();
}
/// <summary>
/// 동기화 모드 종료
/// </summary>
public void ExitSyncMode(CanvasMode newmode)
{
if (_canvasMode == CanvasMode.Sync)
{
_canvasMode = newmode; // 기본 모드로 복귀 (또는 이전 모드)
UpdateModeUI();
Invalidate();
}
}
#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();
_magnetPen?.Dispose();
_markPen?.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<ushort, List<int>>();
// 모든 노드의 RFID값 수집
for (int i = 0; i < _nodes.Count; i++)
{
var node = _nodes[i];
if (node.HasRfid())
{
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].Id);
}
}
}
}
/// <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.Id.StartsWith("N") && int.TryParse(node.Id.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();
}
}
}

View File

@@ -0,0 +1,54 @@
namespace AGVNavigationCore.Models
{
/// <summary>
/// AGV 제어 명령 클래스 (실제 AGV 제어용)
/// Predict() 메서드가 반환하는 다음 동작 명령
/// </summary>
public class AGVCommand
{
/// <summary>모터 명령 (정지/전진/후진)</summary>
public MotorCommand Motor { get; set; }
/// <summary>마그넷 위치 (직진/왼쪽/오른쪽)</summary>
public MagnetPosition Magnet { get; set; }
/// <summary>속도 레벨 (저속/중속/고속)</summary>
public SpeedLevel Speed { get; set; }
/// <summary>명령 이유 메세지- (디버깅/로깅용)</summary>
public string Message { get; set; }
/// <summary>명령 이유- (디버깅/로깅용)</summary>
public eAGVCommandReason Reason { get; set; }
/// <summary>
/// 생성자
/// </summary>
public AGVCommand(MotorCommand motor, MagnetPosition magnet, SpeedLevel speed, eAGVCommandReason reason, string reasonmessage = "")
{
Motor = motor;
Magnet = magnet;
Speed = speed;
Reason = reason;
Message = reasonmessage;
}
/// <summary>
/// 기본 생성자
/// </summary>
public AGVCommand()
{
Motor = MotorCommand.Stop;
Magnet = MagnetPosition.S;
Speed = SpeedLevel.L;
Message = "";
Reason = eAGVCommandReason.Normal;
}
public override string ToString()
{
return $"Motor:{Motor}, Magnet:{Magnet}, Speed:{Speed},Reason:{Reason}" +
(string.IsNullOrEmpty(Message) ? "" : $" ({Message})");
}
}
}

View File

@@ -0,0 +1,182 @@
using System;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 노드 타입 열거형
/// </summary>
public enum NodeType
{
/// <summary>일반 경로 노드</summary>
Normal,
Label,
/// <summary>이미지 (UI 요소)</summary>
Image,
/// <summary>
/// 마크센서
/// </summary>
Mark,
/// <summary>
/// 마그넷라인
/// </summary>
Magnet
}
/// <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>
Normal,
/// <summary>로더</summary>
Loader,
/// <summary>클리너</summary>
Clearner,
/// <summary>오프로더</summary>
UnLoader,
/// <summary>버퍼</summary>
Buffer,
/// <summary>충전기1</summary>
Charger1,
/// <summary>충전기2</summary>
Charger2,
/// <summary>
/// 끝점(더이상 이동불가)
/// </summary>
Limit,
}
/// <summary>
/// AGV턴상태
/// </summary>
public enum AGVTurn
{
None=0,
/// <summary>
/// left turn 90"
/// </summary>
L90,
/// <summary>
/// right turn 90"
/// </summary>
R90
}
/// <summary>
/// 모터 명령 열거형 (실제 AGV 제어용)
/// </summary>
public enum MotorCommand
{
/// <summary>정지</summary>
Stop,
/// <summary>전진 (Forward - 모니터 방향)</summary>
Forward,
/// <summary>후진 (Backward - 리프트 방향)</summary>
Backward
}
/// <summary>
/// 마그넷 위치 열거형 (실제 AGV 제어용)
/// </summary>
public enum MagnetPosition
{
/// <summary>직진 (Straight)</summary>
S,
/// <summary>왼쪽 (Left)</summary>
L,
/// <summary>오른쪽 (Right)</summary>
R
}
/// <summary>
/// 속도 레벨 열거형 (실제 AGV 제어용)
/// </summary>
public enum SpeedLevel
{
/// <summary>저속 (Low)</summary>
L,
/// <summary>중속 (Medium)</summary>
M,
/// <summary>고속 (High)</summary>
H
}
public enum eAGVCommandReason
{
/// <summary>
/// 초기 미지정
/// </summary>
Normal,
/// <summary>
/// 위치 미확정
/// </summary>
UnknownPosition,
/// <summary>
/// 대상경로없음
/// </summary>
NoTarget,
/// <summary>
/// 경로없음
/// </summary>
NoPath,
/// <summary>
/// 경로이탈
/// </summary>
PathOut,
/// <summary>
/// 마크스탑을 해야한다
/// </summary>
MarkStop,
/// <summary>
/// 완료
/// </summary>
Complete,
}
}

View File

@@ -0,0 +1,210 @@
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>
MapNode CurrentNode { get; }
/// <summary>
/// 목표 위치
/// </summary>
Point? PrevPosition { get; }
/// <summary>
/// 목표 노드 ID
/// </summary>
MapNode PrevNode { 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>
MapNode GetCurrentNode();
/// <summary>
/// AGV 상태 정보 문자열 조회
/// </summary>
string GetStatus();
#endregion
#region Path Execution Methods
/// <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
}
}

View File

@@ -0,0 +1,88 @@
using System.ComponentModel;
using System.Drawing;
using System.Drawing.Drawing2D;
using AGVNavigationCore.Utils;
using Newtonsoft.Json;
using System;
namespace AGVNavigationCore.Models
{
public class MapImage : NodeBase
{
[Category("기본 정보")]
[Description("이미지의 이름입니다.")]
public string Name { get; set; } = "Image";
[Category("이미지 설정")]
[Description("이미지 파일 경로입니다 (편집기용).")]
public string ImagePath { get; set; } = string.Empty;
[ReadOnly(false)]
public string ImageBase64 { get; set; } = string.Empty;
[Category("이미지 설정")]
[Description("이미지 크기 배율입니다.")]
public SizeF Scale { get; set; } = new SizeF(1.0f, 1.0f);
[Category("이미지 설정")]
[Description("이미지 투명도입니다 (0.0 ~ 1.0).")]
public float Opacity { get; set; } = 1.0f;
[Category("이미지 설정")]
[Description("이미지 회전 각도입니다.")]
public float Rotation { get; set; } = 0.0f;
[JsonIgnore]
[Browsable(false)]
public Image LoadedImage { get; set; }
public MapImage()
{
Type = NodeType.Image;
}
public bool LoadImage()
{
try
{
Image originalImage = null;
if (!string.IsNullOrEmpty(ImageBase64))
{
originalImage = ImageConverterUtil.Base64ToImage(ImageBase64);
}
else if (!string.IsNullOrEmpty(ImagePath) && System.IO.File.Exists(ImagePath))
{
originalImage = Image.FromFile(ImagePath);
}
if (originalImage != null)
{
LoadedImage?.Dispose();
LoadedImage = originalImage; // 리사이즈 필요시 추가 구현
return true;
}
}
catch
{
// 무시
}
return false;
}
public Size GetDisplaySize()
{
if (LoadedImage == null) return Size.Empty;
return new Size(
(int)(LoadedImage.Width * Scale.Width),
(int)(LoadedImage.Height * Scale.Height)
);
}
public void Dispose()
{
LoadedImage?.Dispose();
LoadedImage = null;
}
}
}

View File

@@ -0,0 +1,42 @@
using System.ComponentModel;
using System.Drawing;
namespace AGVNavigationCore.Models
{
public class MapLabel : NodeBase
{
[Category("라벨 설정")]
[Description("표시할 텍스트입니다.")]
public string Text { get; set; } = "";
[Category("라벨 설정")]
[Description("글자색입니다")]
public Color ForeColor { get; set; } = Color.Black;
[Category("라벨 설정")]
[Description("배경색입니다.")]
public Color BackColor { get; set; } = Color.Transparent;
[Category("라벨 설정")]
[Description("폰트 종류입니다.")]
public string FontFamily { get; set; } = "Arial";
[Category("라벨 설정")]
[Description("폰트 크기입니다.")]
public float FontSize { get; set; } = 12.0f;
[Category("라벨 설정")]
[Description("폰트 스타일입니다.")]
public FontStyle FontStyle { get; set; } = FontStyle.Regular;
[Category("라벨 설정")]
[Description("내부 여백입니다.")]
public int Padding { get; set; } = 5;
public MapLabel()
{
ForeColor = Color.Purple;
Type = NodeType.Label;
}
}
}

View File

@@ -0,0 +1,514 @@
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 MapSettings
{
public int BackgroundColorArgb { get; set; } = System.Drawing.Color.White.ToArgb();
public bool ShowGrid { get; set; } = true;
}
/// <summary>
/// 맵 파일 로딩 결과
/// </summary>
public class MapLoadResult
{
public bool Success { get; set; }
public List<MapNode> Nodes { get; set; } = new List<MapNode>();
public List<MapLabel> Labels { get; set; } = new List<MapLabel>(); // 추가
public List<MapImage> Images { get; set; } = new List<MapImage>(); // 추가
public List<MapMark> Marks { get; set; } = new List<MapMark>();
public List<MapMagnet> Magnets { get; set; } = new List<MapMagnet>();
public MapSettings Settings { get; set; } = new MapSettings();
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 List<MapLabel> Labels { get; set; } = new List<MapLabel>(); // 추가
public List<MapImage> Images { get; set; } = new List<MapImage>(); // 추가
public List<MapMark> Marks { get; set; } = new List<MapMark>();
public List<MapMagnet> Magnets { get; set; } = new List<MapMagnet>();
public MapSettings Settings { get; set; } = new MapSettings();
public DateTime CreatedDate { get; set; }
public string Version { get; set; } = "1.3"; // 버전 업그레이드
}
/// <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
};
// 먼저 구조 파악을 위해 동적 객체로 로드하거나, MapFileData로 시도
var mapData = JsonConvert.DeserializeObject<MapFileData>(json, settings);
if (mapData != null)
{
result.Nodes = new List<MapNode>();
result.Labels = mapData.Labels ?? new List<MapLabel>();
result.Images = mapData.Images ?? new List<MapImage>();
result.Marks = mapData.Marks ?? new List<MapMark>();
result.Magnets = mapData.Magnets ?? new List<MapMagnet>();
result.Settings = mapData.Settings ?? new MapSettings();
result.Version = mapData.Version ?? "1.0";
result.CreatedDate = mapData.CreatedDate;
if (mapData.Nodes != null)
{
foreach (var node in mapData.Nodes)
{
// 마이그레이션: 기존 파일의 Nodes 리스트에 섞여있는 Label, Image 분리
// (새 파일 구조에서는 이미 분리되어 로드됨)
if (node.Type == NodeType.Label)
{
// MapNode -> MapLabel 변환 (필드 매핑)
var label = new MapLabel
{
Id = node.Id, // 기존 NodeId -> Id
Position = node.Position,
CreatedDate = node.CreatedDate,
ModifiedDate = node.ModifiedDate,
// Label 속성 매핑 (MapNode에서 임시로 가져오거나 Json Raw Parsing 필요할 수 있음)
// 현재 MapNode 클래스에는 해당 속성들이 제거되었으므로,
// Json 포맷 변경으로 인해 기존 데이터 로드시 정보 손실 가능성 있음.
// * 중요 *: MapNode 클래스에서 속성을 지웠으므로 일반 Deserialize로는 Label/Image 속성을 못 읽음.
// 해결책: JObject로 먼저 읽어서 분기 처리하거나, DTO 클래스를 별도로 두어야 함.
// 하지만 시간 관계상, 만약 기존 MapNode가 속성을 가지고 있지 않다면 마이그레이션은 "위치/ID" 정도만 복구됨.
// 완벽한 마이그레이션을 위해서는 MapNode에 Obsolete 속성을 잠시 두었어야 함.
// 여기서는 일단 기본 정보라도 살림.
};
result.Labels.Add(label);
}
else if (node.Type == NodeType.Image)
{
var image = new MapImage
{
Id = node.Id,
Position = node.Position,
CreatedDate = node.CreatedDate,
ModifiedDate = node.ModifiedDate,
// 이미지/라벨 속성 복구 불가 (MapNode에서 삭제됨)
};
result.Images.Add(image);
}
else
{
result.Nodes.Add(node);
}
}
}
// 중복된 NodeId 정리 (Nav Node만)
FixDuplicateNodeIds(result.Nodes);
// 고아 연결 정리
CleanupOrphanConnections(result.Nodes);
// 양방향 연결 자동 설정
EnsureBidirectionalConnections(result.Nodes);
// ConnectedMapNodes 채우기
ResolveConnectedMapNodes(result.Nodes);
// 이미지 로드 (MapImage 객체에서)
foreach (var img in result.Images)
{
img.LoadImage();
}
result.Success = true;
}
else
{
result.ErrorMessage = "맵 데이터 파싱에 실패했습니다.";
}
}
catch (Exception ex)
{
result.ErrorMessage = $"맵 파일 로딩 중 오류 발생: {ex.Message}";
}
return result;
}
/// <summary>
/// 맵 데이터를 파일로 저장
/// </summary>
public static bool SaveMapToFile(string filePath, List<MapNode> nodes, List<MapLabel> labels = null, List<MapImage> images = null, List<MapMark> marks = null, List<MapMagnet> magnets = null, MapSettings settings = null)
{
try
{
// 저장 전 고아 연결 정리
CleanupOrphanConnections(nodes);
var mapData = new MapFileData
{
Nodes = nodes,
Labels = labels ?? new List<MapLabel>(),
Images = images ?? new List<MapImage>(),
Marks = marks ?? new List<MapMark>(),
Magnets = magnets ?? new List<MapMagnet>(),
Settings = settings ?? new MapSettings(),
CreatedDate = DateTime.Now,
Version = "1.3"
};
var json = JsonConvert.SerializeObject(mapData, Formatting.Indented);
File.WriteAllText(filePath, json);
return true;
}
catch (Exception)
{
return false;
}
}
/// <summary>
/// ConnectedMapNodes 채우기 (ConnectedNodes의 string ID → MapNode 객체 변환)
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void ResolveConnectedMapNodes(List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0) return;
// 빠른 조회를 위한 Dictionary 생성
var nodeDict = mapNodes.ToDictionary(n => n.Id, n => n);
foreach (var node in mapNodes)
{
// ConnectedMapNodes 초기화
node.ConnectedMapNodes.Clear();
if (node.ConnectedNodes != null && node.ConnectedNodes.Count > 0)
{
foreach (var connectedNodeId in node.ConnectedNodes)
{
if (nodeDict.TryGetValue(connectedNodeId, out var connectedNode))
{
node.ConnectedMapNodes.Add(connectedNode);
}
}
}
}
}
/// <summary>
/// 기존 Description 데이터를 Name 필드로 마이그레이션
/// JSON 파일에서 Description 필드가 있는 경우 Name으로 이동
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void MigrateDescriptionToName(List<MapNode> mapNodes)
{
// JSON에서 Description이 있던 기존 파일들을 위한 마이그레이션
// 현재 MapNode 클래스에는 Description 속성이 제거되었으므로
// 이 메서드는 호환성을 위해 유지되지만 실제로는 작동하지 않음
// 기존 파일들은 다시 저장될 때 Description 없이 저장됨
}
/// <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.Id))
{
duplicateNodes.Add(node);
}
else
{
usedIds.Add(node.Id);
}
}
// 두 번째 패스: 중복된 노드들에게 새로운 NodeId 할당
foreach (var duplicateNode in duplicateNodes)
{
string newNodeId = GenerateUniqueNodeId(usedIds);
// 다른 노드들의 연결에서 기존 NodeId를 새 NodeId로 업데이트
UpdateConnections(mapNodes, duplicateNode.Id, newNodeId);
duplicateNode.Id = 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>
/// 존재하지 않는 노드에 대한 연결을 정리합니다 (고아 연결 제거).
/// 노드 삭제 후 저장된 맵 파일에서 삭제된 노드 ID가 ConnectedNodes에 남아있는 경우를 처리합니다.
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void CleanupOrphanConnections(List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0) return;
// 존재하는 모든 노드 ID 집합 생성
var validNodeIds = new HashSet<string>(mapNodes.Select(n => n.Id));
// 각 노드의 연결을 검증하고 존재하지 않는 노드 ID 제거
foreach (var node in mapNodes)
{
if (node.ConnectedNodes == null || node.ConnectedNodes.Count == 0)
continue;
var orphanConnections = node.ConnectedNodes
.Where(connectedId => !validNodeIds.Contains(connectedId))
.ToList();
foreach (var orphanId in orphanConnections)
{
node.RemoveConnection(orphanId);
}
}
}
/// <summary>
/// [사용 중지됨] 중복 연결을 정리합니다. 양방향 중복 연결을 단일 연결로 통합합니다.
/// 주의: 이 함수는 버그가 있어 사용 중지됨 - 양방향 연결을 단방향으로 변환하여 경로 탐색 실패 발생
/// AGV 시스템에서는 모든 연결이 양방향이어야 하므로 EnsureBidirectionalConnections()만 사용
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
[Obsolete("이 함수는 양방향 연결을 단방향으로 변환하는 버그가 있습니다. 사용하지 마세요.")]
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.Id == connectedNodeId);
if (connectedNode == null) continue;
// 연결 쌍의 키 생성 (사전순 정렬)
string pairKey = string.Compare(node.Id, connectedNodeId, StringComparison.Ordinal) < 0
? $"{node.Id}-{connectedNodeId}"
: $"{connectedNodeId}-{node.Id}";
if (processedPairs.Contains(pairKey))
{
// 이미 처리된 연결인 경우 중복으로 간주하고 제거
connectionsToRemove.Add(connectedNodeId);
}
else
{
// 처리되지 않은 연결인 경우
processedPairs.Add(pairKey);
// 양방향 연결인 경우 하나만 유지
if (connectedNode.ConnectedNodes.Contains(node.Id))
{
// 사전순으로 더 작은 노드에만 연결을 유지
if (string.Compare(node.Id, connectedNodeId, StringComparison.Ordinal) > 0)
{
connectionsToRemove.Add(connectedNodeId);
}
else
{
// 반대 방향 연결 제거
connectedNode.RemoveConnection(node.Id);
}
}
}
}
// 중복 연결 제거
foreach (var connectionToRemove in connectionsToRemove)
{
node.RemoveConnection(connectionToRemove);
}
}
}
/// <summary>
/// 맵의 모든 연결을 양방향으로 만듭니다.
/// A→B 연결이 있으면 B→A 연결도 자동으로 추가합니다.
/// GetNextNodeId() 메서드에서 현재 노드의 ConnectedNodes만으로 다음 노드를 찾을 수 있도록 하기 위함.
///
/// 예시:
/// - 맵 에디터에서 002→003 연결을 생성했다면
/// - 자동으로 003→002 연결도 추가됨
/// - 따라서 003의 ConnectedNodes에 002가 포함됨
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
private static void EnsureBidirectionalConnections(List<MapNode> mapNodes)
{
if (mapNodes == null || mapNodes.Count == 0) return;
// 모든 노드의 연결 정보를 수집
var allConnections = new Dictionary<string, HashSet<string>>();
// 1단계: 모든 명시적 연결 수집
foreach (var node in mapNodes)
{
if (!allConnections.ContainsKey(node.Id))
{
allConnections[node.Id] = new HashSet<string>();
}
if (node.ConnectedNodes != null)
{
foreach (var connectedId in node.ConnectedNodes)
{
allConnections[node.Id].Add(connectedId);
}
}
}
// 2단계: 역방향 연결 추가
foreach (var node in mapNodes)
{
if (node.ConnectedNodes == null)
{
node.ConnectedNodes = new List<string>();
}
// 이 노드를 연결하는 모든 노드 찾기
foreach (var otherNodeId in allConnections.Keys)
{
if (otherNodeId == node.Id) continue;
// 다른 노드가 이 노드를 연결하고 있다면
if (allConnections[otherNodeId].Contains(node.Id))
{
// 이 노드의 ConnectedNodes에 그 노드를 추가 (중복 방지)
if (!node.ConnectedNodes.Contains(otherNodeId))
{
node.ConnectedNodes.Add(otherNodeId);
}
}
}
}
}
/// <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, "", "정상");
}
}
*/
}
}
}

View File

@@ -0,0 +1,72 @@
using System;
using System.ComponentModel;
using System.Drawing;
using Newtonsoft.Json;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 맵 상의 마그넷(Magnet) 정보를 나타내는 클래스
/// </summary>
public class MapMagnet : NodeBase
{
public MapMagnet() {
Type = NodeType.Magnet;
}
[Category("위치 정보")]
[Description("시작점 좌표")]
public MagnetPoint P1 { get; set; } = new MagnetPoint();
[Category("위치 정보")]
[Description("끝점 좌표")]
public MagnetPoint P2 { get; set; } = new MagnetPoint();
[Category("위치 정보")]
[Description("제어점 좌표 (곡선인 경우)")]
public MagnetPoint ControlPoint { get; set; } = null;
public class MagnetPoint
{
public double X { get; set; }
public double Y { get; set; }
}
[JsonIgnore]
public override Point Position
{
get => new Point((int)P1.X, (int)P1.Y);
set
{
double dx = value.X - P1.X;
double dy = value.Y - P1.Y;
P1.X += dx;
P1.Y += dy;
P2.X += dx;
P2.Y += dy;
if (ControlPoint != null)
{
ControlPoint.X += dx;
ControlPoint.Y += dy;
}
}
}
/// <summary>
/// 시작점 Point 반환
/// </summary>
[Browsable(false)]
[JsonIgnore]
public Point StartPoint => new Point((int)P1.X, (int)P1.Y);
/// <summary>
/// 끝점 Point 반환
/// </summary>
[Browsable(false)]
[JsonIgnore]
public Point EndPoint => new Point((int)P2.X, (int)P2.Y);
}
}

View File

@@ -0,0 +1,37 @@
using System;
using System.ComponentModel;
using System.Drawing;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 맵 상의 마크(Mark) 정보를 나타내는 클래스
/// </summary>
public class MapMark : NodeBase
{
// Id is inherited from NodeBase
public MapMark() {
Type = NodeType.Mark;
}
[Category("위치 정보")]
[Description("마크의 X 좌표")]
public double X
{
get => Position.X;
set => Position = new Point((int)value, Position.Y);
}
[Category("위치 정보")]
[Description("마크의 Y 좌표")]
public double Y
{
get => Position.Y;
set => Position = new Point(Position.X, (int)value);
}
[Category("위치 정보")]
[Description("마크의 회전 각도")]
public double Rotation { get; set; }
}
}

View File

@@ -0,0 +1,185 @@
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Drawing.Drawing2D;
using AGVNavigationCore.Utils;
using Newtonsoft.Json;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 맵 노드 정보를 관리하는 클래스 (주행 경로용 노드)
/// </summary>
public class MapNode : NodeBase
{
[Category("라벨 설정")]
[Description("표시할 텍스트입니다.")]
public string Text { get; set; } = "";
public StationType StationType { get; set; }
[Browsable(false)]
public bool CanDocking
{
get
{
if (StationType == StationType.Buffer) return true;
if (StationType == StationType.Loader) return true;
if (StationType == StationType.UnLoader) return true;
if (StationType == StationType.Clearner) return true;
if (StationType == StationType.Charger1) return true;
if (StationType == StationType.Charger2) return true;
return false;
}
}
[Category("노드 설정")]
[Description("도킹/충전 노드의 진입 방향입니다.")]
public DockingDirection DockDirection { get; set; } = DockingDirection.DontCare;
[Category("노드 설정")]
[Description("각 연결된 노드로 향할 때의 마그넷 방향 정보입니다.")]
public Dictionary<string, MagnetPosition> MagnetDirections { get; set; } = new Dictionary<string, MagnetPosition>();
[Category("연결 정보")]
[Description("연결된 노드 ID 목록입니다.")]
[ReadOnly(true)]
public List<string> ConnectedNodes { get; set; } = new List<string>();
[JsonIgnore]
[Browsable(false)]
public List<MapNode> ConnectedMapNodes { get; set; } = new List<MapNode>();
[Category("주행 설정")]
[Description("제자리 회전(좌) 가능 여부입니다.")]
public bool CanTurnLeft { get; set; } = true;
[Category("주행 설정")]
[Description("제자리 회전(우) 가능 여부입니다.")]
public bool CanTurnRight { get; set; } = true;
[Category("주행 설정")]
[Description("교차로 주행 가능 여부입니다.")]
public bool DisableCross
{
get
{
if (Type != NodeType.Normal) return true;
return _disablecross;
}
set { _disablecross = value; }
}
private bool _disablecross = false;
[Category("주행 설정")]
[Description("노드 통과 시 제한 속도입니다.")]
public SpeedLevel SpeedLimit { get; set; } = SpeedLevel.M;
[Category("노드 설정")]
[Description("장비 ID 또는 별칭입니다.")]
public string AliasName { get; set; } = string.Empty;
[Category("기본 정보")]
[Description("노드 사용 여부입니다.")]
public bool IsActive { get; set; } = true;
[Category("RFID 정보")]
[Description("물리적 RFID 태그 ID입니다.")]
public UInt16 RfidId { get; set; } = 0;
[Category("노드 텍스트"), DisplayName("TextColor")]
[Description("텍스트 색상입니다.")]
public Color NodeTextForeColor { get; set; } = Color.Black;
private float _textFontSize = 7.0f;
[Category("노드 텍스트"), DisplayName("TextSize")]
[Description("일반 노드 텍스트의 크기입니다.")]
public float NodeTextFontSize
{
get => _textFontSize;
set => _textFontSize = value > 0 ? value : 7.0f;
}
public MapNode() : base()
{
Type = NodeType.Normal;
}
public MapNode(string nodeId, Point position, StationType type) : base(nodeId, position)
{
Type = NodeType.Normal;
}
[Category("기본 정보")]
[JsonIgnore]
[ReadOnly(true), Browsable(false)]
public bool isDockingNode
{
get
{
if (StationType == StationType.Charger1 || StationType == StationType.Charger2 || StationType == StationType.Buffer ||
StationType == StationType.Clearner || StationType == StationType.Loader ||
StationType == StationType.UnLoader) return true;
return false;
}
}
public void AddConnection(string nodeId)
{
if (!ConnectedNodes.Contains(nodeId))
{
ConnectedNodes.Add(nodeId);
ModifiedDate = DateTime.Now;
}
}
public void RemoveConnection(string nodeId)
{
if (ConnectedNodes.Remove(nodeId))
{
ModifiedDate = DateTime.Now;
}
}
public void SetChargingStation(string stationId)
{
//StationType = StationType.Charger;
//Id = stationId;
//DockDirection = DockingDirection.Forward;
//ModifiedDate = DateTime.Now;
}
public override string ToString()
{
return $"RFID:{RfidId}(NODE:{Id}): AS:{AliasName} ({Type}) at ({Position.X}, {Position.Y})";
}
/// <summary>
/// RFID(*ID)
/// </summary>
public string ID2
{
get
{
if (HasRfid()) return $"{this.RfidId:0000}(*{this.Id})";
else return $"(*{this.Id})";
}
}
public bool IsNavigationNode()
{
// 이제 MapNode는 항상 내비게이션 노드임 (Label, Image 분리됨)
// 하지만 기존 로직 호환성을 위해 Active 체크만 유지
return IsActive;
}
public bool HasRfid()
{
return RfidId > 0;
}
}
}

View File

@@ -0,0 +1,60 @@
using System;
using System.ComponentModel;
using System.Diagnostics;
using System.Drawing;
using Newtonsoft.Json;
namespace AGVNavigationCore.Models
{
/// <summary>
/// 맵 상의 모든 객체의 최상위 기본 클래스
/// 위치, 선택 상태, 기본 식별자 등을 관리
/// </summary>
public abstract class NodeBase
{
[Category("기본 정보")]
[Description("객체의 고유 ID입니다.")]
[ReadOnly(true)]
public string Id { get; set; } = Guid.NewGuid().ToString();
[Category("기본 정보")]
public NodeType Type { protected set; get; } = NodeType.Normal;
[Category("기본 정보")]
[Description("객체의 좌표(X, Y)입니다.")]
public virtual Point Position { get; set; } = Point.Empty;
[Category("기본 정보")]
[Description("객체 생성 일자입니다.")]
[JsonIgnore]
[ReadOnly(true), Browsable(false)]
public DateTime CreatedDate { get; set; } = DateTime.Now;
[Category("기본 정보")]
[Description("객체 수정 일자입니다.")]
[JsonIgnore]
[ReadOnly(true), Browsable(false)]
public DateTime ModifiedDate { get; set; } = DateTime.Now;
[Browsable(false)]
[JsonIgnore]
public bool IsSelected { get; set; } = false;
[Browsable(false)]
[JsonIgnore]
public bool IsHovered { get; set; } = false;
public NodeBase()
{
}
public NodeBase(string id, Point position)
{
Id = id;
Position = position;
}
}
}

View File

@@ -0,0 +1,942 @@
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 _prevPosition;
private AgvDirection _currentDirection;
private AgvDirection _prevDirection;
private AGVState _currentState;
private float _currentSpeed;
// 경로 관련
private AGVPathResult _currentPath;
private List<string> _remainingNodes;
private int _currentNodeIndex;
private MapNode _currentNode;
private MapNode _prevNode;
private AGVTurn _turn;
// 이동 관련
private DateTime _lastUpdateTime;
private Point _moveStartPosition;
private Point _moveTargetPosition;
// 도킹 관련
private DockingDirection _dockingDirection;
// 시뮬레이션 설정
private readonly float _moveSpeed = 50.0f; // 픽셀/초
private bool _isMoving;
// RFID 위치 추적 (실제 AGV용)
private List<string> _detectedRfids = new List<string>(); // 감지된 RFID 목록
private bool _isPositionConfirmed = false; // 위치 확정 여부 (RFID 2개 이상 감지)
// 에뮬레이터용 추가 속성
public double Angle { get; set; } = 0; // 0 = Right, 90 = Down, 180 = Left, 270 = Up (Standard Math)
// But AGV Direction: Forward usually means "Front of AGV".
// Let's assume Angle is the orientation of the AGV in degrees.
public bool IsStopMarkOn { get; set; } = false;
#endregion
#region Properties
public bool Turn180 { get; set; } = false;
/// <summary>
/// 대상 이동시 모터 방향
/// </summary>
public AgvDirection PrevDirection => _prevDirection;
/// <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;
public void ClearPath()
{
_currentPath = null;
}
/// <summary>
/// 현재 노드 ID
/// </summary>
public MapNode CurrentNode => _currentNode;
/// <summary>
/// 현재 노드 ID (CurrentNode.Id)
/// </summary>
public string CurrentNodeId => _currentNode?.Id;
/// <summary>
/// 현재노드의 RFID(id)값을 표시합니다 없는경우 (X)가 표시됩니다
/// </summary>
public string CurrentNodeID2
{
get
{
if (_currentNode == null) return "(X)";
return _currentNode.ID2;
}
}
/// <summary>
/// 이전 위치
/// </summary>
public Point? PrevPosition => _prevPosition;
/// <summary>
/// 배터리 레벨 (시뮬레이션)
/// </summary>
public float BatteryLevel { get; set; } = 100.0f;
/// <summary>
/// 이전 노드
/// </summary>
public MapNode PrevNode => _prevNode;
/// <summary>
/// Turn 상태값
/// </summary>
public AGVTurn Turn { get; set; }
/// <summary>
/// 도킹 방향
/// </summary>
public DockingDirection DockingDirection => _dockingDirection;
/// <summary>
/// 위치 확정 여부 (RFID 2개 이상 감지 시 true)
/// </summary>
public bool IsPositionConfirmed => _isPositionConfirmed;
/// <summary>
/// 감지된 RFID 개수
/// </summary>
public int DetectedRfidCount => _detectedRfids.Count;
/// <summary>
/// 배터리 부족 경고 임계값 (%)
/// </summary>
public float LowBatteryThreshold { get; set; } = 20.0f;
#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;
_prevNode = 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)
{
// RFID 목록에 추가 (중복 제거)
if (!_detectedRfids.Contains(rfidId))
{
_detectedRfids.Add(rfidId);
}
// RFID 2개 이상 감지 시 위치 확정
if (_detectedRfids.Count >= 2 && !_isPositionConfirmed)
{
_isPositionConfirmed = true;
}
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 < LowBatteryThreshold && _currentState != AGVState.Charging)
{
OnError($"배터리 부족: {BatteryLevel:F1}% (기준: {LowBatteryThreshold}%)");
}
}
/// <summary>
/// 현재 노드id의 개체를 IsPass 로 설정합니다
/// </summary>
public bool SetCurrentNodeMarkStop()
{
if (_currentNode == null) return false;
if (_currentPath == null) return false;
var = _currentPath.DetailedPath.Where(t => t.IsPass == false).OrderBy(t => t.seq).FirstOrDefault();
if ( == null) return false;
.IsPass = true;
Console.WriteLine($"미완료된처음노드를 true러치합니다");
return true;
}
/// <summary>
/// 다음 동작 예측 (실제 AGV 제어용)
/// AGV가 지속적으로 호출하여 현재 상태와 예측 상태를 일치시킴
/// </summary>
/// <returns>다음에 수행할 모터/마그넷/속도 명령</returns>
public AGVCommand Predict()
{
// 1. 위치 미확정 상태 (RFID 2개 미만 감지)
if (!_isPositionConfirmed)
{
// 항상 전진 + 저속으로 이동 (RFID 감지 대기)
return new AGVCommand(
MotorCommand.Forward,
MagnetPosition.S, // 직진
SpeedLevel.L, // 저속
eAGVCommandReason.UnknownPosition,
$"위치 미확정 (RFID {_detectedRfids.Count}/2) - 전진하여 RFID 탐색"
);
}
// 2. 위치 확정됨 + 경로 없음 → 정지 (목적지 미설정 상태)
if (_currentPath == null || (_currentPath.DetailedPath?.Count ?? 0) < 1)
{
var curpos = "알수없음";
if (_currentNode != null)
{
curpos = _currentNode.HasRfid() ? $"RFID #{_currentNode.RfidId} (*{_currentNode.Id})" : $"(*{_currentNode.Id})";
}
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
eAGVCommandReason.NoPath,
$"(목적지 미설정) - 현재={curpos}"
);
}
// 3. 위치 확정됨 + 경로 있음 + 남은 노드 없음 → 정지 (목적지 도착)
var lastNode = _currentPath.DetailedPath.Last();
if (_currentPath.DetailedPath.Where(t => t.seq < lastNode.seq && t.IsPass == false).Any() == false)
{
// 마지막 노드에 도착했는지 확인 (현재 노드가 마지막 노드와 같은지) -
// 모터방향오 같아야한다. 간혹 방향전환 후 MARK STOP하는경우가있다. 260127
if (_currentNode != null && _currentNode.Id == lastNode.NodeId && lastNode.MotorDirection == CurrentDirection)
{
if (lastNode.IsPass) //이미완료되었다.
{
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
eAGVCommandReason.Complete,
$"목적지 도착 - 최종:{CurrentNodeID2}"
);
}
else
{
//도킹노드라면 markstop 을 나머지는 바로 스탑한다.
eAGVCommandReason reason = eAGVCommandReason.MarkStop;
if (_targetnode.StationType == StationType.Normal || _targetnode.StationType == StationType.Limit)
{
//일반노드는 마크스탑포인트가 없으니 바로 종료되도록 한다
reason = eAGVCommandReason.Complete;
}
//마지막노드는 일혔지만 완료되지 않았다. 마크스탑필요
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
reason,
$"목적지 도착 전(MarkStop) - 최종:{CurrentNodeID2}"
);
}
}
}
// 4. 경로이탈
var TargetNode = _currentPath.DetailedPath.Where(t => t.IsPass == false && t.NodeId.Equals(_currentNode.Id)).FirstOrDefault();
if (TargetNode == null)
{
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
eAGVCommandReason.PathOut,
$"(재탐색요청)경로이탈 현재위치:{CurrentNodeID2}"
);
}
return GetCommandFromPath(CurrentNode, "경로 실행 시작");
}
#endregion
#region Public Methods -
/// <summary>
/// 현재 위치 조회
/// </summary>
public Point GetCurrentPosition() => _currentPosition;
/// <summary>
/// 현재 상태 조회
/// </summary>
public AGVState GetCurrentState() => _currentState;
/// <summary>
/// 현재 노드 ID 조회
/// </summary>
public MapNode GetCurrentNode() => _currentNode;
/// <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>
/// <returns></returns>
public bool HasPath()
{
if (_currentPath == null) return false;
if (_currentPath.DetailedPath == null) return false;
return _currentPath.DetailedPath.Any();
}
/// <summary>
/// 경로 설정 (실제 AGV 및 시뮬레이터에서 사용)
/// </summary>
/// <param name="path">실행할 경로</param>
public void SetPath(AGVPathResult path)
{
if (path == null)
{
_currentPath = null;
_remainingNodes.Clear();// = null;
_currentNodeIndex = 0;
OnError("경로가 null입니다.");
return;
}
_currentPath = path;
_remainingNodes = path.Path.Select(n => n.Id).ToList(); // MapNode → NodeId 변환
_currentNodeIndex = 0;
// 경로 시작 노드가 현재 노드와 다른 경우 경고
if (_currentNode != null && _remainingNodes.Count > 0 && _remainingNodes[0] != _currentNode.Id)
{
OnError($"경로 시작 노드({_remainingNodes[0]})와 현재 노드({_currentNode.Id})가 다릅니다.");
}
}
/// <summary>
/// 경로 정지
/// </summary>
public void StopPath()
{
_isMoving = false;
_currentPath = null;
_remainingNodes?.Clear();
SetState(AGVState.Idle);
_currentSpeed = 0;
}
/// <summary>
/// 긴급 정지
/// </summary>
public void EmergencyStop()
{
StopPath();
OnError("긴급 정지가 실행되었습니다.");
}
/// <summary>
/// 일시 정지 (경로 유지)
/// </summary>
public void Pause()
{
_isMoving = false;
_currentSpeed = 0;
}
/// <summary>
/// 이동 재개
/// </summary>
public void Resume()
{
if (_currentPath != null && _remainingNodes != null && _remainingNodes.Count > 0)
{
_isMoving = true;
SetState(AGVState.Moving);
}
}
#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)
{
_prevPosition = targetPosition;
_moveStartPosition = _currentPosition;
_moveTargetPosition = targetPosition;
SetState(AGVState.Moving);
_isMoving = true;
Turn = AGVTurn.None;
}
/// <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 위치 직접 설정
/// PrevPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
/// </summary>
/// <param name="node">현재 노드</param>
/// <param name="newPosition">새로운 위치</param>
/// <param name="motorDirection">모터이동방향</param>
public void SetPosition(MapNode node, AgvDirection motorDirection)
{
// 현재 위치를 이전 위치로 저장 (리프트 방향 계산용)
if (_currentNode != null && _currentNode.Id != node.Id)
{
_prevPosition = _currentPosition; // 이전 위치
_prevNode = _currentNode;
_prevDirection = _currentDirection;
}
////모터방향이 다르다면 적용한다
//if (_currentDirection != motorDirection)
//{
// _prevDirection = motorDirection;
//}
// 새로운 위치 설정
_currentPosition = node.Position;
_currentDirection = motorDirection;
_currentNode = node;
// 🔥 노드 ID를 RFID로 간주하여 감지 목록에 추가 (시뮬레이터용)
if (!string.IsNullOrEmpty(node.Id) && !_detectedRfids.Contains(node.Id))
{
_detectedRfids.Add(node.Id);
}
// 🔥 RFID 2개 이상 감지 시 위치 확정
if (_detectedRfids.Count >= 2 && !_isPositionConfirmed)
{
_isPositionConfirmed = true;
}
//현재 경로값이 있는지 확인한다.
if (CurrentPath != null && CurrentPath.DetailedPath != null && CurrentPath.DetailedPath.Any())
{
var item = CurrentPath.DetailedPath.FirstOrDefault(t => t.NodeId == node.Id && t.IsPass == false);
if (item != null)
{
// [PathJump Check] 점프한 노드 개수 확인
// 현재 노드(item)보다 이전인데 아직 IsPass가 안 된 노드의 개수
int skippedCount = CurrentPath.DetailedPath.Count(t => t.seq < item.seq && t.IsPass == false);
if (skippedCount > 2)
{
OnError($"PathJump: {skippedCount}개의 노드를 건너뛰었습니다. (허용: 2개, 현재노드: {node.Id})");
return;
}
//item.IsPass = true;
//이전노드는 모두 지나친걸로 한다
CurrentPath.DetailedPath.Where(t => t.seq < item.seq).ToList().ForEach(t => t.IsPass = true);
}
}
// 위치 변경 이벤트 발생
PositionChanged?.Invoke(this, (_currentPosition, _currentDirection, _currentNode));
}
#endregion
/// <summary>
/// 노드 ID를 RFID 값으로 변환 (NodeResolver 사용)
/// </summary>
public ushort GetRfidByNodeId(List<MapNode> _mapNodes, string nodeId)
{
var node = _mapNodes?.FirstOrDefault(n => n.Id == nodeId);
if ((node?.HasRfid() ?? false) == false) return 0;
return node.RfidId;
}
#region Private Methods
/// <summary>
/// DetailedPath에서 노드 정보를 찾아 AGVCommand 생성
/// </summary>
private AGVCommand GetCommandFromPath(MapNode targetNode, string actionDescription)
{
// DetailedPath가 없으면 기본 명령 반환
if (_currentPath == null || _currentPath.DetailedPath == null || _currentPath.DetailedPath.Count == 0)
{
// [Refactor] Predict와 일관성 유지: 경로가 없으면 정지
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
eAGVCommandReason.NoPath,
$"{actionDescription} (DetailedPath 없음)"
);
}
// DetailedPath에서 targetNodeId에 해당하는 NodeMotorInfo 찾기
// 지나가지 않은 경로를 찾는다
var nodeInfo = _currentPath.DetailedPath.FirstOrDefault(n => n.NodeId == targetNode.Id && n.IsPass == false);
if (nodeInfo == null)
{
// 못 찾으면 기본 명령 반환
var defaultMotor = _currentDirection == AgvDirection.Forward
? MotorCommand.Forward
: MotorCommand.Backward;
return new AGVCommand(
defaultMotor,
MagnetPosition.S,
SpeedLevel.M,
eAGVCommandReason.NoTarget,
$"{actionDescription} (노드 {targetNode.Id} 정보 없음)"
);
}
// MotorDirection → MotorCommand 변환
MotorCommand motorCmd;
switch (nodeInfo.MotorDirection)
{
case AgvDirection.Forward:
motorCmd = MotorCommand.Forward;
break;
case AgvDirection.Backward:
motorCmd = MotorCommand.Backward;
break;
default:
motorCmd = MotorCommand.Stop;
break;
}
// MagnetDirection → MagnetPosition 변换
MagnetPosition magnetPos;
switch (nodeInfo.MagnetDirection)
{
case PathFinding.Planning.MagnetDirection.Left:
magnetPos = MagnetPosition.L;
break;
case PathFinding.Planning.MagnetDirection.Right:
magnetPos = MagnetPosition.R;
break;
case PathFinding.Planning.MagnetDirection.Straight:
default:
magnetPos = MagnetPosition.S;
break;
}
// [Speed Control] NodeMotorInfo에 설정된 속도 사용
// 단, 회전 구간 등에서 안전을 위해 강제 감속이 필요한 경우 로직 추가 가능
// 현재는 사용자 설정 우선
SpeedLevel speed = nodeInfo.Speed;
// Optional: 회전 시 강제 감속 로직 (사용자 요청에 따라 주석 처리 또는 제거 가능)
// if (nodeInfo.CanRotate || nodeInfo.IsDirectionChangePoint) speed = SpeedLevel.L;
return new AGVCommand(
motorCmd,
magnetPos,
speed,
eAGVCommandReason.Normal,
$"{actionDescription} → {targetNode.Id} (Motor:{motorCmd}, Magnet:{magnetPos})"
);
}
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);
}
public MapNode StartNode { get; set; } = null;
private MapNode _targetnode = null;
/// <summary>
/// 목적지를 설정합니다. 목적지가 변경되면 경로계산정보가 삭제 됩니다.
/// </summary>
public MapNode TargetNode
{
get
{
return _targetnode;
}
set
{
if (_targetnode != value)
{
_currentPath = null;
_targetnode = value;
}
}
}
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 void OnError(string message)
{
SetState(AGVState.Error);
ErrorOccurred?.Invoke(this, message);
}
#endregion
#region Cleanup
/// <summary>
/// 리소스 정리
/// </summary>
public void Dispose()
{
StopPath();
}
#endregion
}
}

View File

@@ -0,0 +1,327 @@
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.Id] = junctionInfo;
}
}
}
/// <summary>
/// 개별 노드의 갈림길 정보 분석
/// </summary>
private JunctionInfo AnalyzeNode(MapNode node)
{
var junction = new JunctionInfo(node.Id);
// 양방향 연결을 고려하여 모든 연결된 노드 찾기
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 connectedNode in node.ConnectedMapNodes)
{
if (connectedNode != null)
{
connected.Add(connectedNode.Id);
}
}
// 역방향 연결된 노드들 (다른 노드에서 이 노드로 연결)
foreach (var otherNode in _mapNodes)
{
if (otherNode.Id != node.Id && otherNode.ConnectedMapNodes.Any(n => n.Id == node.Id))
{
connected.Add(otherNode.Id);
}
}
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.Id == 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>
/// <param name="fromNodeId">이전 노드 ID</param>
/// <param name="currentNodeId">현재 노드 ID</param>
/// <param name="toNodeId">목표 노드 ID</param>
/// <param name="motorDirection">AGV 모터 방향 (Forward/Backward)</param>
/// <returns>마그넷 방향 (모터 방향 고려)</returns>
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId, AgvDirection motorDirection )
{
if (!_junctions.ContainsKey(currentNodeId))
return MagnetDirection.Straight;
var junction = _junctions[currentNodeId];
if (!junction.IsJunction)
return MagnetDirection.Straight;
// 실제 각도 기반으로 마그넷 방향 계산
var fromNode = _mapNodes.FirstOrDefault(n => n.Id == fromNodeId);
var currentNode = _mapNodes.FirstOrDefault(n => n.Id == currentNodeId);
var toNode = _mapNodes.FirstOrDefault(n => n.Id == 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;
// 전진 방향 기준으로 마그넷 방향 결정
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
MagnetDirection baseMagnetDirection;
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
baseMagnetDirection = MagnetDirection.Straight;
else if (angleDiff < 0) // 음수면 왼쪽 회전
baseMagnetDirection = MagnetDirection.Left;
else // 양수면 오른쪽 회전
baseMagnetDirection = MagnetDirection.Right;
// 후진 모터 방향일 경우 마그넷 방향 반대로 설정
// Forward: Left/Right 그대로 사용
// Backward: Left ↔ Right 반대로 사용
if (motorDirection == AgvDirection.Backward)
{
if (baseMagnetDirection == MagnetDirection.Left)
return MagnetDirection.Right;
else if (baseMagnetDirection == MagnetDirection.Right)
return MagnetDirection.Left;
}
return baseMagnetDirection;
}
/// <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;
}
}
}

View File

@@ -0,0 +1,314 @@
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>
/// 경로 노드 목록 (시작 → 목적지 순서)
/// </summary>
public List<MapNode> Path { get; set; }
/// <summary>
/// AGV 명령어 목록 (이동 방향 시퀀스)
/// </summary>
public List<AgvDirection> Commands { 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 Message { 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 MapNode PrevNode { get; set; }
/// <summary>
/// PrevNode 에서 현재위치까지 이동한 모터의 방향값
/// </summary>
public AgvDirection PrevDirection { get; set; }
public MapNode Gateway { get; set; }
/// <summary>
/// 기본 생성자
/// </summary>
public AGVPathResult()
{
Success = false;
Path = new List<MapNode>();
Commands = new List<AgvDirection>();
DetailedPath = new List<NodeMotorInfo>();
TotalDistance = 0;
CalculationTimeMs = 0;
ExploredNodes = 0;
EstimatedTimeSeconds = 0;
RotationCount = 0;
Message = string.Empty;
PlanDescription = string.Empty;
RequiredDirectionChange = false;
DirectionChangeNode = string.Empty;
DockingValidation = DockingValidationResult.CreateNotRequired();
PrevNode = null;
PrevDirection = AgvDirection.Stop;
}
/// <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<MapNode> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
{
var result = new AGVPathResult
{
Success = true,
Path = new List<MapNode>(path),
Commands = new List<AgvDirection>(commands),
TotalDistance = totalDistance,
CalculationTimeMs = calculationTimeMs
};
result.CalculateMetrics();
return result;
}
/// <summary>
/// 실패 결과 생성
/// </summary>
/// <param name="errorMessage">오류 메시지</param>
/// <param name="calculationTimeMs">계산 시간</param>
/// <param name="exploredNodes">탐색된 노드 수</param>
/// <returns>실패 결과</returns>
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs = 0, int exploredNodes = 0)
{
return new AGVPathResult
{
Success = false,
Message = errorMessage,
CalculationTimeMs = calculationTimeMs,
ExploredNodes = exploredNodes
};
}
/// <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 GetDetailedPathInfo(bool shortmessage = false)
{
if (!Success)
{
return $"경로 계산 실패: {Message} (계산시간: {CalculationTimeMs}ms)";
}
var data = DetailedPath.Select(t =>
{
if (shortmessage)
return $"{t.RfidId:00}{t.MotorDirection.ToString().Substring(0, 1)}{t.MagnetDirection.ToString().Substring(0, 1)}";
else
return $"{t.RfidId}[{t.NodeId}] {t.MotorDirection.ToString().Substring(0, 1)}-{t.MagnetDirection.ToString().Substring(0, 1)}";
});
return string.Join(" → ", data);
}
/// <summary>
/// 단순 경로 목록 반환 (호환성용 - 노드 ID 문자열 목록)
/// </summary>
/// <returns>노드 ID 목록</returns>
public List<string> GetSimplePath()
{
if (DetailedPath != null && DetailedPath.Count > 0)
{
return DetailedPath.Select(n => n.NodeId).ToList();
}
return Path?.Select(n => n.Id).ToList() ?? 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: {Message}";
}
}
}
}

View File

@@ -0,0 +1,622 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.PathFinding.Core
{
/// <summary>
/// A* 알고리즘 기반 경로 탐색기
/// </summary>
public class AStarPathfinder
{
private Dictionary<string, PathNode> _nodeMap;
private List<MapNode> _mapNodes;
private Dictionary<string, MapNode> _mapNodeLookup; // Quick lookup for node ID -> MapNode
/// <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>();
_mapNodeLookup = new Dictionary<string, MapNode>();
}
/// <summary>
/// 맵 노드 설정
/// </summary>
/// <param name="mapNodes">맵 노드 목록</param>
public void SetMapNodes(List<MapNode> mapNodes)
{
_mapNodes = mapNodes ?? new List<MapNode>();
_nodeMap.Clear();
_mapNodeLookup.Clear();
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
foreach (var mapNode in _mapNodes)
{
_mapNodeLookup[mapNode.Id] = mapNode; // Add to lookup table
if (mapNode.IsNavigationNode())
{
var pathNode = new PathNode(mapNode.Id, mapNode.Position);
_nodeMap[mapNode.Id] = pathNode;
}
}
// 단일 연결을 양방향으로 확장
foreach (var mapNode in _mapNodes)
{
if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.Id))
{
var pathNode = _nodeMap[mapNode.Id];
foreach (var connectedNode in mapNode.ConnectedMapNodes)
{
if (connectedNode != null && _nodeMap.ContainsKey(connectedNode.Id))
{
// 양방향 연결 생성 (단일 연결이 양방향을 의미)
if (!pathNode.ConnectedNodes.Contains(connectedNode.Id))
{
pathNode.ConnectedNodes.Add(connectedNode.Id);
}
var connectedPathNode = _nodeMap[connectedNode.Id];
if (!connectedPathNode.ConnectedNodes.Contains(mapNode.Id))
{
connectedPathNode.ConnectedNodes.Add(mapNode.Id);
}
}
}
}
}
}
/// <summary>
/// 노드 ID로 MapNode 가져오기 (헬퍼 메서드)
/// </summary>
private MapNode GetMapNode(string nodeId)
{
return _mapNodeLookup.ContainsKey(nodeId) ? _mapNodeLookup[nodeId] : null;
}
/// <summary>
/// 경로 찾기 (A* 알고리즘)
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindPathAStar(MapNode start, MapNode end)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(start.Id))
{
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {start.Id}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(end.Id))
{
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {end.Id}", stopwatch.ElapsedMilliseconds, 0);
}
//출발지와 목적지가 동일한 경우
if (start.Id == end.Id)
{
//var startMapNode = GetMapNode(start);
var singlePath = new List<MapNode> { start };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
var startNode = _nodeMap[start.Id];
var endNode = _nodeMap[end.Id];
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 == end.Id)
{
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>
///// 경유지를 거쳐 경로 찾기 (오버로드)
///// 여러 경유지를 순차적으로 거쳐서 최종 목적지까지의 경로를 계산합니다.
///// 기존 FindPath를 여러 번 호출하여 각 구간의 경로를 합칩니다.
///// </summary>
///// <param name="startNodeId">시작 노드 ID</param>
///// <param name="endNodeId">최종 목적지 노드 ID</param>
///// <param name="waypointNodeIds">경유지 노드 ID 배열 (선택사항)</param>
///// <returns>경로 계산 결과 (모든 경유지를 거친 전체 경로)</returns>
//public AGVPathResult FindPath(string startNodeId, string endNodeId, params string[] waypointNodeIds)
//{
// var stopwatch = System.Diagnostics.Stopwatch.StartNew();
// try
// {
// // 경유지가 없으면 기본 FindPath 호출
// if (waypointNodeIds == null || waypointNodeIds.Length == 0)
// {
// return FindPathAStar(startNodeId, endNodeId);
// }
// // 경유지 유효성 검증
// var validWaypoints = new List<string>();
// foreach (var waypointId in waypointNodeIds)
// {
// if (string.IsNullOrEmpty(waypointId))
// continue;
// if (!_nodeMap.ContainsKey(waypointId))
// {
// return AGVPathResult.CreateFailure($"경유지 노드를 찾을 수 없습니다: {waypointId}", stopwatch.ElapsedMilliseconds, 0);
// }
// validWaypoints.Add(waypointId);
// }
// // 경유지가 없으면 기본 경로 계산
// if (validWaypoints.Count == 0)
// {
// return FindPathAStar(startNodeId, endNodeId);
// }
// // 첫 번째 경유지가 시작노드와 같은지 검사
// if (validWaypoints[0] == startNodeId)
// {
// return AGVPathResult.CreateFailure(
// $"첫 번째 경유지({validWaypoints[0]})가 시작 노드({startNodeId})와 동일합니다. 경유지는 시작노드와 달라야 합니다.",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 마지막 경유지가 목적지노드와 같은지 검사
// if (validWaypoints[validWaypoints.Count - 1] == endNodeId)
// {
// return AGVPathResult.CreateFailure(
// $"마지막 경유지({validWaypoints[validWaypoints.Count - 1]})가 목적지 노드({endNodeId})와 동일합니다. 경유지는 목적지노드와 달라야 합니다.",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 연속된 중복만 제거 (순서 유지)
// // 예: [1, 2, 2, 3, 2] -> [1, 2, 3, 2] (연속 중복만 제거)
// var deduplicatedWaypoints = new List<string>();
// string lastWaypoint = null;
// foreach (var waypoint in validWaypoints)
// {
// if (waypoint != lastWaypoint)
// {
// deduplicatedWaypoints.Add(waypoint);
// lastWaypoint = waypoint;
// }
// }
// validWaypoints = deduplicatedWaypoints;
// // 최종 경로 리스트와 누적 값
// var combinedPath = new List<MapNode>();
// float totalDistance = 0;
// long totalCalculationTime = 0;
// // 현재 시작점
// string currentStart = startNodeId;
// // 1단계: 각 경유지까지의 경로 계산
// for (int i = 0; i < validWaypoints.Count; i++)
// {
// string waypoint = validWaypoints[i];
// // 현재 위치에서 경유지까지의 경로 계산
// var segmentResult = FindPathAStar(currentStart, waypoint);
// if (!segmentResult.Success)
// {
// return AGVPathResult.CreateFailure(
// $"경유지 {i + 1}({waypoint})까지의 경로 계산 실패: {segmentResult.ErrorMessage}",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 경로 합치기 (첫 번째 구간이 아니면 시작점 제거하여 중복 방지)
// if (combinedPath.Count > 0 && segmentResult.Path.Count > 0)
// {
// // 시작 노드 제거 (이전 경로의 마지막 노드와 동일)
// combinedPath.AddRange(segmentResult.Path.Skip(1));
// }
// else
// {
// combinedPath.AddRange(segmentResult.Path);
// }
// totalDistance += segmentResult.TotalDistance;
// totalCalculationTime += segmentResult.CalculationTimeMs;
// // 다음 경유지의 시작점은 현재 경유지
// currentStart = waypoint;
// }
// // 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
// var finalSegmentResult = FindPathAStar(currentStart, endNodeId);
// if (!finalSegmentResult.Success)
// {
// return AGVPathResult.CreateFailure(
// $"최종 목적지까지의 경로 계산 실패: {finalSegmentResult.ErrorMessage}",
// stopwatch.ElapsedMilliseconds, 0);
// }
// // 최종 경로 합치기 (시작점 제거)
// if (combinedPath.Count > 0 && finalSegmentResult.Path.Count > 0)
// {
// combinedPath.AddRange(finalSegmentResult.Path.Skip(1));
// }
// else
// {
// combinedPath.AddRange(finalSegmentResult.Path);
// }
// totalDistance += finalSegmentResult.TotalDistance;
// totalCalculationTime += finalSegmentResult.CalculationTimeMs;
// stopwatch.Stop();
// // 결과 생성
// return AGVPathResult.CreateSuccess(
// combinedPath,
// new List<AgvDirection>(),
// totalDistance,
// totalCalculationTime
// );
// }
// catch (Exception ex)
// {
// return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
// }
//}
/// <summary>
/// 두 경로 결과를 합치기
/// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침
/// </summary>
/// <param name="previousResult">이전 경로 결과</param>
/// <param name="currentResult">현재 경로 결과</param>
/// <returns>합쳐진 경로 결과</returns>
public AGVPathResult CombineResults( AGVPathResult previousResult, AGVPathResult currentResult)
{
// 입력 검증
if (previousResult == null)
return currentResult;
if (currentResult == null)
return previousResult;
if (!previousResult.Success)
return AGVPathResult.CreateFailure(
$"이전 경로 결과 실패: {previousResult.Message}",
previousResult.CalculationTimeMs);
if (!currentResult.Success)
return AGVPathResult.CreateFailure(
$"현재 경로 결과 실패: {currentResult.Message}",
currentResult.CalculationTimeMs);
// 경로가 비어있는 경우 처리
if (previousResult.Path == null || previousResult.Path.Count == 0)
return currentResult;
if (currentResult.Path == null || currentResult.Path.Count == 0)
return previousResult;
// 합친 경로 생성
var combinedPath = new List<MapNode>(previousResult.Path);
var combinedCommands = new List<AgvDirection>(previousResult.Commands);
var combinedDetailedPath = new List<NodeMotorInfo>(previousResult.DetailedPath ?? new List<NodeMotorInfo>());
// 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교
string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].Id;
string firstNodeOfCurrent = currentResult.Path[0].Id;
if (lastNodeOfPrevious == firstNodeOfCurrent)
{
// 첫 번째 노드 제거 (중복 제거)
combinedPath.RemoveAt(combinedPath.Count - 1);
combinedPath.AddRange(currentResult.Path);
// DetailedPath도 첫 번째 노드 제거
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
{
combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1);
combinedDetailedPath.AddRange(currentResult.DetailedPath);
}
}
else
{
// 그대로 붙임
combinedPath.AddRange(currentResult.Path);
// DetailedPath도 그대로 붙임
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
{
combinedDetailedPath.AddRange(currentResult.DetailedPath);
}
}
// 명령어 합치기
combinedCommands.AddRange(currentResult.Commands);
// 총 거리 합산
float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance;
// 계산 시간 합산
long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs;
// 합쳐진 결과 생성
var result = AGVPathResult.CreateSuccess(
combinedPath,
combinedCommands,
combinedDistance,
combinedCalculationTime
);
// DetailedPath 설정
result.DetailedPath = combinedDetailedPath;
result.PrevNode = previousResult.PrevNode;
result.PrevDirection = previousResult.PrevDirection;
return result;
}
///// <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 = FindPathAStar(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<MapNode> ReconstructPath(PathNode endNode)
{
var path = new List<MapNode>();
var current = endNode;
while (current != null)
{
var mapNode = GetMapNode(current.NodeId);
if (mapNode != null)
{
path.Add(mapNode);
}
current = current.Parent;
}
path.Reverse();
return path;
}
/// <summary>
/// 경로의 총 거리 계산
/// </summary>
private float CalculatePathDistance(List<MapNode> path)
{
if (path.Count < 2) return 0;
float totalDistance = 0;
for (int i = 0; i < path.Count - 1; i++)
{
var nodeId1 = path[i].Id;
var nodeId2 = path[i + 1].Id;
if (_nodeMap.ContainsKey(nodeId1) && _nodeMap.ContainsKey(nodeId2))
{
totalDistance += _nodeMap[nodeId1].DistanceTo(_nodeMap[nodeId2]);
}
}
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;
}
/// <summary>
/// 방향 전환을 위한 대체 노드 찾기
/// 교차로에 연결된 노드 중에서 왔던 길과 갈 길이 아닌 다른 노드를 찾음
/// 방향 전환 시 왕복 경로에 사용될 노드
/// </summary>
/// <param name="junctionNodeId">교차로 노드 ID (B)</param>
/// <param name="previousNodeId">이전 노드 ID (A - 왔던 길)</param>
/// <param name="targetNodeId">목표 노드 ID (C - 갈 길)</param>
/// <param name="mapNodes">전체 맵 노드 목록</param>
/// <returns>방향 전환에 사용할 대체 노드, 없으면 null</returns>
public MapNode FindAlternateNodeForDirectionChange(
string junctionNodeId,
string previousNodeId,
string targetNodeId)
{
// 입력 검증
if (string.IsNullOrEmpty(junctionNodeId) || string.IsNullOrEmpty(previousNodeId) || string.IsNullOrEmpty(targetNodeId))
return null;
if (_mapNodes == null || _mapNodes.Count == 0)
return null;
// 교차로 노드 찾기
var junctionNode = _mapNodes.FirstOrDefault(n => n.Id == junctionNodeId);
if (junctionNode == null || junctionNode.ConnectedNodes == null || junctionNode.ConnectedNodes.Count == 0)
return null;
// 교차로에 연결된 모든 노드 중에서 조건을 만족하는 노드 찾기
// 조건:
// 1. 이전 노드(왔던 길)가 아님
// 2. 목표 노드(갈 길)가 아님
// 3. 실제로 존재하는 노드
// 4. 활성 상태인 노드
// 5. 네비게이션 가능한 노드
var alternateNodes = new List<MapNode>();
foreach (var connectedNodeId in junctionNode.ConnectedNodes)
{
if (connectedNodeId == null) continue;
// 조건 1: 왔던 길이 아님
if (connectedNodeId == previousNodeId) continue;
// 조건 2: 갈 길이 아님
if (connectedNodeId == targetNodeId) continue;
// 조건 3, 4, 5: 존재하고, 활성 상태이고, 네비게이션 가능
var connectedNode = _mapNodes.FirstOrDefault(n => n.Id == connectedNodeId);
if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
{
alternateNodes.Add(connectedNode);
}
}
// 찾은 노드가 없으면 null 반환
if (alternateNodes.Count == 0)
return null;
// 여러 개 찾았으면 첫 번째 노드 반환
// (필요시 거리 기반으로 가장 가까운 노드를 선택할 수도 있음)
return alternateNodes[0];
}
}
}

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;
}
}
}

View File

@@ -0,0 +1,749 @@
using System;
using System.Collections.Generic;
using System.Drawing;
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 DirectionalPathfinder _directionPathfinder;
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);
_directionPathfinder = new DirectionalPathfinder();
}
/// <summary>
/// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
/// </summary>
/// <param name="startNode">기준이 되는 노드</param>
/// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
public MapNode FindNearestJunction(MapNode startNode)
{
if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
return null;
// 교차로: 3개 이상의 노드가 연결된 노드
var junctions = _mapNodes.Where(n =>
n.IsActive &&
n.IsNavigationNode() &&
n.ConnectedNodes != null &&
n.DisableCross == false &&
n.ConnectedNodes.Count >= 3 &&
n.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false &&
n.Id != startNode.Id
).ToList();
// docking 포인트가 연결된 노드는 제거한다.
if (junctions.Count == 0)
return null;
// 직선 거리 기반으로 가장 가까운 교차로 찾기
MapNode nearestJunction = null;
float minDistance = float.MaxValue;
foreach (var junction in junctions)
{
float dx = junction.Position.X - startNode.Position.X;
float dy = junction.Position.Y - startNode.Position.Y;
float distance = (float)Math.Sqrt(dx * dx + dy * dy);
if (distance < minDistance)
{
minDistance = distance;
nearestJunction = junction;
}
}
return nearestJunction;
}
/// <summary>
/// 지정한 노드에서 경로상 가장 가까운 교차로를 찾는다.
/// (최단 경로 내에서 3개 이상 연결된 교차로를 찾음)
/// </summary>
/// <param name="startNode">시작 노드</param>
/// <param name="targetNode">목적지 노드</param>
/// <returns>경로상의 가장 가까운 교차로 노드 (또는 null)</returns>
public MapNode FindNearestJunctionOnPath(AGVPathResult pathResult)
{
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
return null;
// 경로상의 모든 노드 중 교차로(3개 이상 연결) 찾기
var StartNode = pathResult.Path.First();
foreach (var pathNode in pathResult.Path)
{
if (pathNode != null &&
pathNode.IsActive &&
pathNode.IsNavigationNode() &&
pathNode.DisableCross == false &&
pathNode.ConnectedNodes != null &&
pathNode.ConnectedNodes.Count >= 3 &&
pathNode.ConnectedMapNodes.Where(t => t.CanDocking).Any() == false)
{
if (pathNode.Id.Equals(StartNode.Id) == false)
return pathNode;
}
}
return null;
}
public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode)
{
// 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
return _basicPathfinder.FindPathAStar(startNode, targetNode);
}
/// <summary>
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
/// </summary>
/// <summary>
/// 단순 경로 찾기 (복잡한 제약조건/방향전환 로직 없이 A* 결과만 반환)
/// </summary>
public AGVPathResult FindBasicPath(MapNode startNode, MapNode targetNode, MapNode _prevNode, AgvDirection prevDirection)
{
// 1. 입력 검증
if (startNode == null || targetNode == null)
return AGVPathResult.CreateFailure("노드 정보 오류", 0, 0);
// 2. A* 경로 탐색
var pathResult = _basicPathfinder.FindPathAStar(startNode, targetNode);
pathResult.PrevNode = _prevNode;
pathResult.PrevDirection = prevDirection;
if (!pathResult.Success)
return AGVPathResult.CreateFailure(pathResult.Message ?? "경로 없음", 0, 0);
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
// 3. 상세 데이터 생성 (갈림길 마그넷 방향 계산 포함)
if (pathResult.Path != null && pathResult.Path.Count > 0)
{
var detailedPath = new List<NodeMotorInfo>();
for (int i = 0; i < pathResult.Path.Count; i++)
{
var node = pathResult.Path[i];
var nextNode = (i + 1 < pathResult.Path.Count) ? pathResult.Path[i + 1] : null;
// 마그넷 방향 계산 (갈림길인 경우)
// 마그넷 방향 계산 (갈림길인 경우)
MagnetDirection magnetDirection = MagnetDirection.Straight;
//갈림길에 있다면 미리 방향을 저장해준다.
if ((node.ConnectedNodes?.Count ?? 0) > 2 && nextNode != null)
{
//다음 노드ID를 확인해서 마그넷 방향 데이터를 찾는다.
if (node.MagnetDirections.ContainsKey(nextNode.Id) == false)
{
return AGVPathResult.CreateFailure($"{node.ID2}->{nextNode.ID2} 의 (목표)갈림길 방향이 입력되지 않았습니다", 0, 0);
}
else
{
var magdir = node.MagnetDirections[nextNode.Id].ToString();
if (magdir == "L") magnetDirection = MagnetDirection.Left;
else if (magdir == "R") magnetDirection = MagnetDirection.Right;
}
}
var nodeInfo = new NodeMotorInfo(i + 1, node.Id, node.RfidId, prevDirection, nextNode, magnetDirection);
// 속도 설정
var mapNode = _mapNodes.FirstOrDefault(n => n.Id == node.Id);
if (mapNode != null)
{
nodeInfo.Speed = mapNode.SpeedLimit;
detailedPath.Add(nodeInfo);
}
}
pathResult.DetailedPath = detailedPath;
}
return pathResult;
}
/// <summary>
/// 길목(Gateway) 기반 고급 경로 계산 (기존 SimulatorForm.CalcPath 이관)
/// </summary>
public AGVPathResult CalculatePath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection prevDir)
{
AGVPathResult Retval;
// var o_StartNode = startNode;
// startNode, targetNode는 이미 인자로 받음
if (startNode == null || targetNode == null) return AGVPathResult.CreateFailure("시작/종료노드가 지정되지 않음");
try
{
// 종료노드라면 이전위치로 이동시켜야한다. (Simulator Logic)
// 만약 시작노드가 끝단(ConnectedMapNodes.Count == 1)이라면,
// AGV가 해당 노드에 '도착'한 상태가 아니라 '작업' 중일 수 있으므로
// 이전 노드(진입점)로 위치를 보정하여 경로를 계산한다.
AGVPathResult LimitPath = null;
if (startNode.ConnectedMapNodes.Count == 1)
{
// 시작점 -> 이전점 경로 (보통 후진이나 전진 1칸)
LimitPath = this.FindPathAStar(startNode, prevNode);
if (LimitPath.Success)
{
for (int i = 0; i < LimitPath.Path.Count; i++)
{
var nodeinfo = LimitPath.Path[i];
var dir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
LimitPath.DetailedPath.Add(new NodeMotorInfo(i + 1, nodeinfo.Id, nodeinfo.RfidId, dir));
}
// 시작 위치 및 방향 변경
// var org_start = startNode; // Unused
startNode = prevNode;
prevNode = LimitPath.Path.First(); // startNode (original)
prevDir = (prevDir == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
}
else
{
// 경로 생성 실패 시 보정 없이 진행하거나 에러 처리
// 여기서는 일단 기존 로직대로 진행
}
}
// 2. Buffer-to-Buffer 예외 처리
// 05~31 구간 체크
var node05 = _mapNodes.FirstOrDefault(n => n.RfidId == 5);
var node31 = _mapNodes.FirstOrDefault(n => n.RfidId == 31);
bool fixpath = false;
Retval = null;
MapNode gatewayNode = null;
if (node05 != null && node31 != null)
{
// 버퍼 구간 경로 테스트
var rlt = this.FindPathAStar(node05, node31);
if (rlt.Success)
{
// 버퍼구간내에 시작과 종료가 모두 포함되어있다
if (rlt.Path.Find(n => n.Id == startNode.Id) != null &&
rlt.Path.Find(n => n.Id == targetNode.Id) != null)
{
Retval = CalcPathBufferToBuffer(startNode, targetNode, prevNode, prevDir);
fixpath = true;
}
}
}
if (!fixpath)
{
// 3. 목적지별 Gateway 및 진입 조건 확인
gatewayNode = GetGatewayNode(targetNode);
if (gatewayNode == null)
{
// 게이트웨이가 없는 경우라면(일반 노드 등), Gateway 로직 없이 기본 경로 탐색
Retval = this.FindBasicPath(startNode, targetNode, prevNode, prevDir);
}
else
{
// Gateway Node 찾음
// 4. Start -> Gateway 경로 계산 (A*)
var pathToGateway = this.FindBasicPath(startNode, gatewayNode, prevNode, prevDir);
if (pathToGateway.Success == false)
return AGVPathResult.CreateFailure($"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.Message}");
// 방향을 확인하여, 왔던 방향으로 되돌아가야 한다면 방향 반전
if (pathToGateway.Path.Count > 1)
{
var predictNext = pathToGateway.Path[1];
if (predictNext.Id == prevNode.Id)
{
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
foreach (var item in pathToGateway.DetailedPath)
item.MotorDirection = reverseDir;
}
}
// 마지막 경로는 게이트웨이이므로 제거 (Gateway 진입 후 처리는 GetPathFromGateway에서 담당)
if (pathToGateway.Path.Count > 0 && pathToGateway.Path.Last().Id == gatewayNode.Id)
{
var idx = pathToGateway.Path.Count - 1;
pathToGateway.Path.RemoveAt(idx);
pathToGateway.DetailedPath.RemoveAt(idx);
}
// 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
MapNode GateprevNode = pathToGateway.Path.LastOrDefault() ?? prevNode;
NodeMotorInfo GatePrevDetail = pathToGateway.DetailedPath.LastOrDefault();
var arrivalOrientation = GatePrevDetail?.MotorDirection ?? prevDir;
var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, GateprevNode, arrivalOrientation);
if (!gatewayPathResult.Success)
return AGVPathResult.CreateFailure($"{gatewayPathResult.Message}");
Retval = CombinePaths(pathToGateway, gatewayPathResult);
}
}
//게이트웨이
Retval.Gateway = gatewayNode;
// 경로 오류 검사
if (Retval == null || Retval.Success == false) return Retval ?? AGVPathResult.CreateFailure("경로 계산 결과 없음");
if (LimitPath != null)
{
Retval = CombinePaths(LimitPath, Retval);
}
// 해당 경로와 대상의 도킹포인트 방향 검사
if (targetNode.DockDirection != DockingDirection.DontCare)
{
var lastPath = Retval.DetailedPath.LastOrDefault();
if (lastPath != null)
{
if (targetNode.DockDirection == DockingDirection.Forward && lastPath.MotorDirection != AgvDirection.Forward)
{
return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(FWD) Target:{targetNode.DockDirection}");
}
if (targetNode.DockDirection == DockingDirection.Backward && lastPath.MotorDirection != AgvDirection.Backward)
{
return AGVPathResult.CreateFailure($"생성된 경로와 목적지의 도킹방향이 일치하지 않습니다(BWD) Target:{targetNode.DockDirection}");
}
}
}
// 경로 최적화: A -> B -> A 패턴 제거
// 6[F][R] → 13[B][L] → 6[F][L] 같은 경우 제거
while (fixpath == false)
{
var updatecount = 0;
for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
{
var n1 = Retval.DetailedPath[i];
var n2 = Retval.DetailedPath[i + 1];
var n3 = Retval.DetailedPath[i + 2];
if (n1.NodeId == n3.NodeId)
{
bool isInverse = false;
// 1. 모터 방향이 반대인가? (F <-> B)
bool isMotorInverse = (n1.MotorDirection != n2.MotorDirection) &&
(n1.MotorDirection == AgvDirection.Forward || n1.MotorDirection == AgvDirection.Backward) &&
(n2.MotorDirection == AgvDirection.Forward || n2.MotorDirection == AgvDirection.Backward);
if (isMotorInverse)
{
// 2. 마그넷 방향이 반대인가? (L <-> R, S <-> S)
bool isMagnetInverse = false;
if (n1.MagnetDirection == MagnetDirection.Straight && n2.MagnetDirection == MagnetDirection.Straight) isMagnetInverse = true;
else if (n1.MagnetDirection == MagnetDirection.Left && n2.MagnetDirection == MagnetDirection.Right) isMagnetInverse = true;
else if (n1.MagnetDirection == MagnetDirection.Right && n2.MagnetDirection == MagnetDirection.Left) isMagnetInverse = true;
if (isMagnetInverse) isInverse = true;
}
if (isInverse)
{
// 제자리 회귀 경로 발견 -> 앞의 두 노드(n1, n2)를 제거하여 n3만 남김
Retval.DetailedPath.RemoveAt(i);
Retval.DetailedPath.RemoveAt(i);
if (Retval.Path.Count > i + 1)
{
Retval.Path.RemoveAt(i);
Retval.Path.RemoveAt(i);
}
i--; // 인덱스 재조정
updatecount += 1;
}
}
}
if (updatecount == 0) break;
}
// 불가능한 회전 경로 검사 (사용자 요청 로직 반영)
for (int i = 0; i < Retval.DetailedPath.Count - 2; i++)
{
var n1 = Retval.DetailedPath[i];
var n2 = Retval.DetailedPath[i + 1];
var n3 = Retval.DetailedPath[i + 2];
if (n1.NodeId == n3.NodeId &&
n1.MotorDirection == n3.MotorDirection &&
n1.MotorDirection == n2.MotorDirection) // Fix: 중간 노드 방향도 같을 때만 에러
{
return AGVPathResult.CreateFailure($"불가능한 회전 경로가 포함되어있습니다. {n1.RfidId}->{n2.RfidId}->{n3.RfidId}");
}
}
// 기타 검증 로직 (마지막 노드 도킹, 시작노드 일치 등)
var lastnode = Retval.Path.Last();
if (lastnode.StationType != StationType.Normal)
{
var lastnodePath = Retval.DetailedPath.Last();
if (lastnode.DockDirection == DockingDirection.Forward && lastnodePath.MotorDirection != AgvDirection.Forward)
return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
if (lastnode.DockDirection == DockingDirection.Backward && lastnodePath.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure($"목적지의 모터방향({lastnode.DockDirection}) 불일치 경로방향({lastnodePath.MotorDirection})");
}
// 첫번째 노드 일치 검사 - 필요시 수행 (startNode가 변경될 수 있어서 o_StartNode 등 필요할 수도 있음)
// 여기서는 생략 혹은 간단히 체크
// 되돌아가는 길 방향 일치 검사
if (Retval.DetailedPath.Count > 1)
{
var FirstDetailPath = Retval.DetailedPath[0];
var NextDetailPath = Retval.DetailedPath[1];
AgvDirection? PredictNextDir = null;
if (NextDetailPath.NodeId == prevNode.Id)
{
if (NextDetailPath.MagnetDirection == MagnetDirection.Straight)
PredictNextDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
}
if (PredictNextDir != null && (FirstDetailPath.MotorDirection != (AgvDirection)PredictNextDir))
{
// return AGVPathResult.CreateFailure($"되돌아가는 길인데 방향이 일치하지않음");
// 경고 수준이나 무시 가능한 경우도 있음
}
}
// 연결성 검사
for (int i = 0; i < Retval.DetailedPath.Count - 1; i++)
{
var cnode = Retval.Path[i];
var nnode = Retval.Path[i + 1];
if (cnode.ConnectedNodes.Contains(nnode.Id) == false && cnode.Id != nnode.Id)
{
return AGVPathResult.CreateFailure($"[{cnode.RfidId}] 노드에 연결되지 않은 [{nnode.RfidId}]노드가 지정됨");
}
}
//각 도킹포인트별로 절대 움직이면 안되는 조건확인
var firstnode = Retval.Path.FirstOrDefault();
var firstDet = Retval.DetailedPath.First();
var failmessage = $"[{firstnode.ID2}] 노드의 시작모터 방향({firstDet.MotorDirection})이 올바르지 않습니다";
if (firstnode.StationType == StationType.Charger1 && firstDet.MotorDirection != AgvDirection.Forward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Loader && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.UnLoader && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Clearner && firstDet.MotorDirection != AgvDirection.Backward)
return AGVPathResult.CreateFailure(failmessage);
else if (firstnode.StationType == StationType.Buffer)
{
//버퍼는 도킹이되어잇느닞 확인하고. 그때 방향을 체크해야한다.
}
return Retval;
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"[계산오류] {ex.Message}");
}
}
private AGVPathResult CalcPathBufferToBuffer(MapNode start, MapNode target, MapNode prev, AgvDirection prevDir)
{
// Monitor Side 판단 및 Buffer 간 이동 로직
int deltaX = 0;
int deltaY = 0;
if (prev == null) return AGVPathResult.CreateFailure("이전 노드 정보가 없습니다");
else
{
deltaX = start.Position.X - prev.Position.X;
deltaY = -(start.Position.Y - prev.Position.Y);
}
if (Math.Abs(deltaY) > Math.Abs(deltaX))
deltaX = deltaY;
bool isMonitorLeft = false;
if (deltaX > 0) // 오른쪽(Forward)으로 이동해 옴
isMonitorLeft = (prevDir == AgvDirection.Backward);
else if (deltaX < 0) // 왼쪽(Reverse)으로 이동해 옴
isMonitorLeft = (prevDir == AgvDirection.Forward);
else
return AGVPathResult.CreateFailure("이전 노드와의 방향을 알 수 없습니다");
if (isMonitorLeft)
{
// Monitor Left -> Gateway 탈출
var GateWayNode = _mapNodes.FirstOrDefault(n => n.RfidId == 6);
var reverseDir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
AGVPathResult escPath = null;
if (start.Position.X > prev.Position.X)
escPath = this.FindBasicPath(start, GateWayNode, prev, prevDir);
else
escPath = this.FindBasicPath(start, GateWayNode, prev, reverseDir);
if (!escPath.Success) return AGVPathResult.CreateFailure("버퍼 탈출 경로 실패");
var lastNode = escPath.Path.Last();
var lastPrev = escPath.Path[escPath.Path.Count - 2];
var lastDir = escPath.DetailedPath.Last().MotorDirection;
var gateToTarget = GetPathFromGateway(GateWayNode, target, lastPrev, lastDir);
escPath.Path.RemoveAt(escPath.Path.Count - 1);
escPath.DetailedPath.RemoveAt(escPath.DetailedPath.Count - 1);
return CombinePaths(escPath, gateToTarget);
}
else
{
// Monitor Right -> 직접 진입 또는 Overshoot
bool isTargetLeft = target.Position.X < start.Position.X;
if (target == start)
{
// 제자리 재정렬 (Same as Simulator logic)
var list = new List<MapNode>();
var retval = AGVPathResult.CreateSuccess(list, new List<AgvDirection>(), 0, 0);
var resversedir = prevDir == AgvDirection.Backward ? AgvDirection.Forward : AgvDirection.Backward;
retval.Path.Add(target);
if (deltaX < 0)
{
var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
if (nextNode != null)
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
retval.Path.Add(nextNode);
var lastDefailt = retval.DetailedPath.Last();
retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Forward)
{
Speed = SpeedLevel.M,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo((retval.DetailedPath.Max(t => t.seq) + 1), target.Id, target.RfidId, AgvDirection.Forward));
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
}
else
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, resversedir));
retval.Path.Add(prev);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Last().seq + 1, prev.Id, prev.RfidId, prevDir)
{
Speed = SpeedLevel.M,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, prevDir));
}
}
else
{
retval.DetailedPath.Add(new NodeMotorInfo(1, target.Id, target.RfidId, prevDir));
var nextNode = start.ConnectedMapNodes.Where(t => t.Id != prev.Id && t.StationType == StationType.Buffer).FirstOrDefault();
retval.Path.Add(nextNode);
var lastDefailt = retval.DetailedPath.Last();
retval.DetailedPath.Add(new NodeMotorInfo(lastDefailt.seq + 1, nextNode.Id, nextNode.RfidId, AgvDirection.Backward)
{
Speed = SpeedLevel.L,
});
retval.Path.Add(target);
retval.DetailedPath.Add(new NodeMotorInfo(retval.DetailedPath.Max(t => t.seq) + 1, target.Id, target.RfidId, AgvDirection.Backward));
}
return retval;
}
else if (isTargetLeft)
{
return this.FindBasicPath(start, target, prev, AgvDirection.Backward);
}
else
{
// Overshoot
var path1 = this.FindBasicPath(start, target, prev, AgvDirection.Forward);
if (path1.Path.Count < 2) return AGVPathResult.CreateFailure("Overshoot 경로 생성 실패");
var last = path1.Path.Last();
var lastD = path1.DetailedPath.Last();
path1.Path.RemoveAt(path1.Path.Count - 1);
path1.DetailedPath.RemoveAt(path1.DetailedPath.Count - 1);
path1.Path.Add(last);
path1.DetailedPath.Add(new NodeMotorInfo(lastD.seq + 1, lastD.NodeId, lastD.RfidId, AgvDirection.Backward)
{
Speed = SpeedLevel.L,
});
return path1;
}
}
}
private AGVPathResult GetPathFromGateway(MapNode GTNode, MapNode targetNode, MapNode PrevNode, AgvDirection PrevDirection)
{
AGVPathResult resultPath = null;
var deltaX = GTNode.Position.X - PrevNode.Position.X;
var isMonitorLeft = false;
if (deltaX > 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
if (targetNode.StationType == StationType.Loader || targetNode.StationType == StationType.Charger2)
{
deltaX = GTNode.Position.Y - PrevNode.Position.Y;
if (deltaX < 0) isMonitorLeft = PrevDirection == AgvDirection.Backward;
else isMonitorLeft = PrevDirection == AgvDirection.Forward;
}
switch (targetNode.StationType)
{
case StationType.Loader:
case StationType.Charger2:
case StationType.Charger1:
case StationType.UnLoader:
case StationType.Clearner:
case StationType.Buffer:
var rlt1 = new AGVPathResult();
rlt1.Success = true;
var motdir = targetNode.DockDirection == DockingDirection.Backward ? AgvDirection.Backward : AgvDirection.Forward;
var pathtarget = this.FindBasicPath(GTNode, targetNode, PrevNode, motdir);
if ((targetNode.DockDirection == DockingDirection.Backward && isMonitorLeft) ||
(targetNode.DockDirection == DockingDirection.Forward && !isMonitorLeft))
{
var turnPatterns = GetTurnaroundPattern(GTNode, targetNode);
if (turnPatterns == null || !turnPatterns.Any()) return new AGVPathResult { Success = false, Message = $"회차 패턴 없음: Dir {PrevDirection}" };
foreach (var item in turnPatterns)
{
var rfidvalue = ushort.Parse(item.Substring(0, 4));
var node = _mapNodes.FirstOrDefault(t => t.RfidId == rfidvalue);
rlt1.Path.Add(node);
AgvDirection nodedir = item.Substring(4, 1) == "F" ? AgvDirection.Forward : AgvDirection.Backward;
MagnetDirection magnet = MagnetDirection.Straight;
var magchar = item.Substring(5, 1);
if (magchar == "L") magnet = MagnetDirection.Left;
else if (magchar == "R") magnet = MagnetDirection.Right;
rlt1.DetailedPath.Add(new NodeMotorInfo(rlt1.DetailedPath.Count, node.Id, node.RfidId, nodedir, null, magnet)
{
Speed = SpeedLevel.L,
});
}
if (pathtarget.DetailedPath.First().NodeId != rlt1.DetailedPath.Last().NodeId ||
pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection)
{
// Gateway 턴 마지막 주소 불일치 경고 (로깅 등)
}
pathtarget.Path.RemoveAt(0);
pathtarget.DetailedPath.RemoveAt(0);
}
return CombinePaths(rlt1, pathtarget);
default:
return AGVPathResult.CreateFailure($"지원되지 않는 StationType: {targetNode.StationType}");
}
}
private MapNode GetGatewayNode(MapNode node)
{
var rfid = 0;
if (node.StationType == StationType.UnLoader) rfid = 10;
else if (node.StationType == StationType.Charger1) rfid = 9;
else if (node.StationType == StationType.Clearner) rfid = 6;
else if (node.StationType == StationType.Charger2) rfid = 13;
else if (node.StationType == StationType.Loader) rfid = 13;
else if (node.StationType == StationType.Buffer) rfid = 6;
if (rfid == 0) return null;
return _mapNodes.FirstOrDefault(t => t.RfidId == rfid);
}
private List<string> GetTurnaroundPattern(MapNode gatewayNode, MapNode targetNode)
{
switch (gatewayNode.RfidId)
{
case 6:
if (targetNode.StationType == StationType.Buffer)
return new List<string> { "0006BL", "0007FS", "0013BL", "0006BL" };
else
return new List<string> { "0006BL", "0007FS", "0013BL", "0006BS" };
case 9: return new List<string> { "0009FL", "0010BS", "0007FL", "0009FS" };
case 10: return new List<string> { "0010BR", "0009FR", "0007BS", "0010BS" };
case 13: return new List<string> { "0013BL", "0006FL", "0007BS", "0013BS" };
default: return null;
}
}
private AGVPathResult CombinePaths(AGVPathResult p1, AGVPathResult p2)
{
var res = new AGVPathResult();
res.Success = true;
var p1last = p1.DetailedPath.LastOrDefault();
var p2fist = p2.DetailedPath.FirstOrDefault();
if (p1last != null && p2fist != null &&
(p1last.NodeId == p2fist.NodeId && p1last.MotorDirection == p2fist.MotorDirection && p1last.MagnetDirection == p2fist.MagnetDirection))
{
p1.Path.RemoveAt(p1.Path.Count - 1);
p1.DetailedPath.RemoveAt(p1.DetailedPath.Count - 1);
}
foreach (var item in p1.Path) res.Path.Add(item);
foreach (var item in p2.Path) res.Path.Add(item);
foreach (var item in p1.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
foreach (var item in p2.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
return res;
}
}
}

View File

@@ -0,0 +1,71 @@
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<MapNode> DirectionChangePath { get; set; }
public string DirectionChangeNode { get; set; }
public string ErrorMessage { get; set; }
public string PlanDescription { get; set; }
public DirectionChangePlan()
{
DirectionChangePath = new List<MapNode>();
ErrorMessage = string.Empty;
PlanDescription = string.Empty;
}
public static DirectionChangePlan CreateSuccess(List<MapNode> 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);
}
}
}

View File

@@ -0,0 +1,329 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.PathFinding.Planning
{
/// <summary>
/// 방향 기반 경로 탐색기
/// 이전 위치 + 현재 위치 + 이동 방향을 기반으로 다음 노드를 결정
/// </summary>
public class DirectionalPathfinder
{
/// <summary>
/// 이동 방향별 가중치
/// </summary>
public class DirectionWeights
{
public float ForwardWeight { get; set; } = 1.0f; // 직진
public float LeftWeight { get; set; } = 1.5f; // 좌측
public float RightWeight { get; set; } = 1.5f; // 우측
public float BackwardWeight { get; set; } = 2.0f; // 후진
}
private readonly DirectionWeights _weights;
public DirectionalPathfinder(DirectionWeights weights = null)
{
_weights = weights ?? new DirectionWeights();
}
/// <summary>
/// 이전 위치와 현재 위치, 그리고 이동 방향을 기반으로 다음 노드 ID를 반환
/// </summary>
/// <param name="previousPos">이전 위치 (이전 RFID 감지 위치)</param>
/// <param name="currentNode">현재 노드 (현재 RFID 노드)</param>
/// <param name="currentPos">현재 위치</param>
/// <param name="direction">이동 방향 (Forward/Backward/Left/Right)</param>
/// <param name="allNodes">맵의 모든 노드</param>
/// <returns>다음 노드 ID (또는 null)</returns>
public string GetNextNodeId(
Point previousPos,
MapNode currentNode,
Point currentPos,
AgvDirection direction,
List<MapNode> allNodes)
{
// 전제조건: 최소 2개 위치 히스토리 필요
if (previousPos == Point.Empty || currentPos == Point.Empty)
{
return null;
}
if (currentNode == null || allNodes == null || allNodes.Count == 0)
{
return null;
}
// 현재 노드에 연결된 노드들 가져오기
var connectedNodeIds = currentNode.ConnectedNodes;
if (connectedNodeIds == null || connectedNodeIds.Count == 0)
{
return null;
}
// 연결된 노드 중 현재 노드가 아닌 것들만 필터링
var candidateNodes = allNodes.Where(n =>
connectedNodeIds.Contains(n.Id) && n.Id != currentNode.Id
).ToList();
if (candidateNodes.Count == 0)
{
return null;
}
// 이전→현재 벡터 계산 (진행 방향 벡터)
var movementVector = new PointF(
currentPos.X - previousPos.X,
currentPos.Y - previousPos.Y
);
// 벡터 정규화
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
if (movementLength < 0.001f) // 거의 이동하지 않음
{
return candidateNodes[0].Id; // 첫 번째 연결 노드 반환
}
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 각 후보 노드에 대해 방향 점수 계산
var scoredCandidates = new List<(MapNode node, float score)>();
foreach (var candidate in candidateNodes)
{
var toNextVector = new PointF(
candidate.Position.X - currentPos.X,
candidate.Position.Y - currentPos.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
if (toNextLength < 0.001f)
{
continue;
}
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 진행 방향 기반 점수 계산
float score = CalculateDirectionalScore(
normalizedMovement,
normalizedToNext,
direction
);
scoredCandidates.Add((candidate, score));
}
if (scoredCandidates.Count == 0)
{
return null;
}
// 가장 높은 점수를 가진 노드 반환
var bestCandidate = scoredCandidates.OrderByDescending(x => x.score).First();
return bestCandidate.node.Id;
}
/// <summary>
/// 이동 방향을 기반으로 방향 점수를 계산
/// 높은 점수 = 더 나은 선택지
/// </summary>
private float CalculateDirectionalScore(
PointF movementDirection, // 정규화된 이전→현재 벡터
PointF nextDirection, // 정규화된 현재→다음 벡터
AgvDirection requestedDir) // 요청된 이동 방향
{
float baseScore = 0;
// 벡터 간 각도 계산 (내적)
float dotProduct = (movementDirection.X * nextDirection.X) +
(movementDirection.Y * nextDirection.Y);
// 외적으로 좌우 판별 (Z 성분)
float crossProduct = (movementDirection.X * nextDirection.Y) -
(movementDirection.Y * nextDirection.X);
switch (requestedDir)
{
case AgvDirection.Forward:
// Forward: 직진 방향 선호 (dotProduct ≈ 1)
if (dotProduct > 0.9f) // 거의 같은 방향
{
baseScore = 100.0f * _weights.ForwardWeight;
}
else if (dotProduct > 0.5f) // 비슷한 방향
{
baseScore = 80.0f * _weights.ForwardWeight;
}
else if (dotProduct > 0.0f) // 약간 다른 방향
{
baseScore = 50.0f * _weights.ForwardWeight;
}
else if (dotProduct > -0.5f) // 거의 반대 방향 아님
{
baseScore = 20.0f * _weights.BackwardWeight;
}
else
{
baseScore = 0.0f; // 완전 반대
}
break;
case AgvDirection.Backward:
// Backward: 역진 방향 선호 (dotProduct ≈ -1)
if (dotProduct < -0.9f) // 거의 반대 방향
{
baseScore = 100.0f * _weights.BackwardWeight;
}
else if (dotProduct < -0.5f) // 비슷하게 반대
{
baseScore = 80.0f * _weights.BackwardWeight;
}
else if (dotProduct < 0.0f) // 약간 다른 방향
{
baseScore = 50.0f * _weights.BackwardWeight;
}
else if (dotProduct < 0.5f) // 거의 같은 방향 아님
{
baseScore = 20.0f * _weights.ForwardWeight;
}
else
{
baseScore = 0.0f; // 완전 같은 방향
}
break;
case AgvDirection.Left:
// Left: 좌측 방향 선호
// Forward 상태에서: crossProduct > 0 = 좌측
// Backward 상태에서: crossProduct < 0 = 좌측 (반대)
if (dotProduct > 0.0f) // Forward 상태
{
// crossProduct > 0이면 좌측
if (crossProduct > 0.5f)
{
baseScore = 100.0f * _weights.LeftWeight;
}
else if (crossProduct > 0.0f)
{
baseScore = 70.0f * _weights.LeftWeight;
}
else if (crossProduct > -0.5f)
{
baseScore = 50.0f * _weights.ForwardWeight;
}
else
{
baseScore = 30.0f * _weights.RightWeight;
}
}
else // Backward 상태 - 좌우 반전
{
// Backward에서 좌측 = crossProduct < 0
if (crossProduct < -0.5f)
{
baseScore = 100.0f * _weights.LeftWeight;
}
else if (crossProduct < 0.0f)
{
baseScore = 70.0f * _weights.LeftWeight;
}
else if (crossProduct < 0.5f)
{
baseScore = 50.0f * _weights.BackwardWeight;
}
else
{
baseScore = 30.0f * _weights.RightWeight;
}
}
break;
case AgvDirection.Right:
// Right: 우측 방향 선호
// Forward 상태에서: crossProduct < 0 = 우측
// Backward 상태에서: crossProduct > 0 = 우측 (반대)
if (dotProduct > 0.0f) // Forward 상태
{
// crossProduct < 0이면 우측
if (crossProduct < -0.5f)
{
baseScore = 100.0f * _weights.RightWeight;
}
else if (crossProduct < 0.0f)
{
baseScore = 70.0f * _weights.RightWeight;
}
else if (crossProduct < 0.5f)
{
baseScore = 50.0f * _weights.ForwardWeight;
}
else
{
baseScore = 30.0f * _weights.LeftWeight;
}
}
else // Backward 상태 - 좌우 반전
{
// Backward에서 우측 = crossProduct > 0
if (crossProduct > 0.5f)
{
baseScore = 100.0f * _weights.RightWeight;
}
else if (crossProduct > 0.0f)
{
baseScore = 70.0f * _weights.RightWeight;
}
else if (crossProduct > -0.5f)
{
baseScore = 50.0f * _weights.BackwardWeight;
}
else
{
baseScore = 30.0f * _weights.LeftWeight;
}
}
break;
}
return baseScore;
}
/// <summary>
/// 벡터 간 각도를 도 단위로 계산
/// </summary>
private float CalculateAngle(PointF vector1, PointF vector2)
{
float dotProduct = (vector1.X * vector2.X) + (vector1.Y * vector2.Y);
float magnitude1 = (float)Math.Sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y);
float magnitude2 = (float)Math.Sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y);
if (magnitude1 < 0.001f || magnitude2 < 0.001f)
{
return 0;
}
float cosAngle = dotProduct / (magnitude1 * magnitude2);
cosAngle = Math.Max(-1.0f, Math.Min(1.0f, cosAngle)); // 범위 제한
return (float)(Math.Acos(cosAngle) * 180.0 / Math.PI);
}
}
}

View File

@@ -0,0 +1,131 @@
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>
/// 일련번호
/// </summary>
public int seq { get; set; }
/// <summary>
/// 노드 ID
/// </summary>
public string NodeId { get; set; }
/// <summary>
/// RFID Value
/// </summary>
public ushort RfidId { get; set; }
/// <summary>
/// 해당 노드에서의 모터방향
/// </summary>
public AgvDirection MotorDirection { get; set; }
/// <summary>
/// 해당 노드에서의 제한 속도
/// </summary>
public SpeedLevel Speed { get; set; } = SpeedLevel.M;
/// <summary>
/// 마그넷 센서 방향 제어 (갈림길 처리용)
/// </summary>
public MagnetDirection MagnetDirection { get; set; }
/// <summary>
/// 다음 노드 ID (경로예측용)
/// </summary>
public MapNode NextNode { 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 bool IsPass { get; set; }
/// <summary>
/// 특수 동작 설명
/// </summary>
public string SpecialActionDescription { get; set; }
public NodeMotorInfo(int seqno,string nodeId,ushort rfid, AgvDirection motorDirection, MapNode nextNodeId = null, MagnetDirection magnetDirection = MagnetDirection.Straight)
{
seq = seqno;
NodeId = nodeId;
RfidId = rfid;
MotorDirection = motorDirection;
MagnetDirection = magnetDirection;
NextNode = nextNodeId;
CanRotate = false;
IsDirectionChangePoint = false;
RequiresSpecialAction = false;
SpecialActionDescription = string.Empty;
IsPass = false;
}
/// <summary>
/// 디버깅용 문자열 표현
/// </summary>
public override string ToString()
{
var result = $"R{RfidId}[*{NodeId}]:{MotorDirection}";
// 마그넷 방향이 직진이 아닌 경우 표시
if (MagnetDirection != MagnetDirection.Straight)
result += $"({MagnetDirection})";
if (IsDirectionChangePoint)
result += " [방향전환]";
if (CanRotate)
result += " [회전가능]";
if (RequiresSpecialAction)
result += $" [특수동작:{SpecialActionDescription}]";
if (IsPass) result += "(O)";
return result;
}
}
}

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
};
}
}
}

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})";
}
}
}

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")]

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월 업데이트

View File

@@ -0,0 +1,125 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// AGV 방향 기반 다음 노드 계산기
/// VirtualAGV 또는 실제 AGV 시스템에서 현재 방향을 알 때, 다음 목적지 노드를 결정
/// </summary>
public class AGVDirectionCalculator
{
private DirectionalPathfinder _pathfinder;
public AGVDirectionCalculator(DirectionalPathfinder.DirectionWeights weights = null)
{
_pathfinder = new DirectionalPathfinder(weights);
}
/// <summary>
/// 이전 RFID 위치 + 현재 위치 + 현재 방향을 기반으로 다음 노드 ID를 반환
///
/// 사용 예시:
/// - 001에서 002로 이동 후 GetNextNodeId(001_pos, 002_node, 002_pos, Forward) → 003
/// - 003에서 004로 이동 후, Left 선택 → 030
/// - 004에서 003으로 이동(Backward) 후, GetNextNodeId(..., Backward) → 002
/// </summary>
/// <param name="previousRfidPos">이전 RFID 감지 위치</param>
/// <param name="currentNode">현재 RFID 노드</param>
/// <param name="currentRfidPos">현재 RFID 감지 위치</param>
/// <param name="direction">이동 방향</param>
/// <param name="allNodes">맵의 모든 노드</param>
/// <returns>다음 노드 ID (실패 시 null)</returns>
public string GetNextNodeId(
Point previousRfidPos,
MapNode currentNode,
Point currentRfidPos,
AgvDirection direction,
List<MapNode> allNodes)
{
// 유효성 검사
if (previousRfidPos == Point.Empty)
{
throw new ArgumentException("previousRfidPos는 빈 값일 수 없습니다. 최소 2개의 위치 히스토리가 필요합니다.");
}
if (currentNode == null)
{
throw new ArgumentNullException(nameof(currentNode), "currentNode는 null일 수 없습니다.");
}
if (allNodes == null || allNodes.Count == 0)
{
throw new ArgumentException("allNodes는 비어있을 수 없습니다.");
}
return _pathfinder.GetNextNodeId(
previousRfidPos,
currentNode,
currentRfidPos,
direction,
allNodes
);
}
/// <summary>
/// 현재 모터 상태를 기반으로 실제 선택된 방향을 분석
/// VirtualAGV의 현재/이전 상태로부터 선택된 방향을 역추적
/// </summary>
public AgvDirection AnalyzeSelectedDirection(
Point previousPos,
Point currentPos,
MapNode selectedNextNode,
List<MapNode> connectedNodes)
{
if (previousPos == Point.Empty || currentPos == Point.Empty || selectedNextNode == null)
{
return AgvDirection.Forward;
}
// 이동 벡터
var movementVector = new PointF(
currentPos.X - previousPos.X,
currentPos.Y - previousPos.Y
);
// 다음 노드 벡터
var nextVector = new PointF(
selectedNextNode.Position.X - currentPos.X,
selectedNextNode.Position.Y - currentPos.Y
);
// 내적 계산 (유사도)
float dotProduct = (movementVector.X * nextVector.X) +
(movementVector.Y * nextVector.Y);
// 외적 계산 (좌우 판별)
float crossProduct = (movementVector.X * nextVector.Y) -
(movementVector.Y * nextVector.X);
// 진행 방향 판별
if (dotProduct > 0) // 같은 방향으로 진행
{
if (Math.Abs(crossProduct) < 0.1f) // 거의 직진
{
return AgvDirection.Forward;
}
else if (crossProduct > 0) // 좌측으로 회전
{
return AgvDirection.Left;
}
else // 우측으로 회전
{
return AgvDirection.Right;
}
}
else // 반대 방향으로 진행 (후진)
{
return AgvDirection.Backward;
}
}
}
}

View File

@@ -0,0 +1,464 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Analysis;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// AGV 방향 계산 헬퍼 유틸리티
/// 현재 위치에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 계산
/// 이전 이동 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
/// </summary>
public static class DirectionalHelper
{
/// <summary>
/// AGV방향과 일치하는지 확인한다. 단 원본위치에서 dock 위치가 Don't Care 라면 true가 반환 됩니다.
/// </summary>
/// <param name="dock"></param>
/// <param name="agvdirection"></param>
/// <returns></returns>
public static bool MatchAGVDirection(this DockingDirection dock, AgvDirection agvdirection)
{
if (dock == DockingDirection.DontCare) return true;
if (dock == DockingDirection.Forward && agvdirection == AgvDirection.Forward) return true;
if (dock == DockingDirection.Backward && agvdirection == AgvDirection.Backward) return true;
return false;
}
private static JunctionAnalyzer _junctionAnalyzer;
/// <summary>
/// JunctionAnalyzer 초기화 (첫 호출 시)
/// </summary>
private static void InitializeJunctionAnalyzer(List<MapNode> allNodes)
{
if (_junctionAnalyzer == null && allNodes != null)
{
_junctionAnalyzer = new JunctionAnalyzer(allNodes);
}
}
/// <summary>
/// 현재 노드에서 주어진 모터 방향과 마그넷 방향으로 이동할 때 다음 노드를 반환
/// 이전 모터 방향과 마그넷 방향을 고려하여 더 정확한 경로 예측
/// </summary>
/// <param name="currentNode">현재 노드</param>
/// <param name="prevNode">이전 노드 (진행 방향 기준점)</param>
/// <param name="prevDirection">이전 구간의 모터 방향</param>
/// <param name="direction">현재 모터 방향 (Forward 또는 Backward)</param>
/// <param name="magnetDirection">현재 마그넷 방향 (Straight/Left/Right)</param>
/// <param name="allNodes">모든 맵 노드</param>
/// <returns>다음 노드 (또는 null)</returns>
public static MapNode GetNextNodeByDirection(
MapNode currentNode,
MapNode prevNode,
AgvDirection prevDirection,
AgvDirection direction,
MagnetDirection magnetDirection,
List<MapNode> allNodes)
{
if (currentNode == null || prevNode == null || allNodes == null)
return null;
// JunctionAnalyzer 초기화
InitializeJunctionAnalyzer(allNodes);
// 현재 노드에 연결된 노드들 중 이전 노드가 아닌 노드들만 필터링
var connectedMapNodes = currentNode.ConnectedMapNodes;
if (connectedMapNodes == null || connectedMapNodes.Count == 0)
return null;
List<MapNode> candidateNodes = new List<MapNode>();
if (prevDirection == direction)
{
candidateNodes = connectedMapNodes.Where(n => n.Id != prevNode.Id).ToList();
}
else
{
candidateNodes = connectedMapNodes.ToList();
}
if (candidateNodes.Count == 0)
return null;
// 이전→현재 이동 벡터
var movementVector = new PointF(
currentNode.Position.X - prevNode.Position.X,
currentNode.Position.Y - prevNode.Position.Y
);
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
if (movementLength < 0.001f)
return candidateNodes[0];
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 각 후보 노드에 대해 점수 계산
MapNode bestNode = null;
float bestScore = float.MinValue;
Console.WriteLine(
$"\n[GetNextNodeByDirection] ========== 다음 노드 선택 시작 ==========");
Console.WriteLine(
$" 현재노드: {currentNode.RfidId}[{currentNode.Id}]({currentNode.Position.X:F1}, {currentNode.Position.Y:F1})");
Console.WriteLine(
$" 이전노드: {prevNode.RfidId}[{prevNode.Id}]({prevNode.Position.X:F1}, {prevNode.Position.Y:F1})");
Console.WriteLine(
$" 이동벡터: ({movementVector.X:F2}, {movementVector.Y:F2}) → 정규화: ({normalizedMovement.X:F3}, {normalizedMovement.Y:F3})");
Console.WriteLine(
$" 현재방향: {direction}, 이전방향: {prevDirection}, 마그넷방향: {magnetDirection}");
Console.WriteLine(
$" 후보노드 개수: {candidateNodes.Count}");
foreach (var candidate in candidateNodes)
{
var toNextVector = new PointF(
candidate.Position.X - currentNode.Position.X,
candidate.Position.Y - currentNode.Position.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
if (toNextLength < 0.001f)
continue;
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 내적 계산 (유사도: -1 ~ 1)
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float score;
if (direction == prevDirection)
{
// Forward: 진행 방향과 유사한 방향 선택 (높은 내적 = 좋음)
score = dotProduct;
}
else // Backward
{
// Backward: 진행 방향과 반대인 방향 선택 (낮은 내적 = 좋음)
score = -dotProduct;
}
Console.WriteLine(
$"\n [후보] {candidate.RfidId}[{candidate.Id}]({candidate.Position.X:F1}, {candidate.Position.Y:F1})");
Console.WriteLine(
$" 벡터: ({toNextVector.X:F2}, {toNextVector.Y:F2}), 길이: {toNextLength:F2}");
Console.WriteLine(
$" 정규화벡터: ({normalizedToNext.X:F3}, {normalizedToNext.Y:F3})");
Console.WriteLine(
$" 내적(dotProduct): {dotProduct:F4}");
Console.WriteLine(
$" 기본점수 ({(direction == prevDirection ? "" : "")}): {score:F4}");
// 이전 모터 방향이 제공된 경우: 방향 일관성 보너스 추가
var scoreBeforeMotor = score;
score = ApplyMotorDirectionConsistencyBonus(
score,
direction,
prevDirection,
dotProduct
);
Console.WriteLine(
$" 모터방향 적용 후: {scoreBeforeMotor:F4} → {score:F4}");
// 마그넷 방향을 고려한 점수 조정
var scoreBeforeMagnet = score;
score = ApplyMagnetDirectionBonus(
score,
magnetDirection,
normalizedMovement,
normalizedToNext,
currentNode,
candidate,
direction
);
Console.WriteLine(
$" 마그넷방향 적용 후: {scoreBeforeMagnet:F4} → {score:F4}");
if (score > bestScore)
{
bestScore = score;
bestNode = candidate;
Console.WriteLine(
$" ⭐ 현재 최고점수 선택됨!");
}
}
Console.WriteLine(
$"\n 최종선택: {bestNode?.RfidId ?? 0}[{bestNode?.Id ?? "null"}] (점수: {bestScore:F4})");
Console.WriteLine(
$"[GetNextNodeByDirection] ========== 다음 노드 선택 종료 ==========\n");
return bestNode;
}
/// <summary>
/// 모터 방향 일관성을 고려한 점수 보정
/// 같은 방향으로 계속 이동하는 경우 보너스 점수 부여
/// </summary>
/// <param name="baseScore">기본 점수</param>
/// <param name="currentDirection">현재 모터 방향</param>
/// <param name="prevMotorDirection">이전 모터 방향</param>
/// <param name="dotProduct">벡터 내적값</param>
/// <returns>조정된 점수</returns>
private static float ApplyMotorDirectionConsistencyBonus(
float baseScore,
AgvDirection currentDirection,
AgvDirection prevMotorDirection,
float dotProduct)
{
float adjustedScore = baseScore;
// 모터 방향이 변경되지 않은 경우: 일관성 보너스
if (currentDirection == prevMotorDirection)
{
// Forward 지속: 직진 방향으로의 이동 선호
// Backward 지속: 반대 방향으로의 이동 선호
const float CONSISTENCY_BONUS = 0.2f;
adjustedScore += CONSISTENCY_BONUS;
System.Diagnostics.Debug.WriteLine(
$"[DirectionalHelper] 모터 방향 일관성 보너스: {currentDirection} → {currentDirection} " +
$"(점수: {baseScore:F3} → {adjustedScore:F3})");
}
else
{
// 모터 방향이 변경된 경우: 방향 변경 페널티
const float DIRECTION_CHANGE_PENALTY = 0.15f;
adjustedScore -= DIRECTION_CHANGE_PENALTY;
System.Diagnostics.Debug.WriteLine(
$"[DirectionalHelper] 모터 방향 변경 페널티: {prevMotorDirection} → {currentDirection} " +
$"(점수: {baseScore:F3} → {adjustedScore:F3})");
}
return adjustedScore;
}
/// <summary>
/// 마그넷 방향을 고려한 점수 보정
/// Straight/Left/Right 마그넷 방향에 따라 후보 노드를 평가
/// </summary>
/// <param name="baseScore">기본 점수</param>
/// <param name="magnetDirection">마그넷 방향 (Straight/Left/Right)</param>
/// <param name="normalizedMovement">정규화된 이동 벡터</param>
/// <param name="normalizedToNext">정규화된 다음 이동 벡터</param>
/// <param name="currentNode">현재 노드</param>
/// <param name="candidate">후보 노드</param>
/// <returns>조정된 점수</returns>
private static float ApplyMagnetDirectionBonus(
float baseScore,
MagnetDirection magnetDirection,
PointF normalizedMovement,
PointF normalizedToNext,
MapNode currentNode,
MapNode candidate,
AgvDirection direction)
{
float adjustedScore = baseScore;
// Straight: 일직선 방향 (높은 내적 보너스)
if (magnetDirection == MagnetDirection.Straight)
{
const float STRAIGHT_BONUS = 0.5f;
adjustedScore += STRAIGHT_BONUS;
Console.WriteLine(
$" [마그넷 판정] Straight 보너스 +0.5: {baseScore:F4} → {adjustedScore:F4}");
}
// Left 또는 Right: 모터 위치에 따른 회전 방향 판단
else if (magnetDirection == MagnetDirection.Left || magnetDirection == MagnetDirection.Right)
{
// 2D 외적: movement × toNext = movement.X * toNext.Y - movement.Y * toNext.X
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
(normalizedMovement.Y * normalizedToNext.X);
bool isLeftMotorMatch = false;
bool isRightMotorMatch = false;
// ===== 정방향(Forward) 이동 =====
if (direction == AgvDirection.Forward)
{
// Forward 이동 시 외적 판정:
// - 외적 < 0 (음수) = 반시계 회전 = Left 모터 멈춤
// - 외적 > 0 (양수) = 시계 회전 = Right 모터 멈춤
//
// 예: 004 → 012 → 016 (Left 모터)
// 외적 = -0.9407 (음수) → 반시계 → Left 일치 ✅
isLeftMotorMatch = crossProduct < 0; // 음수 = 반시계 = Left 멈춤
isRightMotorMatch = crossProduct > 0; // 양수 = 시계 = Right 멈춤
}
// ===== 역방향(Backward) 이동 =====
else // Backward
{
// Backward 이동 시 외적 판정:
// - 외적 < 0 (음수) = 시계 회전 = Left 모터 멈춤
// - 외적 > 0 (양수) = 반시계 회전 = Right 모터 멈춤
//
// 예: 012 → 004 → 003 (Left 모터)
// 외적 = 0.9334 (양수) → 반시계(역방향 기준 시계) → Left 일치 ✅
isLeftMotorMatch = crossProduct > 0; // 양수 = 시계(역) = Left 멈춤
isRightMotorMatch = crossProduct < 0; // 음수 = 반시계(역) = Right 멈춤
}
Console.WriteLine(
$" [마그넷 판정] 외적(Cross): {crossProduct:F4}, Left모터일치: {isLeftMotorMatch}, Right모터일치: {isRightMotorMatch} [{direction}]");
// 외적의 절대값으로 회전 강도 판단 (0에 가까우면 약함, 1에 가까우면 강함)
float rotationStrength = Math.Abs(crossProduct);
if ((magnetDirection == MagnetDirection.Left && isLeftMotorMatch) ||
(magnetDirection == MagnetDirection.Right && isRightMotorMatch))
{
// 올바른 모터 방향: 회전 강도에 비례한 보너스
// 강한 회전(|외적| ≈ 1): +2.0
// 약한 회전(|외적| ≈ 0.2): +0.4
float magnetBonus = rotationStrength * 2.0f;
adjustedScore += magnetBonus;
Console.WriteLine(
$" [마그넷 판정] ✅ {magnetDirection} 모터 일치 (회전강도: {rotationStrength:F4}, 보너스 +{magnetBonus:F4}): {baseScore:F4} → {adjustedScore:F4}");
}
else
{
// 잘못된 모터 방향: 회전 강도에 비례한 페널티
// 강한 회전(|외적| ≈ 1): -2.0
// 약한 회전(|외적| ≈ 0.2): -0.4
float magnetPenalty = rotationStrength * 2.0f;
adjustedScore -= magnetPenalty;
string actualMotor = crossProduct > 0 ? "Left" : "Right";
Console.WriteLine(
$" [마그넷 판정] ❌ {magnetDirection} 모터 불일치 (실제: {actualMotor}, 회전강도: {rotationStrength:F4}, 페널티 -{magnetPenalty:F4}): {baseScore:F4} → {adjustedScore:F4}");
}
}
return adjustedScore;
}
/// <summary>
/// 모터 방향을 고려한 다음 노드 선택 (디버깅/분석용)
/// </summary>
public static (MapNode node, float score, string reason) GetNextNodeByDirectionWithDetails(
MapNode currentNode,
MapNode prevNode,
AgvDirection direction,
List<MapNode> allNodes,
AgvDirection? prevMotorDirection)
{
if (currentNode == null || prevNode == null || allNodes == null)
return (null, 0, "입력 파라미터가 null입니다");
var connectedMapNodes = currentNode.ConnectedMapNodes;
if (connectedMapNodes == null || connectedMapNodes.Count == 0)
return (null, 0, "연결된 노드가 없습니다");
var candidateNodes = connectedMapNodes.ToList();
if (candidateNodes.Count == 0)
return (null, 0, "후보 노드가 없습니다");
var movementVector = new PointF(
currentNode.Position.X - prevNode.Position.X,
currentNode.Position.Y - prevNode.Position.Y
);
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
if (movementLength < 0.001f)
return (candidateNodes[0], 1.0f, "움직임이 거의 없음");
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
MapNode bestNode = null;
float bestScore = float.MinValue;
string reason = "";
foreach (var candidate in candidateNodes)
{
var toNextVector = new PointF(
candidate.Position.X - currentNode.Position.X,
candidate.Position.Y - currentNode.Position.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
if (toNextLength < 0.001f)
continue;
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float score = (direction == AgvDirection.Forward) ? dotProduct : -dotProduct;
if (prevMotorDirection.HasValue)
{
score = ApplyMotorDirectionConsistencyBonus(
score,
direction,
prevMotorDirection.Value,
dotProduct
);
}
if (score > bestScore)
{
bestScore = score;
bestNode = candidate;
// 선택 이유 생성
if (prevMotorDirection.HasValue && direction == prevMotorDirection)
{
reason = $"모터 방향 일관성 유지 ({direction}) → {candidate.Id}";
}
else if (prevMotorDirection.HasValue)
{
reason = $"모터 방향 변경 ({prevMotorDirection} → {direction}) → {candidate.Id}";
}
else
{
reason = $"방향 기반 선택 ({direction}) → {candidate.Id}";
}
}
}
return (bestNode, bestScore, reason);
}
}
}

View File

@@ -0,0 +1,198 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
using Newtonsoft.Json;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// DirectionalPathfinder 테스트 클래스
/// NewMap.json 로드하여 방향별 다음 노드를 검증
/// </summary>
public class DirectionalPathfinderTest
{
private List<MapNode> _allNodes;
private Dictionary<ushort, MapNode> _nodesByRfidId;
private AGVDirectionCalculator _calculator;
public DirectionalPathfinderTest()
{
_nodesByRfidId = new Dictionary<ushort, MapNode>();
_calculator = new AGVDirectionCalculator();
}
/// <summary>
/// NewMap.json 파일 로드
/// </summary>
public bool LoadMapFile(string filePath)
{
try
{
if (!File.Exists(filePath))
{
Console.WriteLine($"파일을 찾을 수 없습니다: {filePath}");
return false;
}
string jsonContent = File.ReadAllText(filePath);
var mapData = JsonConvert.DeserializeObject<MapFileData>(jsonContent);
if (mapData?.Nodes == null || mapData.Nodes.Count == 0)
{
Console.WriteLine("맵 파일이 비어있습니다.");
return false;
}
_allNodes = mapData.Nodes;
// RFID ID로 인덱싱
foreach (var node in _allNodes)
{
if (node.HasRfid())
{
_nodesByRfidId[node.RfidId] = node;
}
}
Console.WriteLine($"✓ 맵 파일 로드 성공: {_allNodes.Count}개 노드 로드");
return true;
}
catch (Exception ex)
{
Console.WriteLine($"✗ 맵 파일 로드 실패: {ex.Message}");
return false;
}
}
/// <summary>
/// 테스트: RFID 번호로 노드를 찾고, 다음 노드를 계산
/// </summary>
public void TestDirectionalMovement(ushort previousRfidId, ushort currentRfidId, AgvDirection direction)
{
Console.WriteLine($"\n========================================");
Console.WriteLine($"테스트: {previousRfidId} → {currentRfidId} (방향: {direction})");
Console.WriteLine($"========================================");
// RFID ID로 노드 찾기
if (!_nodesByRfidId.TryGetValue(previousRfidId, out var previousNode))
{
Console.WriteLine($"✗ 이전 RFID를 찾을 수 없습니다: {previousRfidId}");
return;
}
if (!_nodesByRfidId.TryGetValue(currentRfidId, out var currentNode))
{
Console.WriteLine($"✗ 현재 RFID를 찾을 수 없습니다: {currentRfidId}");
return;
}
Console.WriteLine($"이전 노드: {previousNode.Id} (RFID: {previousNode.RfidId}) - 위치: {previousNode.Position}");
Console.WriteLine($"현재 노드: {currentNode.Id} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
Console.WriteLine($"이동 벡터: ({currentNode.Position.X - previousNode.Position.X}, " +
$"{currentNode.Position.Y - previousNode.Position.Y})");
// 다음 노드 계산
string nextNodeId = _calculator.GetNextNodeId(
previousNode.Position,
currentNode,
currentNode.Position,
direction,
_allNodes
);
if (string.IsNullOrEmpty(nextNodeId))
{
Console.WriteLine($"✗ 다음 노드를 찾을 수 없습니다.");
return;
}
// 다음 노드 정보 출력
var nextNode = _allNodes.FirstOrDefault(n => n.Id == nextNodeId);
if (nextNode != null)
{
Console.WriteLine($"✓ 다음 노드: {nextNode.Id} (RFID: {nextNode.RfidId}) - 위치: {nextNode.Position}");
Console.WriteLine($" ├─ 노드 타입: {GetNodeTypeName(nextNode.Type)}");
Console.WriteLine($" └─ 연결된 노드: {string.Join(", ", nextNode.ConnectedNodes)}");
}
else
{
Console.WriteLine($"✗ 다음 노드 정보를 찾을 수 없습니다: {nextNodeId}");
}
}
/// <summary>
/// 모든 노드 정보 출력
/// </summary>
public void PrintAllNodes()
{
Console.WriteLine("\n========== 모든 노드 정보 ==========");
foreach (var node in _allNodes.OrderBy(n => n.RfidId))
{
Console.WriteLine($"{node.RfidId:D3} → {node.Id} ({GetNodeTypeName(node.Type)})");
Console.WriteLine($" 위치: {node.Position}, 연결: {string.Join(", ", node.ConnectedNodes)}");
}
}
/// <summary>
/// 특정 RFID 노드의 상세 정보 출력
/// </summary>
public void PrintNodeInfo(ushort rfidId)
{
if (!_nodesByRfidId.TryGetValue(rfidId, out var node))
{
Console.WriteLine($"노드를 찾을 수 없습니다: {rfidId}");
return;
}
Console.WriteLine($"\n========== RFID {rfidId} 상세 정보 ==========");
Console.WriteLine($"노드 ID: {node.Id}");
Console.WriteLine($"RFID: {node.RfidId}");
Console.WriteLine($"ALIAS: {node.AliasName}");
Console.WriteLine($"위치: {node.Position}");
Console.WriteLine($"타입: {GetNodeTypeName(node.Type)}");
Console.WriteLine($"TurnLeft/Right/교차로 : {(node.CanTurnLeft ? "O":"X")}/{(node.CanTurnRight ? "O" : "X")}/{(node.DisableCross ? "X" : "O")}");
Console.WriteLine($"활성: {node.IsActive}");
Console.WriteLine($"연결된 노드:");
if (node.ConnectedNodes.Count == 0)
{
Console.WriteLine(" (없음)");
}
else
{
foreach (var connectedId in node.ConnectedNodes)
{
var connectedNode = _allNodes.FirstOrDefault(n => n.Id == connectedId);
if (connectedNode != null)
{
Console.WriteLine($" → {connectedId} (RFID: {connectedNode.RfidId}) - 위치: {connectedNode.Position}");
}
else
{
Console.WriteLine($" → {connectedId} (노드 찾을 수 없음)");
}
}
}
}
private string GetNodeTypeName(NodeType type)
{
return type.ToString();
}
// JSON 파일 매핑을 위한 임시 클래스
[Serializable]
private class MapFileData
{
[JsonProperty("Nodes")]
public List<MapNode> Nodes { get; set; }
[JsonProperty("RfidMappings")]
public List<dynamic> RfidMappings { get; set; }
}
}
}

View File

@@ -0,0 +1,346 @@
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)
{
// 경로가 없거나 실패한 경우
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 경로 없음");
return DockingValidationResult.CreateNotRequired();
}
if (pathResult.DetailedPath.Any() == false && pathResult.Path.Any() && pathResult.Path.Count == 2 &&
pathResult.Path[0].Id == pathResult.Path[1].Id)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 검증 불필요: 동일포인트");
return DockingValidationResult.CreateNotRequired();
}
// 목적지 노드 가져오기 (Path는 이제 List<MapNode>)
var LastNode = pathResult.Path[pathResult.Path.Count - 1];
if (LastNode == null)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드가 null입니다");
return DockingValidationResult.CreateNotRequired();
}
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {LastNode.Id} 타입:{LastNode.Type} ({(int)LastNode.Type})");
////detail 경로 이동 예측 검증
//for (int i = 0; i < pathResult.DetailedPath.Count - 1; i++)
//{
// var curNodeId = pathResult.DetailedPath[i].NodeId;
// var nextNodeId = pathResult.DetailedPath[i + 1].NodeId;
// var curNode = mapNodes?.FirstOrDefault(n => n.Id == curNodeId);
// var nextNode = mapNodes?.FirstOrDefault(n => n.Id == nextNodeId);
// if (curNode != null && nextNode != null)
// {
// MapNode prevNode = null;
// AgvDirection prevDir = AgvDirection.Stop;
// if (i == 0)
// {
// prevNode = pathResult.PrevNode;
// prevDir = pathResult.PrevDirection;
// }
// else
// {
// var prevNodeId = pathResult.DetailedPath[i - 1].NodeId;
// prevNode = mapNodes?.FirstOrDefault(n => n.Id == prevNodeId);
// prevDir = pathResult.DetailedPath[i - 1].MotorDirection;
// }
// if (prevNode != null)
// {
// // DirectionalHelper를 사용하여 예상되는 다음 노드 확인
// Console.WriteLine(
// $"\n[ValidateDockingDirection] 경로 검증 단계 {i}:");
// Console.WriteLine(
// $" 이전→현재→다음: {prevNode.Id}({prevNode.RfidId}) → {curNode.Id}({curNode.RfidId}) → {nextNode.Id}({nextNode.RfidId})");
// Console.WriteLine(
// $" 현재 노드 위치: ({curNode.Position.X:F1}, {curNode.Position.Y:F1})");
// Console.WriteLine(
// $" 이전 모터방향: {prevDir}, 현재 모터방향: {pathResult.DetailedPath[i].MotorDirection}");
// Console.WriteLine(
// $" 마그넷방향: {pathResult.DetailedPath[i].MagnetDirection}");
// var expectedNextNode = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// pathResult.DetailedPath[i].MagnetDirection,
// mapNodes
// );
// var expectedNextNodeL = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// PathFinding.Planning.MagnetDirection.Left,
// mapNodes
// );
// var expectedNextNodeR = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// PathFinding.Planning.MagnetDirection.Right,
// mapNodes
// );
// var expectedNextNodeS = DirectionalHelper.GetNextNodeByDirection(
// curNode,
// prevNode,
// prevDir,
// pathResult.DetailedPath[i].MotorDirection,
// PathFinding.Planning.MagnetDirection.Straight,
// mapNodes
// );
// Console.WriteLine(
// $" [예상] GetNextNodeByDirection 결과: {expectedNextNode?.Id ?? "null"}");
// Console.WriteLine(
// $" [실제] DetailedPath 다음 노드: {nextNode.RfidId}[{nextNode.Id}]");
// if (expectedNextNode != null && !expectedNextNode.Id.Equals(nextNode.Id))
// {
// string error =
// $"[DockingValidator] ⚠️ 경로 방향 불일치" +
// $"\n현재={curNode.RfidId}[{curNodeId}] 이전={prevNode.RfidId}[{(prevNode?.Id ?? string.Empty)}] " +
// $"\n예상다음={expectedNextNode.RfidId}[{expectedNextNode.Id}] 실제다음={nextNode.RfidId}[{nextNodeId}]";
// Console.WriteLine(
// $"[ValidateDockingDirection] ❌ 경로 방향 불일치 검출!");
// Console.WriteLine(
// $" 이동 벡터:");
// Console.WriteLine(
// $" 이전→현재: ({(curNode.Position.X - prevNode.Position.X):F2}, {(curNode.Position.Y - prevNode.Position.Y):F2})");
// Console.WriteLine(
// $" 현재→예상: ({(expectedNextNode.Position.X - curNode.Position.X):F2}, {(expectedNextNode.Position.Y - curNode.Position.Y):F2})");
// Console.WriteLine(
// $" 현재→실제: ({(nextNode.Position.X - curNode.Position.X):F2}, {(nextNode.Position.Y - curNode.Position.Y):F2})");
// Console.WriteLine($"[ValidateDockingDirection] 에러메시지: {error}");
// return DockingValidationResult.CreateInvalid(
// LastNode.Id,
// LastNode.Type,
// pathResult.DetailedPath[i].MotorDirection,
// pathResult.DetailedPath[i].MotorDirection,
// error);
// }
// else
// {
// Console.WriteLine(
// $" ✅ 경로 방향 일치!");
// }
// }
// }
//}
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
if (LastNode.DockDirection == DockingDirection.DontCare)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {LastNode.DockDirection}");
return DockingValidationResult.CreateNotRequired();
}
// 필요한 도킹 방향 확인
var requiredDirection = GetRequiredDockingDirection(LastNode.DockDirection);
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 필요한 도킹 방향: {requiredDirection}");
var LastNodeInfo = pathResult.DetailedPath.Last();
if (LastNodeInfo.NodeId != LastNode.Id)
{
string error = $"마지막 노드의 도킹방향과 경로정보의 노드ID 불일치: 필요={LastNode.Id}, 계산됨={LastNodeInfo.NodeId }";
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
return DockingValidationResult.CreateInvalid(
LastNode.Id,
LastNode.Type,
requiredDirection,
LastNodeInfo.MotorDirection,
error);
}
// 검증 수행
if (LastNodeInfo.MotorDirection == requiredDirection && pathResult.DetailedPath[pathResult.DetailedPath.Count - 1].MotorDirection == requiredDirection)
{
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ✅ 도킹 검증 성공");
return DockingValidationResult.CreateValid(
LastNode.Id,
LastNode.Type,
requiredDirection,
LastNodeInfo.MotorDirection);
}
else
{
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(LastNodeInfo.MotorDirection)}";
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
return DockingValidationResult.CreateInvalid(
LastNode.Id,
LastNode.Type,
requiredDirection,
LastNodeInfo.MotorDirection,
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<MapNode> 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;
}
// 목적지 노드 확인 (Path는 이제 List<MapNode>)
var lastNode = path[path.Count - 1];
if (lastNode == null)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 목적지 노드가 null입니다");
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 secondLastNode = path[path.Count - 2];
if (secondLastNode == null)
{
System.Diagnostics.Debug.WriteLine($"[CalculateFinalDirection] 이전 노드가 null입니다");
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] 마지막 구간: {secondLastNode.Id} → {lastNode.Id}, 벡터: ({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}";
}
}
}
}

View File

@@ -0,0 +1,342 @@
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using AGVNavigationCore.Models;
using AGVNavigationCore.PathFinding.Planning;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// GetNextNodeId() 메서드의 동작을 검증하는 테스트 클래스
///
/// 테스트 시나리오:
/// - 001(65,229) → 002(206,244) → Forward → 003이 나와야 함
/// - 001(65,229) → 002(206,244) → Backward → 001이 나와야 함
/// - 002(206,244) → 003(278,278) → Forward → 004가 나와야 함
/// - 002(206,244) → 003(278,278) → Backward → 002가 나와야 함
/// </summary>
public class GetNextNodeIdTest
{
/// <summary>
/// 가상의 VirtualAGV 상태를 시뮬레이션하여 GetNextNodeId 테스트
/// </summary>
public void TestGetNextNodeId()
{
Console.WriteLine("\n================================================");
Console.WriteLine("GetNextNodeId() 동작 검증");
Console.WriteLine("================================================\n");
// 테스트 노드 생성
var node001 = new MapNode { Id = "N001", RfidId = 001, Position = new Point(65, 229), ConnectedNodes = new List<string> { "N002" } };
var node002 = new MapNode { Id = "N002", RfidId = 002, Position = new Point(206, 244), ConnectedNodes = new List<string> { "N001", "N003" } };
var node003 = new MapNode { Id = "N003", RfidId = 003, Position = new Point(278, 278), ConnectedNodes = new List<string> { "N002", "N004" } };
var node004 = new MapNode { Id = "N004", RfidId = 004, Position = new Point(380, 340), ConnectedNodes = new List<string> { "N003", "N022", "N031" } };
var allNodes = new List<MapNode> { node001, node002, node003, node004 };
// VirtualAGV 시뮬레이션 (실제 인스턴스 생성 불가하므로 로직만 재현)
Console.WriteLine("테스트 시나리오 1: 001 → 002 → Forward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Forward 이동: 001에서 002로, 다음은 Forward",
node001.Position, node002, node003,
AgvDirection.Forward, allNodes,
"003 (예상)"
);
Console.WriteLine("\n테스트 시나리오 2: 001 → 002 → Backward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Backward 이동: 001에서 002로, 다음은 Backward",
node001.Position, node002, node001,
AgvDirection.Backward, allNodes,
"001 (예상)"
);
Console.WriteLine("\n테스트 시나리오 3: 002 → 003 → Forward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Forward 이동: 002에서 003으로, 다음은 Forward",
node002.Position, node003, node004,
AgvDirection.Forward, allNodes,
"004 (예상)"
);
Console.WriteLine("\n테스트 시나리오 4: 002 → 003 Forward → Backward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Forward 이동: 002에서 003으로, 다음은 Backward (경로 반대)",
node002.Position, node003, node002,
AgvDirection.Backward, allNodes,
"002 (예상 - 경로 반대)"
);
Console.WriteLine("\n테스트 시나리오 5: 002 → 003 Backward → Forward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Backward 이동: 002에서 003으로, 다음은 Forward (경로 반대)",
node002.Position, node003, node002,
AgvDirection.Forward, allNodes,
"002 (예상 - 경로 반대)",
AgvDirection.Backward // 현재 모터 방향
);
Console.WriteLine("\n테스트 시나리오 6: 002 → 003 Backward → Backward");
Console.WriteLine("─────────────────────────────────────────");
TestScenario(
"Backward 이동: 002에서 003으로, 다음은 Backward (경로 계속)",
node002.Position, node003, node004,
AgvDirection.Backward, allNodes,
"004 (예상 - 경로 계속)",
AgvDirection.Backward // 현재 모터 방향
);
Console.WriteLine("\n\n================================================");
Console.WriteLine("테스트 완료");
Console.WriteLine("================================================\n");
}
/// <summary>
/// 개별 테스트 시나리오 실행
/// </summary>
private void TestScenario(
string description,
Point prevPos,
MapNode currentNode,
MapNode expectedNextNode,
AgvDirection direction,
List<MapNode> allNodes,
string expectedNodeIdStr,
AgvDirection? currentMotorDirection = null)
{
// 현재 모터 방향이 지정되지 않으면 direction과 동일하다고 가정
AgvDirection motorDir = currentMotorDirection ?? direction;
Console.WriteLine($"설명: {description}");
Console.WriteLine($"이전 위치: {prevPos} (RFID: {allNodes.First(n => n.Position == prevPos)?.RfidId.ToString("0000") ?? "?"})");
Console.WriteLine($"현재 노드: {currentNode.Id} (RFID: {currentNode.RfidId}) - 위치: {currentNode.Position}");
Console.WriteLine($"현재 모터 방향: {motorDir}");
Console.WriteLine($"요청 방향: {direction}");
// 이동 벡터 계산
var movementVector = new PointF(
currentNode.Position.X - prevPos.X,
currentNode.Position.Y - prevPos.Y
);
Console.WriteLine($"이동 벡터: ({movementVector.X}, {movementVector.Y})");
// 각 후보 노드에 대한 점수 계산
Console.WriteLine($"\n현재 노드({currentNode.Id})의 ConnectedNodes: {string.Join(", ", currentNode.ConnectedNodes)}");
Console.WriteLine($"가능한 다음 노드들:");
var candidateNodes = allNodes.Where(n =>
currentNode.ConnectedNodes.Contains(n.Id) && n.Id != currentNode.Id
).ToList();
foreach (var candidate in candidateNodes)
{
var score = CalculateScoreAndPrint(movementVector, currentNode.Position, candidate, direction);
string isExpected = (candidate.Id == expectedNextNode.Id) ? " ← 예상 노드" : "";
Console.WriteLine($" {candidate.Id} (RFID: {candidate.RfidId}) - 위치: {candidate.Position} - 점수: {score:F1}{isExpected}");
}
// 최고 점수 노드 선택
var bestCandidate = GetBestCandidate(movementVector, currentNode.Position, candidateNodes, direction);
Console.WriteLine($"\n✓ 선택된 노드: {bestCandidate.Id} (RFID: {bestCandidate.RfidId})");
if (bestCandidate.Id == expectedNextNode.Id)
{
Console.WriteLine($"✅ 정답! ({expectedNodeIdStr})");
}
else
{
Console.WriteLine($"❌ 오답! 예상: {expectedNextNode.Id}, 실제: {bestCandidate.Id}");
}
}
/// <summary>
/// 점수 계산 및 상세 정보 출력
/// </summary>
private float CalculateScoreAndPrint(PointF movementVector, Point currentPos, MapNode candidate, AgvDirection direction)
{
// 벡터 정규화
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
// 다음 벡터
var toNextVector = new PointF(
candidate.Position.X - currentPos.X,
candidate.Position.Y - currentPos.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
// 내적 및 외적 계산
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
(normalizedMovement.Y * normalizedToNext.X);
float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);
return score;
}
/// <summary>
/// 점수 계산 (VirtualAGV.CalculateDirectionalScore()와 동일)
/// </summary>
private float CalculateDirectionalScore(float dotProduct, float crossProduct, AgvDirection direction)
{
float baseScore = 0;
switch (direction)
{
case AgvDirection.Forward:
if (dotProduct > 0.9f)
baseScore = 100.0f;
else if (dotProduct > 0.5f)
baseScore = 80.0f;
else if (dotProduct > 0.0f)
baseScore = 50.0f;
else if (dotProduct > -0.5f)
baseScore = 20.0f;
break;
case AgvDirection.Backward:
if (dotProduct < -0.9f)
baseScore = 100.0f;
else if (dotProduct < -0.5f)
baseScore = 80.0f;
else if (dotProduct < 0.0f)
baseScore = 50.0f;
else if (dotProduct < 0.5f)
baseScore = 20.0f;
break;
case AgvDirection.Left:
if (dotProduct > 0.0f)
{
if (crossProduct > 0.5f)
baseScore = 100.0f;
else if (crossProduct > 0.0f)
baseScore = 70.0f;
else if (crossProduct > -0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
else
{
if (crossProduct < -0.5f)
baseScore = 100.0f;
else if (crossProduct < 0.0f)
baseScore = 70.0f;
else if (crossProduct < 0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
break;
case AgvDirection.Right:
if (dotProduct > 0.0f)
{
if (crossProduct < -0.5f)
baseScore = 100.0f;
else if (crossProduct < 0.0f)
baseScore = 70.0f;
else if (crossProduct < 0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
else
{
if (crossProduct > 0.5f)
baseScore = 100.0f;
else if (crossProduct > 0.0f)
baseScore = 70.0f;
else if (crossProduct > -0.5f)
baseScore = 50.0f;
else
baseScore = 30.0f;
}
break;
}
return baseScore;
}
/// <summary>
/// 최고 점수 노드 반환
/// </summary>
private MapNode GetBestCandidate(PointF movementVector, Point currentPos, List<MapNode> candidates, AgvDirection direction)
{
var movementLength = (float)Math.Sqrt(
movementVector.X * movementVector.X +
movementVector.Y * movementVector.Y
);
var normalizedMovement = new PointF(
movementVector.X / movementLength,
movementVector.Y / movementLength
);
MapNode bestCandidate = null;
float bestScore = -1;
foreach (var candidate in candidates)
{
var toNextVector = new PointF(
candidate.Position.X - currentPos.X,
candidate.Position.Y - currentPos.Y
);
var toNextLength = (float)Math.Sqrt(
toNextVector.X * toNextVector.X +
toNextVector.Y * toNextVector.Y
);
var normalizedToNext = new PointF(
toNextVector.X / toNextLength,
toNextVector.Y / toNextLength
);
float dotProduct = (normalizedMovement.X * normalizedToNext.X) +
(normalizedMovement.Y * normalizedToNext.Y);
float crossProduct = (normalizedMovement.X * normalizedToNext.Y) -
(normalizedMovement.Y * normalizedToNext.X);
float score = CalculateDirectionalScore(dotProduct, crossProduct, direction);
if (score > bestScore)
{
bestScore = score;
bestCandidate = candidate;
}
}
return bestCandidate;
}
}
}

View File

@@ -0,0 +1,153 @@
using System;
using System.Drawing;
using System.Drawing.Imaging;
using System.IO;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// 이미지와 문자열 간 변환을 위한 유틸리티 클래스
/// Base64 인코딩을 사용하여 이미지를 문자열로 변환하거나 그 반대로 수행
/// </summary>
public static class ImageConverterUtil
{
/// <summary>
/// Image 객체를 Base64 문자열로 변환
/// </summary>
/// <param name="image">변환할 이미지</param>
/// <param name="format">이미지 포맷 (기본값: PNG)</param>
/// <returns>Base64 인코딩된 문자열, null인 경우 빈 문자열 반환</returns>
public static string ImageToBase64(Image image, ImageFormat format = null)
{
if (image == null)
return string.Empty;
try
{
format = format ?? ImageFormat.Png;
using (var memoryStream = new MemoryStream())
{
image.Save(memoryStream, format);
byte[] imageBytes = memoryStream.ToArray();
return Convert.ToBase64String(imageBytes);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"이미지 변환 실패: {ex.Message}");
return string.Empty;
}
}
/// <summary>
/// 파일 경로의 이미지를 Base64 문자열로 변환
/// </summary>
/// <param name="filePath">이미지 파일 경로</param>
/// <param name="format">변환할 포맷 (기본값: PNG, 원본 포맷 유지하려면 null)</param>
/// <returns>Base64 인코딩된 문자열</returns>
public static string FileToBase64(string filePath, ImageFormat format = null)
{
if (string.IsNullOrEmpty(filePath) || !File.Exists(filePath))
return string.Empty;
try
{
using (var image = Image.FromFile(filePath))
{
return ImageToBase64(image, format ?? ImageFormat.Png);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"파일 변환 실패: {ex.Message}");
return string.Empty;
}
}
/// <summary>
/// Base64 문자열을 Image 객체로 변환
/// </summary>
/// <param name="base64String">Base64 인코딩된 문자열</param>
/// <returns>변환된 Image 객체, 실패 시 null</returns>
public static Image Base64ToImage(string base64String)
{
if (string.IsNullOrEmpty(base64String))
return null;
try
{
byte[] imageBytes = Convert.FromBase64String(base64String);
using (var memoryStream = new MemoryStream(imageBytes))
{
return Image.FromStream(memoryStream);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"Base64 이미지 변환 실패: {ex.Message}");
return null;
}
}
/// <summary>
/// Base64 문자열을 Bitmap 객체로 변환
/// Image 대신 Bitmap을 반환하므로 메모리 관리가 더 안정적
/// </summary>
/// <param name="base64String">Base64 인코딩된 문자열</param>
/// <returns>변환된 Bitmap 객체, 실패 시 null</returns>
public static Bitmap Base64ToBitmap(string base64String)
{
if (string.IsNullOrEmpty(base64String))
return null;
try
{
byte[] imageBytes = Convert.FromBase64String(base64String);
using (var memoryStream = new MemoryStream(imageBytes))
{
return new Bitmap(memoryStream);
}
}
catch (Exception ex)
{
System.Diagnostics.Debug.WriteLine($"Base64 Bitmap 변환 실패: {ex.Message}");
return null;
}
}
/// <summary>
/// Base64 문자열이 유효한지 확인
/// </summary>
/// <param name="base64String">검증할 Base64 문자열</param>
/// <returns>유효하면 true, 그 외 false</returns>
public static bool IsValidBase64(string base64String)
{
if (string.IsNullOrWhiteSpace(base64String))
return false;
try
{
Convert.FromBase64String(base64String);
return true;
}
catch
{
return false;
}
}
/// <summary>
/// Base64 이미지 데이터의 크기를 대략적으로 계산 (바이트 단위)
/// </summary>
/// <param name="base64String">Base64 문자열</param>
/// <returns>예상 바이트 크기</returns>
public static long GetApproximateSize(string base64String)
{
if (string.IsNullOrEmpty(base64String))
return 0;
// Base64는 원본 데이터보다 약 33% 더 큼
return (long)(base64String.Length * 0.75);
}
}
}

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.Id != previousNode.Id).ToList();
}
if (nextNodes.Count == 1)
{
// 직선 경로: 다음 노드 방향으로 예측
targetPosition = nextNodes.First().Position;
calculationMethod = $"전진 경로 예측 ({currentNode.Id}→{nextNodes.First().Id})";
}
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.Id == nodeId);
if (connectedNode != null)
{
connectedNodes.Add(connectedNode);
}
}
return connectedNodes;
}
}
}

View File

@@ -0,0 +1,56 @@
using System;
using AGVNavigationCore.Models;
namespace AGVNavigationCore.Utils
{
/// <summary>
/// DirectionalPathfinder 테스트 실행 프로그램
///
/// 사용법:
/// var runner = new TestRunner();
/// runner.RunTests();
/// </summary>
public class TestRunner
{
public void RunTests()
{
string mapFilePath = @"C:\Data\Source\(5613#) ENIG AGV\Source\Cs_HMI\Data\NewMap.json";
var tester = new DirectionalPathfinderTest();
// 맵 파일 로드
if (!tester.LoadMapFile(mapFilePath))
{
Console.WriteLine("맵 파일 로드 실패!");
return;
}
// 모든 노드 정보 출력
tester.PrintAllNodes();
// 테스트 시나리오 1: 001 → 002 → Forward (003 기대)
tester.PrintNodeInfo(001);
tester.PrintNodeInfo(002);
tester.TestDirectionalMovement(001, 002, AgvDirection.Forward);
// 테스트 시나리오 2: 002 → 001 → Backward (000 또는 이전 기대)
tester.TestDirectionalMovement(002, 001, AgvDirection.Backward);
// 테스트 시나리오 3: 002 → 003 → Forward
tester.PrintNodeInfo(003);
tester.TestDirectionalMovement(002, 003, AgvDirection.Forward);
// 테스트 시나리오 4: 003 → 004 → Forward
tester.PrintNodeInfo(004);
tester.TestDirectionalMovement(003, 004, AgvDirection.Forward);
// 테스트 시나리오 5: 003 → 004 → Right (030 기대)
tester.TestDirectionalMovement(003, 004, AgvDirection.Right);
// 테스트 시나리오 6: 004 → 003 → Backward
tester.TestDirectionalMovement(004, 003, AgvDirection.Backward);
Console.WriteLine("\n\n=== 테스트 완료 ===");
}
}
}

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

View File

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