fix: Add motor direction parameter to magnet direction calculation in pathfinding
- Fixed critical issue in ConvertToDetailedPath where motor direction was not passed to GetRequiredMagnetDirection - Motor direction is essential for backward movement as Left/Right directions must be inverted - Modified AGVPathfinder.cs line 280 to pass currentDirection parameter - Ensures backward motor direction properly inverts magnet sensor directions feat: Add waypoint support to pathfinding system - Added FindPath overload with params string[] waypointNodeIds in AStarPathfinder - Supports sequential traversal through multiple intermediate nodes - Validates waypoints and prevents duplicates in sequence - Returns combined path result with aggregated metrics feat: Implement path result merging with DetailedPath preservation - Added CombineResults method in AStarPathfinder for intelligent path merging - Automatically deduplicates nodes when last of previous path equals first of current - Preserves DetailedPath information including motor and magnet directions - Essential for multi-segment path operations feat: Integrate magnet direction with motor direction awareness - Modified JunctionAnalyzer.GetRequiredMagnetDirection to accept AgvDirection parameter - Inverts Left/Right magnet directions when moving Backward - Properly handles motor direction context throughout pathfinding feat: Add automatic start node selection in simulator - Added SetStartNodeToCombo method to SimulatorForm - Automatically selects start node combo box when AGV position is set via RFID - Improves UI usability and workflow efficiency 🤖 Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
@@ -79,6 +79,7 @@
|
||||
<Compile Include="Models\MapNode.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" />
|
||||
@@ -94,6 +95,11 @@
|
||||
</Compile>
|
||||
<Compile Include="Utils\DockingValidator.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>
|
||||
|
||||
@@ -17,9 +17,9 @@ namespace AGVNavigationCore.Controls
|
||||
float BatteryLevel { get; }
|
||||
|
||||
// 이동 경로 정보 추가
|
||||
Point? TargetPosition { get; }
|
||||
Point? PrevPosition { get; }
|
||||
string CurrentNodeId { get; }
|
||||
string TargetNodeId { get; }
|
||||
string PrevNodeId { get; }
|
||||
DockingDirection DockingDirection { get; }
|
||||
|
||||
}
|
||||
|
||||
@@ -76,27 +76,21 @@ namespace AGVNavigationCore.Controls
|
||||
int startX = (bounds.Left / GRID_SIZE) * GRID_SIZE;
|
||||
int startY = (bounds.Top / GRID_SIZE) * GRID_SIZE;
|
||||
|
||||
// 월드 좌표로 그리드 라인 계산
|
||||
// 월드 좌표로 그리드 라인 계산 (Transform이 자동으로 적용됨)
|
||||
for (int x = startX; x <= bounds.Right; x += GRID_SIZE)
|
||||
{
|
||||
// 월드 좌표를 스크린 좌표로 변환
|
||||
int screenX = x * (int)_zoomFactor + _panOffset.X;
|
||||
|
||||
if (x % (GRID_SIZE * 5) == 0)
|
||||
g.DrawLine(new Pen(Color.Gray, 1), screenX, 0, screenX, Height);
|
||||
g.DrawLine(new Pen(Color.Gray, 1), x, bounds.Top, x, bounds.Bottom);
|
||||
else
|
||||
g.DrawLine(_gridPen, screenX, 0, screenX, Height);
|
||||
g.DrawLine(_gridPen, x, bounds.Top, x, bounds.Bottom);
|
||||
}
|
||||
|
||||
for (int y = startY; y <= bounds.Bottom; y += GRID_SIZE)
|
||||
{
|
||||
// 월드 좌표를 스크린 좌표로 변환
|
||||
int screenY = y * (int)_zoomFactor + _panOffset.Y;
|
||||
|
||||
if (y % (GRID_SIZE * 5) == 0)
|
||||
g.DrawLine(new Pen(Color.Gray, 1), 0, screenY, Width, screenY);
|
||||
g.DrawLine(new Pen(Color.Gray, 1), bounds.Left, y, bounds.Right, y);
|
||||
else
|
||||
g.DrawLine(_gridPen, 0, screenY, Width, screenY);
|
||||
g.DrawLine(_gridPen, bounds.Left, y, bounds.Right, y);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -240,86 +234,8 @@ namespace AGVNavigationCore.Controls
|
||||
pathPen.Dispose();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 및 모터방향 정보를 시각화
|
||||
/// </summary>
|
||||
/// <param name="g">Graphics 객체</param>
|
||||
/// <param name="agvResult">AGV 경로 계산 결과</param>
|
||||
private void DrawAGVPath(Graphics g, AGVPathResult agvResult)
|
||||
{
|
||||
if (agvResult?.NodeMotorInfos == null || agvResult.NodeMotorInfos.Count == 0) return;
|
||||
|
||||
|
||||
// 노드별 모터방향 정보를 기반으로 향상된 경로 표시
|
||||
for (int i = 0; i < agvResult.NodeMotorInfos.Count - 1; i++)
|
||||
{
|
||||
var currentMotorInfo = agvResult.NodeMotorInfos[i];
|
||||
var nextMotorInfo = agvResult.NodeMotorInfos[i + 1];
|
||||
|
||||
var currentNode = _nodes?.FirstOrDefault(n => n.NodeId == currentMotorInfo.NodeId);
|
||||
var nextNode = _nodes?.FirstOrDefault(n => n.NodeId == nextMotorInfo.NodeId);
|
||||
|
||||
if (currentNode != null && nextNode != null)
|
||||
{
|
||||
// 모터방향에 따른 색상 결정
|
||||
var motorDirection = currentMotorInfo.MotorDirection;
|
||||
Color pathColor = motorDirection == AgvDirection.Forward ? Color.Green : Color.Orange;
|
||||
|
||||
// 강조된 경로 선 그리기
|
||||
var enhancedPen = new Pen(pathColor, 6) { DashStyle = DashStyle.Solid };
|
||||
g.DrawLine(enhancedPen, currentNode.Position, nextNode.Position);
|
||||
|
||||
// 중간점에 모터방향 화살표 표시
|
||||
var midPoint = new Point(
|
||||
(currentNode.Position.X + nextNode.Position.X) / 2,
|
||||
(currentNode.Position.Y + nextNode.Position.Y) / 2
|
||||
);
|
||||
|
||||
var angle = Math.Atan2(nextNode.Position.Y - currentNode.Position.Y,
|
||||
nextNode.Position.X - currentNode.Position.X);
|
||||
|
||||
// 모터방향별 화살표 그리기
|
||||
DrawDirectionArrow(g, midPoint, angle, motorDirection);
|
||||
|
||||
// 노드 옆에 모터방향 텍스트 표시
|
||||
DrawMotorDirectionLabel(g, currentNode.Position, motorDirection);
|
||||
|
||||
enhancedPen.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
// 마지막 노드의 모터방향 표시
|
||||
if (agvResult.NodeMotorInfos.Count > 0)
|
||||
{
|
||||
var lastMotorInfo = agvResult.NodeMotorInfos[agvResult.NodeMotorInfos.Count - 1];
|
||||
var lastNode = _nodes?.FirstOrDefault(n => n.NodeId == lastMotorInfo.NodeId);
|
||||
if (lastNode != null)
|
||||
{
|
||||
DrawMotorDirectionLabel(g, lastNode.Position, lastMotorInfo.MotorDirection);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 모터방향 레이블 표시
|
||||
/// </summary>
|
||||
/// <param name="g">Graphics 객체</param>
|
||||
/// <param name="nodePosition">노드 위치</param>
|
||||
/// <param name="motorDirection">모터방향</param>
|
||||
private void DrawMotorDirectionLabel(Graphics g, Point nodePosition, AgvDirection motorDirection)
|
||||
{
|
||||
string motorText = motorDirection == AgvDirection.Forward ? "전진" : "후진";
|
||||
Color textColor = motorDirection == AgvDirection.Forward ? Color.DarkGreen : Color.DarkOrange;
|
||||
|
||||
var font = new Font("맑은 고딕", 8, FontStyle.Bold);
|
||||
var brush = new SolidBrush(textColor);
|
||||
|
||||
// 노드 우측 상단에 모터방향 텍스트 표시
|
||||
var textPosition = new Point(nodePosition.X + NODE_RADIUS + 2, nodePosition.Y - NODE_RADIUS - 2);
|
||||
g.DrawString(motorText, font, brush, textPosition);
|
||||
|
||||
font.Dispose();
|
||||
brush.Dispose();
|
||||
}
|
||||
|
||||
private void DrawNodesOnly(Graphics g)
|
||||
{
|
||||
@@ -1027,7 +943,7 @@ namespace AGVNavigationCore.Controls
|
||||
const int liftDistance = AGV_SIZE / 2 + 2; // AGV 본체 면에 바로 붙도록
|
||||
|
||||
var currentPos = agv.CurrentPosition;
|
||||
var targetPos = agv.TargetPosition;
|
||||
var targetPos = agv.PrevPosition;
|
||||
var dockingDirection = agv.DockingDirection;
|
||||
var currentDirection = agv.CurrentDirection;
|
||||
|
||||
@@ -1194,7 +1110,7 @@ namespace AGVNavigationCore.Controls
|
||||
private void DrawAGVLiftDebugInfo(Graphics g, IAGV agv)
|
||||
{
|
||||
var currentPos = agv.CurrentPosition;
|
||||
var targetPos = agv.TargetPosition;
|
||||
var targetPos = agv.PrevPosition;
|
||||
|
||||
// 디버그 정보 (개발용)
|
||||
if (targetPos.HasValue)
|
||||
@@ -1331,7 +1247,7 @@ namespace AGVNavigationCore.Controls
|
||||
const int monitorDistance = AGV_SIZE / 2 + 2; // AGV 본체에서 거리 (리프트와 동일)
|
||||
|
||||
var currentPos = agv.CurrentPosition;
|
||||
var targetPos = agv.TargetPosition;
|
||||
var targetPos = agv.PrevPosition;
|
||||
var dockingDirection = agv.DockingDirection;
|
||||
var currentDirection = agv.CurrentDirection;
|
||||
|
||||
|
||||
@@ -215,21 +215,18 @@ namespace AGVNavigationCore.Controls
|
||||
|
||||
private Point ScreenToWorld(Point screenPoint)
|
||||
{
|
||||
// 변환 행렬 생성 (렌더링과 동일)
|
||||
var transform = new System.Drawing.Drawing2D.Matrix();
|
||||
transform.Scale(_zoomFactor, _zoomFactor);
|
||||
transform.Translate(_panOffset.X, _panOffset.Y);
|
||||
// 스크린 좌표를 월드 좌표로 변환
|
||||
// 역순으로: 팬 오프셋 제거 → 줌 적용
|
||||
float worldX = (screenPoint.X - _panOffset.X) / _zoomFactor;
|
||||
float worldY = (screenPoint.Y - _panOffset.Y) / _zoomFactor;
|
||||
|
||||
// 역변환 행렬로 화면 좌표를 월드 좌표로 변환
|
||||
transform.Invert();
|
||||
var points = new System.Drawing.PointF[] { new System.Drawing.PointF(screenPoint.X, screenPoint.Y) };
|
||||
transform.TransformPoints(points);
|
||||
|
||||
return new Point((int)points[0].X, (int)points[0].Y);
|
||||
return new Point((int)worldX, (int)worldY);
|
||||
}
|
||||
|
||||
private Point WorldToScreen(Point worldPoint)
|
||||
{
|
||||
// 월드 좌표를 스크린 좌표로 변환
|
||||
// 순서: 줌 적용 → 팬 오프셋 추가
|
||||
return new Point(
|
||||
(int)(worldPoint.X * _zoomFactor + _panOffset.X),
|
||||
(int)(worldPoint.Y * _zoomFactor + _panOffset.Y)
|
||||
|
||||
@@ -469,9 +469,10 @@ namespace AGVNavigationCore.Controls
|
||||
/// </summary>
|
||||
/// <param name="agvId">AGV ID</param>
|
||||
/// <param name="position">새로운 위치</param>
|
||||
public void SetAGVPosition(string agvId, Point position)
|
||||
public void SetAGVPosition(string agvId, MapNode node, AgvDirection direction)
|
||||
{
|
||||
UpdateAGVPosition(agvId, position);
|
||||
UpdateAGVPosition(agvId, node.Position);
|
||||
UpdateAGVDirection(agvId, direction);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -87,12 +87,12 @@ namespace AGVNavigationCore.Models
|
||||
/// <summary>
|
||||
/// 목표 위치
|
||||
/// </summary>
|
||||
Point? TargetPosition { get; }
|
||||
Point? PrevPosition { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 목표 노드 ID
|
||||
/// </summary>
|
||||
string TargetNodeId { get; }
|
||||
string PrevNodeId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 방향
|
||||
|
||||
@@ -81,6 +81,9 @@ namespace AGVNavigationCore.Models
|
||||
// 중복 연결 정리 (양방향 중복 제거)
|
||||
CleanupDuplicateConnections(result.Nodes);
|
||||
|
||||
// 양방향 연결 자동 설정 (A→B가 있으면 B→A도 설정)
|
||||
EnsureBidirectionalConnections(result.Nodes);
|
||||
|
||||
// 이미지 노드들의 이미지 로드
|
||||
LoadImageNodes(result.Nodes);
|
||||
|
||||
@@ -324,6 +327,67 @@ namespace AGVNavigationCore.Models
|
||||
}
|
||||
}
|
||||
|
||||
/// <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.NodeId))
|
||||
{
|
||||
allConnections[node.NodeId] = new HashSet<string>();
|
||||
}
|
||||
|
||||
if (node.ConnectedNodes != null)
|
||||
{
|
||||
foreach (var connectedId in node.ConnectedNodes)
|
||||
{
|
||||
allConnections[node.NodeId].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.NodeId) continue;
|
||||
|
||||
// 다른 노드가 이 노드를 연결하고 있다면
|
||||
if (allConnections[otherNodeId].Contains(node.NodeId))
|
||||
{
|
||||
// 이 노드의 ConnectedNodes에 그 노드를 추가 (중복 방지)
|
||||
if (!node.ConnectedNodes.Contains(otherNodeId))
|
||||
{
|
||||
node.ConnectedNodes.Add(otherNodeId);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// MapNode 목록에서 RFID가 없는 노드들에 자동으로 RFID ID를 할당합니다.
|
||||
/// *** 에디터와 시뮬레이터 데이터 불일치 방지를 위해 비활성화됨 ***
|
||||
|
||||
@@ -2,6 +2,7 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Drawing2D;
|
||||
using AGVNavigationCore.Utils;
|
||||
|
||||
namespace AGVNavigationCore.Models
|
||||
{
|
||||
@@ -132,10 +133,16 @@ namespace AGVNavigationCore.Models
|
||||
public bool ShowBackground { get; set; } = false;
|
||||
|
||||
/// <summary>
|
||||
/// 이미지 파일 경로 (NodeType.Image인 경우 사용)
|
||||
/// 이미지 파일 경로 (편집용, 저장시엔 사용되지 않음)
|
||||
/// </summary>
|
||||
[Newtonsoft.Json.JsonIgnore]
|
||||
public string ImagePath { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// Base64 인코딩된 이미지 데이터 (JSON 저장용)
|
||||
/// </summary>
|
||||
public string ImageBase64 { get; set; } = string.Empty;
|
||||
|
||||
/// <summary>
|
||||
/// 이미지 크기 배율 (NodeType.Image인 경우 사용)
|
||||
/// </summary>
|
||||
@@ -331,6 +338,7 @@ namespace AGVNavigationCore.Models
|
||||
BackColor = BackColor,
|
||||
ShowBackground = ShowBackground,
|
||||
ImagePath = ImagePath,
|
||||
ImageBase64 = ImageBase64,
|
||||
Scale = Scale,
|
||||
Opacity = Opacity,
|
||||
Rotation = Rotation
|
||||
@@ -339,7 +347,7 @@ namespace AGVNavigationCore.Models
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이미지 로드 (256x256 이상일 경우 자동 리사이즈)
|
||||
/// 이미지 로드 (Base64 또는 파일 경로에서, 256x256 이상일 경우 자동 리사이즈)
|
||||
/// </summary>
|
||||
/// <returns>로드 성공 여부</returns>
|
||||
public bool LoadImage()
|
||||
@@ -348,11 +356,23 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
try
|
||||
{
|
||||
if (!string.IsNullOrEmpty(ImagePath) && System.IO.File.Exists(ImagePath))
|
||||
Image originalImage = null;
|
||||
|
||||
// 1. 먼저 Base64 데이터 시도
|
||||
if (!string.IsNullOrEmpty(ImageBase64))
|
||||
{
|
||||
originalImage = ImageConverterUtil.Base64ToImage(ImageBase64);
|
||||
}
|
||||
// 2. Base64가 없으면 파일 경로에서 로드
|
||||
else if (!string.IsNullOrEmpty(ImagePath) && System.IO.File.Exists(ImagePath))
|
||||
{
|
||||
originalImage = Image.FromFile(ImagePath);
|
||||
}
|
||||
|
||||
if (originalImage != null)
|
||||
{
|
||||
LoadedImage?.Dispose();
|
||||
var originalImage = Image.FromFile(ImagePath);
|
||||
|
||||
|
||||
// 이미지 크기 체크 및 리사이즈
|
||||
if (originalImage.Width > 256 || originalImage.Height > 256)
|
||||
{
|
||||
@@ -363,7 +383,7 @@ namespace AGVNavigationCore.Models
|
||||
{
|
||||
LoadedImage = originalImage;
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -419,6 +439,33 @@ namespace AGVNavigationCore.Models
|
||||
);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 파일 경로에서 이미지를 Base64로 변환하여 저장
|
||||
/// </summary>
|
||||
/// <param name="filePath">이미지 파일 경로</param>
|
||||
/// <returns>변환 성공 여부</returns>
|
||||
public bool ConvertImageToBase64(string filePath)
|
||||
{
|
||||
if (Type != NodeType.Image) return false;
|
||||
|
||||
try
|
||||
{
|
||||
if (!System.IO.File.Exists(filePath))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
ImageBase64 = ImageConverterUtil.FileToBase64(filePath, System.Drawing.Imaging.ImageFormat.Png);
|
||||
ImagePath = filePath; // 편집용으로 경로 유지
|
||||
|
||||
return !string.IsNullOrEmpty(ImageBase64);
|
||||
}
|
||||
catch (Exception)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 리소스 정리
|
||||
/// </summary>
|
||||
|
||||
@@ -49,11 +49,11 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
private string _agvId;
|
||||
private Point _currentPosition;
|
||||
private Point _targetPosition;
|
||||
private Point _prevPosition;
|
||||
private string _targetId;
|
||||
private string _currentId;
|
||||
private AgvDirection _currentDirection;
|
||||
private AgvDirection _targetDirection;
|
||||
private AgvDirection _prevDirection;
|
||||
private AGVState _currentState;
|
||||
private float _currentSpeed;
|
||||
|
||||
@@ -62,7 +62,7 @@ namespace AGVNavigationCore.Models
|
||||
private List<string> _remainingNodes;
|
||||
private int _currentNodeIndex;
|
||||
private MapNode _currentNode;
|
||||
private MapNode _targetNode;
|
||||
private MapNode _prevNode;
|
||||
|
||||
// 이동 관련
|
||||
private DateTime _lastUpdateTime;
|
||||
@@ -82,6 +82,13 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
#region Properties
|
||||
|
||||
/// <summary>
|
||||
/// 대상 이동시 모터 방향
|
||||
/// </summary>
|
||||
public AgvDirection PrevDirection => _prevDirection;
|
||||
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// AGV ID
|
||||
/// </summary>
|
||||
@@ -131,9 +138,9 @@ namespace AGVNavigationCore.Models
|
||||
public string CurrentNodeId => _currentNode?.NodeId;
|
||||
|
||||
/// <summary>
|
||||
/// 목표 위치
|
||||
/// 이전 위치
|
||||
/// </summary>
|
||||
public Point? TargetPosition => _targetPosition;
|
||||
public Point? PrevPosition => _prevPosition;
|
||||
|
||||
/// <summary>
|
||||
/// 배터리 레벨 (시뮬레이션)
|
||||
@@ -141,9 +148,14 @@ namespace AGVNavigationCore.Models
|
||||
public float BatteryLevel { get; set; } = 100.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 목표 노드 ID
|
||||
/// 이전 노드 ID
|
||||
/// </summary>
|
||||
public string TargetNodeId => _targetNode?.NodeId;
|
||||
public string PrevNodeId => _prevNode?.NodeId;
|
||||
|
||||
/// <summary>
|
||||
/// 이전 노드
|
||||
/// </summary>
|
||||
public MapNode PrevNode => _prevNode;
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 방향
|
||||
@@ -169,7 +181,7 @@ namespace AGVNavigationCore.Models
|
||||
_currentSpeed = 0;
|
||||
_dockingDirection = DockingDirection.Forward; // 기본값: 전진 도킹
|
||||
_currentNode = null;
|
||||
_targetNode = null;
|
||||
_prevNode = null;
|
||||
_isMoving = false;
|
||||
_lastUpdateTime = DateTime.Now;
|
||||
}
|
||||
@@ -284,7 +296,7 @@ namespace AGVNavigationCore.Models
|
||||
if (targetNode != null)
|
||||
{
|
||||
_dockingDirection = GetDockingDirection(targetNode.Type);
|
||||
_targetNode = targetNode;
|
||||
_prevNode = targetNode;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -356,7 +368,7 @@ namespace AGVNavigationCore.Models
|
||||
/// <param name="targetPosition">목표 위치</param>
|
||||
public void MoveTo(Point targetPosition)
|
||||
{
|
||||
_targetPosition = targetPosition;
|
||||
_prevPosition = targetPosition;
|
||||
_moveStartPosition = _currentPosition;
|
||||
_moveTargetPosition = targetPosition;
|
||||
_moveProgress = 0;
|
||||
@@ -407,23 +419,28 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
/// <summary>
|
||||
/// AGV 위치 직접 설정
|
||||
/// TargetPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
|
||||
/// PrevPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
|
||||
/// </summary>
|
||||
/// <param name="node">현재 노드</param>
|
||||
/// <param name="newPosition">새로운 위치</param>
|
||||
/// <param name="motorDirection">모터이동방향</param>
|
||||
public void SetPosition(MapNode node, Point newPosition, AgvDirection motorDirection)
|
||||
public void SetPosition(MapNode node, AgvDirection motorDirection)
|
||||
{
|
||||
// 현재 위치를 이전 위치로 저장 (리프트 방향 계산용)
|
||||
if (_currentPosition != Point.Empty)
|
||||
if (_currentNode != null && _currentNode.NodeId != node.NodeId)
|
||||
{
|
||||
_targetPosition = _currentPosition; // 이전 위치
|
||||
_targetDirection = _currentDirection;
|
||||
_targetNode = node;
|
||||
_prevPosition = _currentPosition; // 이전 위치
|
||||
_prevNode = _currentNode;
|
||||
}
|
||||
|
||||
//모터방향이 다르다면 적용한다
|
||||
if (_currentDirection != motorDirection)
|
||||
{
|
||||
_prevDirection = _currentDirection;
|
||||
}
|
||||
|
||||
// 새로운 위치 설정
|
||||
_currentPosition = newPosition;
|
||||
_currentPosition = node.Position;
|
||||
_currentDirection = motorDirection;
|
||||
_currentNode = node;
|
||||
|
||||
@@ -598,6 +615,254 @@ namespace AGVNavigationCore.Models
|
||||
|
||||
#endregion
|
||||
|
||||
#region Directional Navigation
|
||||
|
||||
/// <summary>
|
||||
/// 현재 이전/현재 위치와 이동 방향을 기반으로 다음 노드 ID를 반환
|
||||
///
|
||||
/// 사용 예시:
|
||||
/// - 001에서 002로 이동 후, Forward 선택 → 003 반환
|
||||
/// - 003에서 004로 이동 후, Right 선택 → 030 반환
|
||||
/// - 004에서 003으로 Backward 이동 중, GetNextNodeId(Backward) → 002 반환
|
||||
///
|
||||
/// 전제조건: SetPosition이 최소 2번 이상 호출되어 _prevPosition이 설정되어야 함
|
||||
/// </summary>
|
||||
/// <param name="direction">이동 방향 (Forward/Backward/Left/Right)</param>
|
||||
/// <param name="allNodes">맵의 모든 노드</param>
|
||||
/// <returns>다음 노드 ID (또는 null)</returns>
|
||||
public MapNode GetNextNodeId(AgvDirection direction, List<MapNode> allNodes)
|
||||
{
|
||||
// 전제조건 검증: 2개 위치 히스토리 필요
|
||||
if ( _prevNode == null)
|
||||
{
|
||||
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.NodeId) && n.NodeId != _currentNode.NodeId
|
||||
).ToList();
|
||||
|
||||
if (candidateNodes.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
// 이전→현재 벡터 계산 (진행 방향 벡터)
|
||||
var movementVector = new PointF(
|
||||
_currentPosition.X - _prevPosition.X,
|
||||
_currentPosition.Y - _prevPosition.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
|
||||
);
|
||||
|
||||
// 각 후보 노드에 대해 방향 점수 계산
|
||||
var bestCandidate = (node: (MapNode)null, score: 0.0f);
|
||||
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
var toNextVector = new PointF(
|
||||
candidate.Position.X - _currentPosition.X,
|
||||
candidate.Position.Y - _currentPosition.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
|
||||
);
|
||||
|
||||
if (score > bestCandidate.score)
|
||||
{
|
||||
bestCandidate = (candidate, score);
|
||||
}
|
||||
}
|
||||
|
||||
return bestCandidate.node;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이동 방향을 기반으로 방향 점수를 계산
|
||||
/// 높은 점수 = 더 나은 선택지
|
||||
/// </summary>
|
||||
private float CalculateDirectionalScore(
|
||||
PointF movementDirection, // 정규화된 이전→현재 벡터
|
||||
PointF nextDirection, // 정규화된 현재→다음 벡터
|
||||
AgvDirection requestedDir) // 요청된 이동 방향
|
||||
{
|
||||
// 벡터 간 내적 계산 (유사도: -1 ~ 1)
|
||||
float dotProduct = (movementDirection.X * nextDirection.X) +
|
||||
(movementDirection.Y * nextDirection.Y);
|
||||
|
||||
// 벡터 간 외적 계산 (좌우 판별: Z 성분)
|
||||
// 양수 = 좌측, 음수 = 우측
|
||||
float crossProduct = (movementDirection.X * nextDirection.Y) -
|
||||
(movementDirection.Y * nextDirection.X);
|
||||
|
||||
float baseScore = 0;
|
||||
|
||||
switch (requestedDir)
|
||||
{
|
||||
case AgvDirection.Forward:
|
||||
// Forward: 현재 모터 상태에 따라 다름
|
||||
// 1) 현재 Forward 모터 상태라면 → 같은 경로 (계속 전진)
|
||||
// 2) 현재 Backward 모터 상태라면 → 반대 경로 (모터 방향 전환)
|
||||
if (_currentDirection == AgvDirection.Forward)
|
||||
{
|
||||
// 이미 Forward 상태, 계속 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;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Backward 상태에서 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:
|
||||
// Backward: 현재 모터 상태에 따라 다름
|
||||
// 1) 현재 Backward 모터 상태라면 → 같은 경로 (Forward와 동일)
|
||||
// 2) 현재 Forward 모터 상태라면 → 반대 경로 (현재의 Backward와 반대)
|
||||
if (_currentDirection == AgvDirection.Backward)
|
||||
{
|
||||
// 이미 Backward 상태, 계속 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;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Forward 상태에서 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:
|
||||
// Left: 좌측 방향 선호
|
||||
if (dotProduct > 0.0f) // Forward 상태
|
||||
{
|
||||
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 // Backward 상태 - 좌우 반전
|
||||
{
|
||||
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:
|
||||
// Right: 우측 방향 선호
|
||||
if (dotProduct > 0.0f) // Forward 상태
|
||||
{
|
||||
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 // Backward 상태 - 좌우 반전
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Cleanup
|
||||
|
||||
/// <summary>
|
||||
|
||||
@@ -206,9 +206,14 @@ namespace AGVNavigationCore.PathFinding.Analysis
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 경로에서 요구되는 마그넷 방향 계산 (전진 방향 기준)
|
||||
/// 특정 경로에서 요구되는 마그넷 방향 계산
|
||||
/// </summary>
|
||||
public MagnetDirection GetRequiredMagnetDirection(string fromNodeId, string currentNodeId, string toNodeId)
|
||||
/// <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;
|
||||
@@ -240,12 +245,26 @@ namespace AGVNavigationCore.PathFinding.Analysis
|
||||
|
||||
// 전진 방향 기준으로 마그넷 방향 결정
|
||||
// 각도 차이가 작으면 직진, 음수면 왼쪽, 양수면 오른쪽
|
||||
MagnetDirection baseMagnetDirection;
|
||||
if (Math.Abs(angleDiff) < Math.PI / 6) // 30도 이내는 직진
|
||||
return MagnetDirection.Straight;
|
||||
baseMagnetDirection = MagnetDirection.Straight;
|
||||
else if (angleDiff < 0) // 음수면 왼쪽 회전
|
||||
return MagnetDirection.Left;
|
||||
baseMagnetDirection = MagnetDirection.Left;
|
||||
else // 양수면 오른쪽 회전
|
||||
return MagnetDirection.Right;
|
||||
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>
|
||||
|
||||
@@ -27,11 +27,6 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
/// </summary>
|
||||
public List<AgvDirection> Commands { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 노드별 모터방향 정보 목록
|
||||
/// </summary>
|
||||
public List<NodeMotorInfo> NodeMotorInfos { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 총 거리
|
||||
/// </summary>
|
||||
@@ -104,7 +99,6 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
Success = false;
|
||||
Path = new List<string>();
|
||||
Commands = new List<AgvDirection>();
|
||||
NodeMotorInfos = new List<NodeMotorInfo>();
|
||||
DetailedPath = new List<NodeMotorInfo>();
|
||||
TotalDistance = 0;
|
||||
CalculationTimeMs = 0;
|
||||
@@ -157,7 +151,6 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
Success = true,
|
||||
Path = new List<string>(path),
|
||||
Commands = new List<AgvDirection>(commands),
|
||||
NodeMotorInfos = new List<NodeMotorInfo>(nodeMotorInfos),
|
||||
TotalDistance = totalDistance,
|
||||
CalculationTimeMs = calculationTimeMs
|
||||
};
|
||||
|
||||
@@ -3,6 +3,7 @@ using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
@@ -166,6 +167,247 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
}
|
||||
}
|
||||
|
||||
/// <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 FindPath(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 FindPath(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<string>();
|
||||
float totalDistance = 0;
|
||||
long totalCalculationTime = 0;
|
||||
|
||||
// 현재 시작점
|
||||
string currentStart = startNodeId;
|
||||
|
||||
// 1단계: 각 경유지까지의 경로 계산
|
||||
for (int i = 0; i < validWaypoints.Count; i++)
|
||||
{
|
||||
string waypoint = validWaypoints[i];
|
||||
|
||||
// 현재 위치에서 경유지까지의 경로 계산
|
||||
var segmentResult = FindPath(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 = FindPath(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.ErrorMessage}",
|
||||
previousResult.CalculationTimeMs);
|
||||
|
||||
if (!currentResult.Success)
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"현재 경로 결과 실패: {currentResult.ErrorMessage}",
|
||||
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<string>(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];
|
||||
string firstNodeOfCurrent = currentResult.Path[0];
|
||||
|
||||
if (lastNodeOfPrevious == firstNodeOfCurrent)
|
||||
{
|
||||
// 첫 번째 노드 제거 (중복 제거)
|
||||
combinedPath.AddRange(currentResult.Path.Skip(1));
|
||||
|
||||
// DetailedPath도 첫 번째 노드 제거
|
||||
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
|
||||
{
|
||||
combinedDetailedPath.AddRange(currentResult.DetailedPath.Skip(1));
|
||||
}
|
||||
}
|
||||
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;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
|
||||
/// </summary>
|
||||
@@ -268,6 +510,7 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 네비게이션 가능한 노드 목록 반환
|
||||
/// </summary>
|
||||
@@ -286,5 +529,69 @@ namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
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.NodeId == 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)
|
||||
{
|
||||
// 조건 1: 왔던 길이 아님
|
||||
if (connectedNodeId == previousNodeId)
|
||||
continue;
|
||||
|
||||
// 조건 2: 갈 길이 아님
|
||||
if (connectedNodeId == targetNodeId)
|
||||
continue;
|
||||
|
||||
// 조건 3, 4, 5: 존재하고, 활성 상태이고, 네비게이션 가능
|
||||
var connectedNode = _mapNodes.FirstOrDefault(n => n.NodeId == connectedNodeId);
|
||||
if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
|
||||
{
|
||||
alternateNodes.Add(connectedNode);
|
||||
}
|
||||
}
|
||||
|
||||
// 찾은 노드가 없으면 null 반환
|
||||
if (alternateNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 여러 개 찾았으면 첫 번째 노드 반환
|
||||
// (필요시 거리 기반으로 가장 가까운 노드를 선택할 수도 있음)
|
||||
return alternateNodes[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -30,130 +30,236 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 계산
|
||||
/// 지정한 노드에서 가장 가까운 교차로(3개 이상 연결된 노드)를 찾는다.
|
||||
/// </summary>
|
||||
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode,
|
||||
MapNode prevNode, AgvDirection currentDirection = AgvDirection.Forward)
|
||||
/// <param name="startNode">기준이 되는 노드</param>
|
||||
/// <returns>가장 가까운 교차로 노드 (또는 null)</returns>
|
||||
public MapNode FindNearestJunction(MapNode startNode)
|
||||
{
|
||||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
if (startNode == null || _mapNodes == null || _mapNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
try
|
||||
// 교차로: 3개 이상의 노드가 연결된 노드
|
||||
var junctions = _mapNodes.Where(n =>
|
||||
n.IsActive &&
|
||||
n.IsNavigationNode() &&
|
||||
n.ConnectedNodes != null &&
|
||||
n.ConnectedNodes.Count >= 3 &&
|
||||
n.NodeId != startNode.NodeId
|
||||
).ToList();
|
||||
|
||||
if (junctions.Count == 0)
|
||||
return null;
|
||||
|
||||
// 직선 거리 기반으로 가장 가까운 교차로 찾기
|
||||
MapNode nearestJunction = null;
|
||||
float minDistance = float.MaxValue;
|
||||
|
||||
foreach (var junction in junctions)
|
||||
{
|
||||
// 입력 검증
|
||||
if (startNode == null)
|
||||
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
|
||||
if (targetNode == null)
|
||||
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
|
||||
if (prevNode == null)
|
||||
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
|
||||
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);
|
||||
|
||||
// 1. 목적지 도킹 방향 요구사항 확인 (노드의 도킹방향 속성에서 확인)
|
||||
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
|
||||
|
||||
|
||||
// 통합된 경로 계획 함수 사용
|
||||
AGVPathResult result = PlanPath(startNode, targetNode, prevNode, requiredDirection, currentDirection);
|
||||
|
||||
result.CalculationTimeMs = stopwatch.ElapsedMilliseconds;
|
||||
|
||||
// 도킹 검증 수행
|
||||
if (result.Success && _mapNodes != null)
|
||||
if (distance < minDistance)
|
||||
{
|
||||
result.DockingValidation = DockingValidator.ValidateDockingDirection(result, _mapNodes, currentDirection);
|
||||
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 nodeId in pathResult.Path)
|
||||
{
|
||||
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
|
||||
if (node != null &&
|
||||
node.IsActive &&
|
||||
node.IsNavigationNode() &&
|
||||
node.ConnectedNodes != null &&
|
||||
node.ConnectedNodes.Count >= 3)
|
||||
{
|
||||
if (node.NodeId.Equals(StartNode) == false)
|
||||
return node;
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
public AGVPathResult FindPath_test(MapNode startNode, MapNode targetNode,
|
||||
MapNode prevNode, AgvDirection currentDirection)
|
||||
{
|
||||
// 입력 검증
|
||||
if (startNode == null)
|
||||
return AGVPathResult.CreateFailure("시작 노드가 null입니다.", 0, 0);
|
||||
if (targetNode == null)
|
||||
return AGVPathResult.CreateFailure("목적지 노드가 null입니다.", 0, 0);
|
||||
if (prevNode == null)
|
||||
return AGVPathResult.CreateFailure("이전위치 노드가 null입니다.", 0, 0);
|
||||
if (startNode == targetNode)
|
||||
return AGVPathResult.CreateFailure("목적지와 현재위치가 동일합니다.", 0, 0);
|
||||
|
||||
var ReverseDirection = (currentDirection == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
|
||||
|
||||
//1.목적지까지의 최단거리 경로를 찾는다.
|
||||
var pathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
|
||||
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
|
||||
|
||||
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
|
||||
if (targetNode.DockDirection == DockingDirection.DontCare ||
|
||||
(targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
|
||||
(targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
|
||||
{
|
||||
MakeDetailData(pathResult, currentDirection);
|
||||
MakeMagnetDirection(pathResult);
|
||||
return pathResult;
|
||||
}
|
||||
|
||||
//3. 도킹방향이 일치하지 않으니 교차로에서 방향을 회전시켜야 한다
|
||||
//최단거리(=minpath)경로에 속하는 교차로가 있다면 그것을 사용하고 없다면 가장 가까운 교차로를 찾는다.
|
||||
var JunctionInPath = FindNearestJunctionOnPath(pathResult);
|
||||
if (JunctionInPath == null)
|
||||
{
|
||||
//시작노드로부터 가까운 교차로 검색
|
||||
JunctionInPath = FindNearestJunction(startNode);
|
||||
|
||||
//종료노드로부터 가까운 교차로 검색
|
||||
if (JunctionInPath == null) JunctionInPath = FindNearestJunction(targetNode);
|
||||
}
|
||||
if (JunctionInPath == null)
|
||||
return AGVPathResult.CreateFailure("교차로가 없어 경로계산을 할 수 없습니다", 0, 0);
|
||||
|
||||
//경유지를 포함하여 경로를 다시 계산한다.
|
||||
|
||||
//1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
|
||||
var path1 = _basicPathfinder.FindPath(startNode.NodeId, JunctionInPath.NodeId);
|
||||
|
||||
// path1의 상세 경로 정보 채우기 (모터 방향 설정)
|
||||
MakeDetailData(path1, currentDirection);
|
||||
|
||||
//2.교차로 - 종료위치
|
||||
var path2 = _basicPathfinder.FindPath(JunctionInPath.NodeId, targetNode.NodeId);
|
||||
MakeDetailData(path2, ReverseDirection);
|
||||
|
||||
//3.방향전환을 위환 대체 노드찾기
|
||||
var tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.NodeId,
|
||||
path1.Path[path1.Path.Count - 2],
|
||||
path2.Path[1]);
|
||||
|
||||
//4. path1 + tempnode + path2 가 최종 위치가 된다.
|
||||
if (tempNode == null)
|
||||
return AGVPathResult.CreateFailure("방향 전환을 위한 대체 노드를 찾을 수 없습니다.", 0, 0);
|
||||
|
||||
// path1 (시작 → 교차로)
|
||||
var combinedResult = path1;
|
||||
|
||||
// 교차로 → 대체노드 경로 계산
|
||||
var pathToTemp = _basicPathfinder.FindPath(JunctionInPath.NodeId, tempNode.NodeId);
|
||||
if (!pathToTemp.Success)
|
||||
return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0);
|
||||
MakeDetailData(pathToTemp, currentDirection);
|
||||
if (pathToTemp.DetailedPath.Count > 1)
|
||||
pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
|
||||
|
||||
// path1 + pathToTemp 합치기
|
||||
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp);
|
||||
|
||||
// 대체노드 → 교차로 경로 계산 (역방향)
|
||||
var pathFromTemp = _basicPathfinder.FindPath(tempNode.NodeId, JunctionInPath.NodeId);
|
||||
if (!pathFromTemp.Success)
|
||||
return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0);
|
||||
MakeDetailData(pathFromTemp, ReverseDirection);
|
||||
|
||||
// (path1 + pathToTemp) + pathFromTemp 합치기
|
||||
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp);
|
||||
|
||||
// (path1 + pathToTemp + pathFromTemp) + path2 합치기
|
||||
combinedResult = _basicPathfinder.CombineResults(combinedResult, path2);
|
||||
|
||||
MakeMagnetDirection(combinedResult);
|
||||
|
||||
return combinedResult;
|
||||
|
||||
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
|
||||
/// </summary>
|
||||
/// <param name="path1"></param>
|
||||
/// <param name="currentDirection"></param>
|
||||
private void MakeDetailData(AGVPathResult path1, AgvDirection currentDirection)
|
||||
{
|
||||
if (path1.Success && path1.Path != null && path1.Path.Count > 0)
|
||||
{
|
||||
var detailedPath1 = new List<NodeMotorInfo>();
|
||||
for (int i = 0; i < path1.Path.Count; i++)
|
||||
{
|
||||
string nodeId = path1.Path[i];
|
||||
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1] : null;
|
||||
|
||||
// 노드 정보 생성 (현재 방향 유지)
|
||||
var nodeInfo = new NodeMotorInfo(
|
||||
nodeId,
|
||||
currentDirection,
|
||||
nextNodeId,
|
||||
MagnetDirection.Straight
|
||||
);
|
||||
|
||||
detailedPath1.Add(nodeInfo);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
|
||||
// path1에 상세 경로 정보 설정
|
||||
path1.DetailedPath = detailedPath1;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드 도킹 방향에 따른 필요한 AGV 방향 반환
|
||||
/// Path에 등록된 방향을 확인하여 마그넷정보를 업데이트 합니다
|
||||
/// </summary>
|
||||
private AgvDirection? GetRequiredDockingDirection(DockingDirection dockDirection)
|
||||
/// <param name="path1"></param>
|
||||
private void MakeMagnetDirection(AGVPathResult path1)
|
||||
{
|
||||
switch (dockDirection)
|
||||
if (path1.Success && path1.DetailedPath != null && path1.DetailedPath.Count > 0)
|
||||
{
|
||||
case DockingDirection.Forward:
|
||||
return AgvDirection.Forward; // 전진 도킹
|
||||
case DockingDirection.Backward:
|
||||
return AgvDirection.Backward; // 후진 도킹
|
||||
case DockingDirection.DontCare:
|
||||
default:
|
||||
return null; // 도킹 방향 상관없음
|
||||
for (int i = 0; i < path1.DetailedPath.Count; i++)
|
||||
{
|
||||
var detailPath = path1.DetailedPath[i];
|
||||
string nodeId = path1.Path[i];
|
||||
string nextNodeId = (i + 1 < path1.Path.Count) ? path1.Path[i + 1] : null;
|
||||
|
||||
// 마그넷 방향 계산 (3개 이상 연결된 교차로에서만 좌/우 가중치 적용)
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = path1.Path[i - 1];
|
||||
if (path1.DetailedPath[i - 1].MotorDirection != detailPath.MotorDirection)
|
||||
detailPath.MagnetDirection = MagnetDirection.Straight;
|
||||
else
|
||||
detailPath.MagnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, nodeId, nextNodeId, detailPath.MotorDirection);
|
||||
}
|
||||
else detailPath.MagnetDirection = MagnetDirection.Straight;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 통합 경로 계획 (직접 경로 또는 방향 전환 경로)
|
||||
/// </summary>
|
||||
private AGVPathResult PlanPath(MapNode startNode, MapNode targetNode, MapNode prevNode, AgvDirection? requiredDirection = null, AgvDirection currentDirection = AgvDirection.Forward)
|
||||
{
|
||||
|
||||
bool needDirectionChange = requiredDirection.HasValue && (currentDirection != requiredDirection.Value);
|
||||
|
||||
//현재 위치에서 목적지까지의 최단 거리 모록을 찾는다.
|
||||
var DirectPathResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
|
||||
//이전 위치에서 목적지까지의 최단 거리를 모록을 찾는다.
|
||||
var DirectPathResultP = _basicPathfinder.FindPath(prevNode.NodeId, targetNode.NodeId);
|
||||
|
||||
//
|
||||
if (DirectPathResultP.Path.Contains(startNode.NodeId))
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (needDirectionChange)
|
||||
{
|
||||
// 방향 전환 경로 계획
|
||||
var directionChangePlan = _directionChangePlanner.PlanDirectionChange(
|
||||
startNode.NodeId, targetNode.NodeId, currentDirection, requiredDirection.Value);
|
||||
|
||||
if (!directionChangePlan.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(directionChangePlan.ErrorMessage, 0, 0);
|
||||
}
|
||||
|
||||
var detailedPath = ConvertDirectionChangePath(directionChangePlan, currentDirection, requiredDirection.Value);
|
||||
float totalDistance = CalculatePathDistance(detailedPath);
|
||||
|
||||
return AGVPathResult.CreateSuccess(
|
||||
detailedPath,
|
||||
totalDistance,
|
||||
0,
|
||||
0,
|
||||
directionChangePlan.PlanDescription,
|
||||
true,
|
||||
directionChangePlan.DirectionChangeNode
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
// 직접 경로 계획
|
||||
var basicResult = _basicPathfinder.FindPath(startNode.NodeId, targetNode.NodeId);
|
||||
|
||||
if (!basicResult.Success)
|
||||
{
|
||||
return AGVPathResult.CreateFailure(basicResult.ErrorMessage, basicResult.CalculationTimeMs, basicResult.ExploredNodeCount);
|
||||
}
|
||||
|
||||
var detailedPath = ConvertToDetailedPath(basicResult.Path, currentDirection);
|
||||
|
||||
return AGVPathResult.CreateSuccess(
|
||||
detailedPath,
|
||||
basicResult.TotalDistance,
|
||||
basicResult.CalculationTimeMs,
|
||||
basicResult.ExploredNodeCount,
|
||||
"직접 경로 - 방향 전환 불필요"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
@@ -174,7 +280,7 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = simplePath[i - 1];
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId, currentDirection);
|
||||
}
|
||||
|
||||
// 노드 정보 생성
|
||||
@@ -198,60 +304,6 @@ namespace AGVNavigationCore.PathFinding.Planning
|
||||
return detailedPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 경로를 상세 경로로 변환
|
||||
/// </summary>
|
||||
private List<NodeMotorInfo> ConvertDirectionChangePath(DirectionChangePlanner.DirectionChangePlan plan, AgvDirection startDirection, AgvDirection endDirection)
|
||||
{
|
||||
var detailedPath = new List<NodeMotorInfo>();
|
||||
var currentDirection = startDirection;
|
||||
|
||||
for (int i = 0; i < plan.DirectionChangePath.Count; i++)
|
||||
{
|
||||
string currentNodeId = plan.DirectionChangePath[i];
|
||||
string nextNodeId = (i + 1 < plan.DirectionChangePath.Count) ? plan.DirectionChangePath[i + 1] : null;
|
||||
|
||||
// 방향 전환 노드에서 방향 변경
|
||||
if (currentNodeId == plan.DirectionChangeNode && currentDirection != endDirection)
|
||||
{
|
||||
currentDirection = endDirection;
|
||||
}
|
||||
|
||||
// 마그넷 방향 계산
|
||||
MagnetDirection magnetDirection = MagnetDirection.Straight;
|
||||
if (i > 0 && nextNodeId != null)
|
||||
{
|
||||
string prevNodeId = plan.DirectionChangePath[i - 1];
|
||||
magnetDirection = _junctionAnalyzer.GetRequiredMagnetDirection(prevNodeId, currentNodeId, nextNodeId);
|
||||
}
|
||||
|
||||
// 특수 동작 확인
|
||||
bool requiresSpecialAction = false;
|
||||
string specialActionDescription = "";
|
||||
|
||||
if (currentNodeId == plan.DirectionChangeNode)
|
||||
{
|
||||
requiresSpecialAction = true;
|
||||
specialActionDescription = $"방향전환: {startDirection} → {endDirection}";
|
||||
}
|
||||
|
||||
// 노드 정보 생성
|
||||
var nodeMotorInfo = new NodeMotorInfo(
|
||||
currentNodeId,
|
||||
currentDirection,
|
||||
nextNodeId,
|
||||
true, // 방향 전환 경로의 경우 회전 가능으로 설정
|
||||
currentNodeId == plan.DirectionChangeNode,
|
||||
magnetDirection,
|
||||
requiresSpecialAction,
|
||||
specialActionDescription
|
||||
);
|
||||
|
||||
detailedPath.Add(nodeMotorInfo);
|
||||
}
|
||||
|
||||
return detailedPath;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 총 거리 계산
|
||||
|
||||
@@ -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.NodeId) && n.NodeId != currentNode.NodeId
|
||||
).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].NodeId; // 첫 번째 연결 노드 반환
|
||||
}
|
||||
|
||||
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.NodeId;
|
||||
}
|
||||
|
||||
/// <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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,197 @@
|
||||
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.agvmap을 로드하여 방향별 다음 노드를 검증
|
||||
/// </summary>
|
||||
public class DirectionalPathfinderTest
|
||||
{
|
||||
private List<MapNode> _allNodes;
|
||||
private Dictionary<string, MapNode> _nodesByRfidId;
|
||||
private AGVDirectionCalculator _calculator;
|
||||
|
||||
public DirectionalPathfinderTest()
|
||||
{
|
||||
_nodesByRfidId = new Dictionary<string, MapNode>();
|
||||
_calculator = new AGVDirectionCalculator();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// NewMap.agvmap 파일 로드
|
||||
/// </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 (!string.IsNullOrEmpty(node.RfidId))
|
||||
{
|
||||
_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(string previousRfidId, string 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.NodeId} (RFID: {previousNode.RfidId}) - 위치: {previousNode.Position}");
|
||||
Console.WriteLine($"현재 노드: {currentNode.NodeId} (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.NodeId == nextNodeId);
|
||||
if (nextNode != null)
|
||||
{
|
||||
Console.WriteLine($"✓ 다음 노드: {nextNode.NodeId} (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.NodeId} ({GetNodeTypeName(node.Type)})");
|
||||
Console.WriteLine($" 위치: {node.Position}, 연결: {string.Join(", ", node.ConnectedNodes)}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 RFID 노드의 상세 정보 출력
|
||||
/// </summary>
|
||||
public void PrintNodeInfo(string rfidId)
|
||||
{
|
||||
if (!_nodesByRfidId.TryGetValue(rfidId, out var node))
|
||||
{
|
||||
Console.WriteLine($"노드를 찾을 수 없습니다: {rfidId}");
|
||||
return;
|
||||
}
|
||||
|
||||
Console.WriteLine($"\n========== RFID {rfidId} 상세 정보 ==========");
|
||||
Console.WriteLine($"노드 ID: {node.NodeId}");
|
||||
Console.WriteLine($"이름: {node.Name}");
|
||||
Console.WriteLine($"위치: {node.Position}");
|
||||
Console.WriteLine($"타입: {GetNodeTypeName(node.Type)}");
|
||||
Console.WriteLine($"회전 가능: {node.CanRotate}");
|
||||
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.NodeId == 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; }
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -20,7 +20,7 @@ namespace AGVNavigationCore.Utils
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
/// <param name="currentDirection">AGV 현재 방향</param>
|
||||
/// <returns>도킹 검증 결과</returns>
|
||||
public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes, AgvDirection currentDirection)
|
||||
public static DockingValidationResult ValidateDockingDirection(AGVPathResult pathResult, List<MapNode> mapNodes)
|
||||
{
|
||||
// 경로가 없거나 실패한 경우
|
||||
if (pathResult == null || !pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
|
||||
@@ -31,53 +31,59 @@ namespace AGVNavigationCore.Utils
|
||||
|
||||
// 목적지 노드 찾기
|
||||
string targetNodeId = pathResult.Path[pathResult.Path.Count - 1];
|
||||
var targetNode = mapNodes?.FirstOrDefault(n => n.NodeId == targetNodeId);
|
||||
var LastNode = mapNodes?.FirstOrDefault(n => n.NodeId == targetNodeId);
|
||||
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {targetNodeId}");
|
||||
|
||||
if (targetNode == null)
|
||||
if (LastNode == null)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드 찾을 수 없음: {targetNodeId}");
|
||||
return DockingValidationResult.CreateNotRequired();
|
||||
}
|
||||
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드 타입: {targetNode.Type} ({(int)targetNode.Type})");
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 목적지 노드: {targetNodeId} 타입:{LastNode.Type} ({(int)LastNode.Type})");
|
||||
|
||||
// 도킹이 필요한 노드인지 확인 (DockDirection이 DontCare가 아닌 경우)
|
||||
if (!IsDockingRequired(targetNode.DockDirection))
|
||||
if (LastNode.DockDirection == DockingDirection.DontCare)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {targetNode.DockDirection}");
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 도킹 불필요: {LastNode.DockDirection}");
|
||||
return DockingValidationResult.CreateNotRequired();
|
||||
}
|
||||
|
||||
// 필요한 도킹 방향 확인
|
||||
var requiredDirection = GetRequiredDockingDirection(targetNode.DockDirection);
|
||||
var requiredDirection = GetRequiredDockingDirection(LastNode.DockDirection);
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 필요한 도킹 방향: {requiredDirection}");
|
||||
|
||||
// 경로 기반 최종 방향 계산
|
||||
var calculatedDirection = CalculateFinalDirection(pathResult.Path, mapNodes, currentDirection);
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] 계산된 최종 방향: {calculatedDirection}");
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] AGV 현재 방향: {currentDirection}");
|
||||
var LastNodeInfo = pathResult.DetailedPath.Last();
|
||||
if (LastNodeInfo.NodeId != LastNode.NodeId)
|
||||
{
|
||||
string error = $"마지막 노드의 도킹방향과 경로정보의 노드ID 불일치: 필요={LastNode.NodeId}, 계산됨={LastNodeInfo.NodeId }";
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
|
||||
return DockingValidationResult.CreateInvalid(
|
||||
targetNodeId,
|
||||
LastNode.Type,
|
||||
requiredDirection,
|
||||
LastNodeInfo.MotorDirection,
|
||||
error);
|
||||
}
|
||||
|
||||
// 검증 수행
|
||||
if (calculatedDirection == requiredDirection)
|
||||
if (LastNodeInfo.MotorDirection == requiredDirection)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ✅ 도킹 검증 성공");
|
||||
return DockingValidationResult.CreateValid(
|
||||
targetNodeId,
|
||||
targetNode.Type,
|
||||
LastNode.Type,
|
||||
requiredDirection,
|
||||
calculatedDirection);
|
||||
LastNodeInfo.MotorDirection);
|
||||
}
|
||||
else
|
||||
{
|
||||
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(calculatedDirection)}";
|
||||
string error = $"도킹 방향 불일치: 필요={GetDirectionText(requiredDirection)}, 계산됨={GetDirectionText(LastNodeInfo.MotorDirection)}";
|
||||
System.Diagnostics.Debug.WriteLine($"[DockingValidator] ❌ 도킹 검증 실패: {error}");
|
||||
return DockingValidationResult.CreateInvalid(
|
||||
targetNodeId,
|
||||
targetNode.Type,
|
||||
LastNode.Type,
|
||||
requiredDirection,
|
||||
calculatedDirection,
|
||||
LastNodeInfo.MotorDirection,
|
||||
error);
|
||||
}
|
||||
}
|
||||
|
||||
342
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/GetNextNodeIdTest.cs
Normal file
342
Cs_HMI/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 { NodeId = "N001", RfidId = "001", Position = new Point(65, 229), ConnectedNodes = new List<string> { "N002" } };
|
||||
var node002 = new MapNode { NodeId = "N002", RfidId = "002", Position = new Point(206, 244), ConnectedNodes = new List<string> { "N001", "N003" } };
|
||||
var node003 = new MapNode { NodeId = "N003", RfidId = "003", Position = new Point(278, 278), ConnectedNodes = new List<string> { "N002", "N004" } };
|
||||
var node004 = new MapNode { NodeId = "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 ?? "?"})");
|
||||
Console.WriteLine($"현재 노드: {currentNode.NodeId} (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.NodeId})의 ConnectedNodes: {string.Join(", ", currentNode.ConnectedNodes)}");
|
||||
Console.WriteLine($"가능한 다음 노드들:");
|
||||
|
||||
var candidateNodes = allNodes.Where(n =>
|
||||
currentNode.ConnectedNodes.Contains(n.NodeId) && n.NodeId != currentNode.NodeId
|
||||
).ToList();
|
||||
|
||||
foreach (var candidate in candidateNodes)
|
||||
{
|
||||
var score = CalculateScoreAndPrint(movementVector, currentNode.Position, candidate, direction);
|
||||
string isExpected = (candidate.NodeId == expectedNextNode.NodeId) ? " ← 예상 노드" : "";
|
||||
Console.WriteLine($" {candidate.NodeId} (RFID: {candidate.RfidId}) - 위치: {candidate.Position} - 점수: {score:F1}{isExpected}");
|
||||
}
|
||||
|
||||
// 최고 점수 노드 선택
|
||||
var bestCandidate = GetBestCandidate(movementVector, currentNode.Position, candidateNodes, direction);
|
||||
|
||||
Console.WriteLine($"\n✓ 선택된 노드: {bestCandidate.NodeId} (RFID: {bestCandidate.RfidId})");
|
||||
|
||||
if (bestCandidate.NodeId == expectedNextNode.NodeId)
|
||||
{
|
||||
Console.WriteLine($"✅ 정답! ({expectedNodeIdStr})");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine($"❌ 오답! 예상: {expectedNextNode.NodeId}, 실제: {bestCandidate.NodeId}");
|
||||
}
|
||||
}
|
||||
|
||||
/// <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
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/ImageConverterUtil.cs
Normal file
153
Cs_HMI/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);
|
||||
}
|
||||
}
|
||||
}
|
||||
56
Cs_HMI/AGVLogic/AGVNavigationCore/Utils/TestRunner.cs
Normal file
56
Cs_HMI/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.agvmap";
|
||||
|
||||
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=== 테스트 완료 ===");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user