파일정리
This commit is contained in:
119
AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj
Normal file
119
AGVLogic/AGVNavigationCore/AGVNavigationCore.csproj
Normal 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>
|
||||
21
AGVLogic/AGVNavigationCore/Controls/AGVState.cs
Normal file
21
AGVLogic/AGVNavigationCore/Controls/AGVState.cs
Normal file
@@ -0,0 +1,21 @@
|
||||
namespace AGVNavigationCore.Controls
|
||||
{
|
||||
#region Interfaces
|
||||
|
||||
/// <summary>
|
||||
/// AGV 상태 열거형
|
||||
/// </summary>
|
||||
public enum AGVState
|
||||
{
|
||||
Idle, // 대기
|
||||
Moving, // 이동 중
|
||||
Rotating, // 회전 중
|
||||
Docking, // 도킹 중
|
||||
Charging, // 충전 중
|
||||
Error // 오류
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endregion
|
||||
}
|
||||
30
AGVLogic/AGVNavigationCore/Controls/IAGV.cs
Normal file
30
AGVLogic/AGVNavigationCore/Controls/IAGV.cs
Normal 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
|
||||
}
|
||||
41
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Designer.cs
generated
Normal file
41
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Designer.cs
generated
Normal 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
|
||||
}
|
||||
}
|
||||
2536
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs
Normal file
2536
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs
Normal file
File diff suppressed because it is too large
Load Diff
1189
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Mouse.cs
Normal file
1189
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Mouse.cs
Normal file
File diff suppressed because it is too large
Load Diff
979
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs
Normal file
979
AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.cs
Normal 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();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
54
AGVLogic/AGVNavigationCore/Models/AGVCommand.cs
Normal file
54
AGVLogic/AGVNavigationCore/Models/AGVCommand.cs
Normal 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})");
|
||||
}
|
||||
}
|
||||
}
|
||||
182
AGVLogic/AGVNavigationCore/Models/Enums.cs
Normal file
182
AGVLogic/AGVNavigationCore/Models/Enums.cs
Normal 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,
|
||||
|
||||
}
|
||||
}
|
||||
210
AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs
Normal file
210
AGVLogic/AGVNavigationCore/Models/IMovableAGV.cs
Normal 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
|
||||
}
|
||||
}
|
||||
88
AGVLogic/AGVNavigationCore/Models/MapImage.cs
Normal file
88
AGVLogic/AGVNavigationCore/Models/MapImage.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
42
AGVLogic/AGVNavigationCore/Models/MapLabel.cs
Normal file
42
AGVLogic/AGVNavigationCore/Models/MapLabel.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
514
AGVLogic/AGVNavigationCore/Models/MapLoader.cs
Normal file
514
AGVLogic/AGVNavigationCore/Models/MapLoader.cs
Normal 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, "", "정상");
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
72
AGVLogic/AGVNavigationCore/Models/MapMagnet.cs
Normal file
72
AGVLogic/AGVNavigationCore/Models/MapMagnet.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
37
AGVLogic/AGVNavigationCore/Models/MapMark.cs
Normal file
37
AGVLogic/AGVNavigationCore/Models/MapMark.cs
Normal 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; }
|
||||
}
|
||||
}
|
||||
185
AGVLogic/AGVNavigationCore/Models/MapNode.cs
Normal file
185
AGVLogic/AGVNavigationCore/Models/MapNode.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
60
AGVLogic/AGVNavigationCore/Models/NodeBase.cs
Normal file
60
AGVLogic/AGVNavigationCore/Models/NodeBase.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
942
AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs
Normal file
942
AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs
Normal 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
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
314
AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
Normal file
314
AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
Normal 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}";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
622
AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
Normal file
622
AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
Normal 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];
|
||||
}
|
||||
}
|
||||
}
|
||||
101
AGVLogic/AGVNavigationCore/PathFinding/Core/PathNode.cs
Normal file
101
AGVLogic/AGVNavigationCore/PathFinding/Core/PathNode.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
749
AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
Normal file
749
AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
131
AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs
Normal file
131
AGVLogic/AGVNavigationCore/PathFinding/Planning/NodeMotorInfo.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,205 @@
|
||||
using System.Collections.Generic;
|
||||
using AGVNavigationCore.Models;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding.Validation
|
||||
{
|
||||
/// <summary>
|
||||
/// 경로 검증 결과 (되돌아가기 패턴 검증 포함)
|
||||
/// </summary>
|
||||
public class PathValidationResult
|
||||
{
|
||||
/// <summary>
|
||||
/// 경로 검증이 필요한지 여부
|
||||
/// </summary>
|
||||
public bool IsValidationRequired { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 경로 검증 통과 여부
|
||||
/// </summary>
|
||||
public bool IsValid { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 검증된 경로
|
||||
/// </summary>
|
||||
public List<string> ValidatedPath { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 검출된 되돌아가기 패턴 목록 (A → B → A 형태)
|
||||
/// </summary>
|
||||
public List<BacktrackingPattern> BacktrackingPatterns { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길 노드 목록
|
||||
/// </summary>
|
||||
public List<string> JunctionNodes { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 시작 노드 ID
|
||||
/// </summary>
|
||||
public string StartNodeId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 목표 노드 ID
|
||||
/// </summary>
|
||||
public string TargetNodeId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 갈림길 노드 ID (방향 전환용)
|
||||
/// </summary>
|
||||
public string JunctionNodeId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 검증 오류 메시지 (실패시)
|
||||
/// </summary>
|
||||
public string ValidationError { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 기본 생성자
|
||||
/// </summary>
|
||||
public PathValidationResult()
|
||||
{
|
||||
IsValidationRequired = false;
|
||||
IsValid = true;
|
||||
ValidatedPath = new List<string>();
|
||||
BacktrackingPatterns = new List<BacktrackingPattern>();
|
||||
JunctionNodes = new List<string>();
|
||||
StartNodeId = string.Empty;
|
||||
TargetNodeId = string.Empty;
|
||||
JunctionNodeId = string.Empty;
|
||||
ValidationError = string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 검증 불필요한 경우 생성
|
||||
/// </summary>
|
||||
public static PathValidationResult CreateNotRequired()
|
||||
{
|
||||
return new PathValidationResult
|
||||
{
|
||||
IsValidationRequired = false,
|
||||
IsValid = true
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 검증 성공 결과 생성
|
||||
/// </summary>
|
||||
public static PathValidationResult CreateValid(List<string> path, string startNodeId, string targetNodeId, string junctionNodeId = "")
|
||||
{
|
||||
return new PathValidationResult
|
||||
{
|
||||
IsValidationRequired = true,
|
||||
IsValid = true,
|
||||
ValidatedPath = new List<string>(path),
|
||||
StartNodeId = startNodeId,
|
||||
TargetNodeId = targetNodeId,
|
||||
JunctionNodeId = junctionNodeId
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 검증 실패 결과 생성 (되돌아가기 패턴 검출)
|
||||
/// </summary>
|
||||
public static PathValidationResult CreateInvalidWithBacktracking(
|
||||
List<string> path,
|
||||
List<BacktrackingPattern> backtrackingPatterns,
|
||||
string startNodeId,
|
||||
string targetNodeId,
|
||||
string junctionNodeId,
|
||||
string error)
|
||||
{
|
||||
return new PathValidationResult
|
||||
{
|
||||
IsValidationRequired = true,
|
||||
IsValid = false,
|
||||
ValidatedPath = new List<string>(path),
|
||||
BacktrackingPatterns = new List<BacktrackingPattern>(backtrackingPatterns),
|
||||
StartNodeId = startNodeId,
|
||||
TargetNodeId = targetNodeId,
|
||||
JunctionNodeId = junctionNodeId,
|
||||
ValidationError = error
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 일반 검증 실패 결과 생성
|
||||
/// </summary>
|
||||
public static PathValidationResult CreateInvalid(string startNodeId, string targetNodeId, string error)
|
||||
{
|
||||
return new PathValidationResult
|
||||
{
|
||||
IsValidationRequired = true,
|
||||
IsValid = false,
|
||||
StartNodeId = startNodeId,
|
||||
TargetNodeId = targetNodeId,
|
||||
ValidationError = error
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 되돌아가기 패턴 정보 (A → B → A)
|
||||
/// </summary>
|
||||
public class BacktrackingPattern
|
||||
{
|
||||
/// <summary>
|
||||
/// 시작 노드 (A)
|
||||
/// </summary>
|
||||
public string StartNode { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 중간 노드 (B)
|
||||
/// </summary>
|
||||
public string MiddleNode { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 되돌아간 노드 (다시 A)
|
||||
/// </summary>
|
||||
public string ReturnNode { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 경로에서의 시작 인덱스
|
||||
/// </summary>
|
||||
public int StartIndex { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 경로에서의 종료 인덱스
|
||||
/// </summary>
|
||||
public int EndIndex { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 기본 생성자
|
||||
/// </summary>
|
||||
public BacktrackingPattern()
|
||||
{
|
||||
StartNode = string.Empty;
|
||||
MiddleNode = string.Empty;
|
||||
ReturnNode = string.Empty;
|
||||
StartIndex = -1;
|
||||
EndIndex = -1;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 되돌아가기 패턴 생성
|
||||
/// </summary>
|
||||
public static BacktrackingPattern Create(string startNode, string middleNode, string returnNode, int startIndex, int endIndex)
|
||||
{
|
||||
return new BacktrackingPattern
|
||||
{
|
||||
StartNode = startNode,
|
||||
MiddleNode = middleNode,
|
||||
ReturnNode = returnNode,
|
||||
StartIndex = startIndex,
|
||||
EndIndex = endIndex
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 패턴 설명 문자열
|
||||
/// </summary>
|
||||
public override string ToString()
|
||||
{
|
||||
return $"{StartNode} → {MiddleNode} → {ReturnNode} (인덱스: {StartIndex}-{EndIndex})";
|
||||
}
|
||||
}
|
||||
}
|
||||
36
AGVLogic/AGVNavigationCore/Properties/AssemblyInfo.cs
Normal file
36
AGVLogic/AGVNavigationCore/Properties/AssemblyInfo.cs
Normal file
@@ -0,0 +1,36 @@
|
||||
using System.Reflection;
|
||||
using System.Runtime.CompilerServices;
|
||||
using System.Runtime.InteropServices;
|
||||
|
||||
// General Information about an assembly is controlled through the following
|
||||
// set of attributes. Change these attribute values to modify the information
|
||||
// associated with an assembly.
|
||||
[assembly: AssemblyTitle("AGVNavigationCore")]
|
||||
[assembly: AssemblyDescription("AGV Navigation and Pathfinding Core Library")]
|
||||
[assembly: AssemblyConfiguration("")]
|
||||
[assembly: AssemblyCompany("ENIG")]
|
||||
[assembly: AssemblyProduct("AGV Navigation System")]
|
||||
[assembly: AssemblyCopyright("Copyright © ENIG 2024")]
|
||||
[assembly: AssemblyTrademark("")]
|
||||
[assembly: AssemblyCulture("")]
|
||||
|
||||
// Setting ComVisible to false makes the types in this assembly not visible
|
||||
// to COM components. If you need to access a type in this assembly from
|
||||
// COM, set the ComVisible attribute to true on that type.
|
||||
[assembly: ComVisible(false)]
|
||||
|
||||
// The following GUID is for the ID of the typelib if this project is exposed to COM
|
||||
[assembly: Guid("c5f7a8b2-8d3e-4a1b-9c6e-7f4d5e2a9b1c")]
|
||||
|
||||
// Version information for an assembly consists of the following four values:
|
||||
//
|
||||
// Major Version
|
||||
// Minor Version
|
||||
// Build Number
|
||||
// Revision
|
||||
//
|
||||
// You can specify all the values or you can default the Build and Revision Numbers
|
||||
// by using the '*' as shown below:
|
||||
// [assembly: AssemblyVersion("1.0.*")]
|
||||
[assembly: AssemblyVersion("1.0.0.0")]
|
||||
[assembly: AssemblyFileVersion("1.0.0.0")]
|
||||
155
AGVLogic/AGVNavigationCore/README.md
Normal file
155
AGVLogic/AGVNavigationCore/README.md
Normal 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월 업데이트
|
||||
125
AGVLogic/AGVNavigationCore/Utils/AGVDirectionCalculator.cs
Normal file
125
AGVLogic/AGVNavigationCore/Utils/AGVDirectionCalculator.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
464
AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
Normal file
464
AGVLogic/AGVNavigationCore/Utils/DirectionalHelper.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
198
AGVLogic/AGVNavigationCore/Utils/DirectionalPathfinderTest.cs
Normal file
198
AGVLogic/AGVNavigationCore/Utils/DirectionalPathfinderTest.cs
Normal 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; }
|
||||
}
|
||||
}
|
||||
}
|
||||
346
AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
Normal file
346
AGVLogic/AGVNavigationCore/Utils/DockingValidator.cs
Normal 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}";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
342
AGVLogic/AGVNavigationCore/Utils/GetNextNodeIdTest.cs
Normal file
342
AGVLogic/AGVNavigationCore/Utils/GetNextNodeIdTest.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
153
AGVLogic/AGVNavigationCore/Utils/ImageConverterUtil.cs
Normal file
153
AGVLogic/AGVNavigationCore/Utils/ImageConverterUtil.cs
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
281
AGVLogic/AGVNavigationCore/Utils/LiftCalculator.cs
Normal file
281
AGVLogic/AGVNavigationCore/Utils/LiftCalculator.cs
Normal 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
56
AGVLogic/AGVNavigationCore/Utils/TestRunner.cs
Normal file
56
AGVLogic/AGVNavigationCore/Utils/TestRunner.cs
Normal 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=== 테스트 완료 ===");
|
||||
}
|
||||
}
|
||||
}
|
||||
29
AGVLogic/AGVNavigationCore/build.bat
Normal file
29
AGVLogic/AGVNavigationCore/build.bat
Normal file
@@ -0,0 +1,29 @@
|
||||
@echo off
|
||||
echo Building V2GDecoder VC++ Project...
|
||||
|
||||
REM Check if Visual Studio 2022 is installed (Professional or Community)
|
||||
set MSBUILD_PRO="C:\Program Files\Microsoft Visual Studio\2022\Professional\MSBuild\Current\Bin\MSBuild.exe"
|
||||
set MSBUILD_COM="C:\Program Files\Microsoft Visual Studio\2022\Community\MSBuild\Current\Bin\MSBuild.exe"
|
||||
set MSBUILD_BT="F:\(VHD) Program Files\Microsoft Visual Studio\2022\MSBuild\Current\Bin\MSBuild.exe"
|
||||
|
||||
if exist %MSBUILD_PRO% (
|
||||
echo "Found Visual Studio 2022 Professional"
|
||||
set MSBUILD=%MSBUILD_PRO%
|
||||
) else if exist %MSBUILD_COM% (
|
||||
echo "Found Visual Studio 2022 Community"
|
||||
set MSBUILD=%MSBUILD_COM%
|
||||
) else if exist %MSBUILD_BT% (
|
||||
echo "Found Visual Studio 2022 BuildTools"
|
||||
set MSBUILD=%MSBUILD_BT%
|
||||
) else (
|
||||
echo "Visual Studio 2022 (Professional or Community) not found!"
|
||||
echo "Please install Visual Studio 2022 or update the MSBuild path."
|
||||
pause
|
||||
exit /b 1
|
||||
)
|
||||
|
||||
REM Build Debug x64 configuration
|
||||
echo Building Debug x64 configuration...
|
||||
%MSBUILD% AGVNavigationCore.csproj
|
||||
|
||||
pause
|
||||
4
AGVLogic/AGVNavigationCore/packages.config
Normal file
4
AGVLogic/AGVNavigationCore/packages.config
Normal file
@@ -0,0 +1,4 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<packages>
|
||||
<package id="Newtonsoft.Json" version="13.0.4" targetFramework="net48" />
|
||||
</packages>
|
||||
Reference in New Issue
Block a user