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

..

Browse Source
This commit is contained in:
backuppc
2026-01-12 17:37:37 +09:00
parent 880dc526da
commit 5801137d63
12 changed files with 356 additions and 1385 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
Cs_HMI/AGVLogic/AGVMapEditor/Forms/MainForm.cs
View File

@@ -35,10 +35,8 @@ namespace AGVMapEditor.Forms
public class NodeConnectionInfo
{
public string FromNodeId { get; set; }
public string FromNodeName { get; set; }
public ushort FromRfidId { get; set; }
public string ToNodeId { get; set; }
public string ToNodeName { get; set; }
public ushort ToRfidId { get; set; }
public string ConnectionType { get; set; }
@@ -46,12 +44,12 @@ namespace AGVMapEditor.Forms
{
// RFID가 있으면 RFID(노드이름), 없으면 NodeID(노드이름) 형태로 표시
string fromDisplay = FromRfidId > 0
? $"{FromRfidId}({FromNodeName})"
: $"---({FromNodeId})";
? $"{FromRfidId:0000}(*{FromNodeId.PadLeft(4,'0')})"
: $"(*{FromNodeId})";
string toDisplay = ToRfidId > 0
? $"{ToRfidId}({ToNodeName})"
: $"---({ToNodeId})";
? $"{ToRfidId:0000}(*{ToNodeId.PadLeft(4, '0')})"
: $"(*{ToNodeId})";
// 양방향 연결은 ↔ 기호 사용
string arrow = ConnectionType == "양방향" ? "↔" : "→";
@@ -828,7 +826,7 @@ namespace AGVMapEditor.Forms
var item = node as MapNode;
if (item.StationType == StationType.Normal)
foreColor = Color.DimGray;
else if (item.StationType == StationType.Charger)
else if (item.StationType == StationType.Charger1 || item.StationType == StationType.Charger2)
foreColor = Color.Red;
else
foreColor = Color.DarkGreen;
@@ -907,10 +905,8 @@ namespace AGVMapEditor.Forms
connections.Add(new NodeConnectionInfo
{
FromNodeId = firstNode.Id,
FromNodeName = "",
FromRfidId = firstNode.RfidId,
ToNodeId = secondNode.Id,
ToNodeName = "",
ToRfidId = secondNode.RfidId,
ConnectionType = "양방향" // 모든 연결이 양방향
});
@@ -923,6 +919,7 @@ namespace AGVMapEditor.Forms
}
// 리스트박스에 표시
lstNodeConnection.Font = new Font("돋움체", 10);
lstNodeConnection.DataSource = null;
lstNodeConnection.DataSource = connections;
lstNodeConnection.DisplayMember = "ToString";

13
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Events.cs
View File

@@ -594,7 +594,7 @@ namespace AGVNavigationCore.Controls
{
if (junctionNode == null) return;
int radius = isGateway ? 35 : 25; // 게이트웨이는 좀 더 크게
int radius = isGateway ? 23 : 18; // 게이트웨이는 좀 더 크게
// 색상 결정: Gateway=진한 주황/골드, 일반 교차로=기존 파랑
Color fillColor = isGateway ? Color.FromArgb(100, 255, 140, 0) : Color.FromArgb(80, 70, 130, 200);
@@ -674,7 +674,7 @@ namespace AGVNavigationCore.Controls
{
case NodeType.Normal:
var item = _selectedNode as MapNode;
if (item.StationType == StationType.Charger)
if ( item.StationType == StationType.Charger1 || item.StationType == StationType.Charger2)
DrawTriangleGhost(g, ghostBrush);
else
DrawPentagonGhost(g, ghostBrush);
@@ -869,7 +869,8 @@ namespace AGVNavigationCore.Controls
case StationType.Buffer:
DrawPentagonNodeShape(g, node, brush);
break;
case StationType.Charger:
case StationType.Charger1:
case StationType.Charger2:
DrawTriangleNodeShape(g, node, brush);
break;
case StationType.Limit:
@@ -1310,7 +1311,8 @@ namespace AGVNavigationCore.Controls
Color bgColor = Color.White;
switch (node.StationType)
{
case StationType.Charger:
case StationType.Charger1:
case StationType.Charger2:
fgColor = Color.White;
bgColor = Color.Tomato;
break;
@@ -1607,7 +1609,8 @@ namespace AGVNavigationCore.Controls
switch (node.StationType)
{
case StationType.Normal: bgColor = Color.DeepSkyBlue; break;
case StationType.Charger: bgColor = Color.Tomato; break;
case StationType.Charger1: bgColor = Color.Tomato; break;
case StationType.Charger2: bgColor = Color.Tomato; break;
case StationType.Loader:
case StationType.UnLoader: bgColor = Color.Gold; break;
case StationType.Clearner: bgColor = Color.DeepSkyBlue; break;

3
Cs_HMI/AGVLogic/AGVNavigationCore/Controls/UnifiedAGVCanvas.Mouse.cs
View File

@@ -492,7 +492,8 @@ namespace AGVNavigationCore.Controls
case StationType.Clearner:
case StationType.Buffer:
return IsPointInPentagon(point, node);
case StationType.Charger:
case StationType.Charger2:
case StationType.Charger1:
return IsPointInTriangle(point, node);
default:
return IsPointInCircle(point, node);

6
Cs_HMI/AGVLogic/AGVNavigationCore/Models/Enums.cs
View File

@@ -72,8 +72,10 @@ namespace AGVNavigationCore.Models
UnLoader,
/// <summary>버퍼</summary>
Buffer,
/// <summary>충전기</summary>
Charger,
/// <summary>충전기1</summary>
Charger1,
/// <summary>충전기2</summary>
Charger2,
/// <summary>
/// 끝점(더이상 이동불가)

13
Cs_HMI/AGVLogic/AGVNavigationCore/Models/MapNode.cs
View File

@@ -30,7 +30,8 @@ namespace AGVNavigationCore.Models
if (StationType == StationType.Loader) return true;
if (StationType == StationType.UnLoader) return true;
if (StationType == StationType.Clearner) return true;
if (StationType == StationType.Charger) return true;
if (StationType == StationType.Charger1) return true;
if (StationType == StationType.Charger2) return true;
return false;
}
}
@@ -115,7 +116,7 @@ namespace AGVNavigationCore.Models
{
get
{
if (StationType == StationType.Charger || StationType == StationType.Buffer ||
if (StationType == StationType.Charger1 || StationType == StationType.Charger2 || StationType == StationType.Buffer ||
StationType == StationType.Clearner || StationType == StationType.Loader ||
StationType == StationType.UnLoader) return true;
return false;
@@ -141,10 +142,10 @@ namespace AGVNavigationCore.Models
public void SetChargingStation(string stationId)
{
StationType = StationType.Charger;
Id = stationId;
DockDirection = DockingDirection.Forward;
ModifiedDate = DateTime.Now;
//StationType = StationType.Charger;
//Id = stationId;
//DockDirection = DockingDirection.Forward;
//ModifiedDate = DateTime.Now;
}
public override string ToString()

327
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
View File

@@ -101,31 +101,32 @@ namespace AGVNavigationCore.PathFinding.Core
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="endNodeId">목적지 노드 ID</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindPathAStar(string startNodeId, string endNodeId)
public AGVPathResult FindPathAStar(MapNode start, MapNode end)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
if (!_nodeMap.ContainsKey(startNodeId))
if (!_nodeMap.ContainsKey(start.Id))
{
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds, 0);
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {start.Id}", stopwatch.ElapsedMilliseconds, 0);
}
if (!_nodeMap.ContainsKey(endNodeId))
if (!_nodeMap.ContainsKey(end.Id))
{
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds, 0);
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {end.Id}", stopwatch.ElapsedMilliseconds, 0);
}
if (startNodeId == endNodeId)
//출발지와 목적지가 동일한 경우
if (start.Id == end.Id)
{
var startMapNode = GetMapNode(startNodeId);
var singlePath = new List<MapNode> { startMapNode };
//var startMapNode = GetMapNode(start);
var singlePath = new List<MapNode> { start };
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
}
var startNode = _nodeMap[startNodeId];
var endNode = _nodeMap[endNodeId];
var startNode = _nodeMap[start.Id];
var endNode = _nodeMap[end.Id];
var openSet = new List<PathNode>();
var closedSet = new HashSet<string>();
var exploredCount = 0;
@@ -142,7 +143,7 @@ namespace AGVNavigationCore.PathFinding.Core
closedSet.Add(currentNode.NodeId);
exploredCount++;
if (currentNode.NodeId == endNodeId)
if (currentNode.NodeId == end.Id)
{
var path = ReconstructPath(currentNode);
var totalDistance = CalculatePathDistance(path);
@@ -180,157 +181,157 @@ 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();
///// <summary>
///// 경유지를 거쳐 경로 찾기 (오버로드)
///// 여러 경유지를 순차적으로 거쳐서 최종 목적지까지의 경로를 계산합니다.
///// 기존 FindPath를 여러 번 호출하여 각 구간의 경로를 합칩니다.
///// </summary>
///// <param name="startNodeId">시작 노드 ID</param>
///// <param name="endNodeId">최종 목적지 노드 ID</param>
///// <param name="waypointNodeIds">경유지 노드 ID 배열 (선택사항)</param>
///// <returns>경로 계산 결과 (모든 경유지를 거친 전체 경로)</returns>
//public AGVPathResult FindPath(string startNodeId, string endNodeId, params string[] waypointNodeIds)
//{
// var stopwatch = System.Diagnostics.Stopwatch.StartNew();
try
{
// 경유지가 없으면 기본 FindPath 호출
if (waypointNodeIds == null || waypointNodeIds.Length == 0)
{
return FindPathAStar(startNodeId, endNodeId);
}
// try
// {
// // 경유지가 없으면 기본 FindPath 호출
// if (waypointNodeIds == null || waypointNodeIds.Length == 0)
// {
// return FindPathAStar(startNodeId, endNodeId);
// }
// 경유지 유효성 검증
var validWaypoints = new List<string>();
foreach (var waypointId in waypointNodeIds)
{
if (string.IsNullOrEmpty(waypointId))
continue;
// // 경유지 유효성 검증
// 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);
}
// if (!_nodeMap.ContainsKey(waypointId))
// {
// return AGVPathResult.CreateFailure($"경유지 노드를 찾을 수 없습니다: {waypointId}", stopwatch.ElapsedMilliseconds, 0);
// }
validWaypoints.Add(waypointId);
}
// validWaypoints.Add(waypointId);
// }
// 경유지가 없으면 기본 경로 계산
if (validWaypoints.Count == 0)
{
return FindPathAStar(startNodeId, endNodeId);
}
// // 경유지가 없으면 기본 경로 계산
// if (validWaypoints.Count == 0)
// {
// return FindPathAStar(startNodeId, endNodeId);
// }
// 첫 번째 경유지가 시작노드와 같은지 검사
if (validWaypoints[0] == startNodeId)
{
return AGVPathResult.CreateFailure(
$"첫 번째 경유지({validWaypoints[0]})가 시작 노드({startNodeId})와 동일합니다. 경유지는 시작노드와 달라야 합니다.",
stopwatch.ElapsedMilliseconds, 0);
}
// // 첫 번째 경유지가 시작노드와 같은지 검사
// 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);
}
// // 마지막 경유지가 목적지노드와 같은지 검사
// 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;
// // 연속된 중복만 제거 (순서 유지)
// // 예: [1, 2, 2, 3, 2] -> [1, 2, 3, 2] (연속 중복만 제거)
// var deduplicatedWaypoints = new List<string>();
// string lastWaypoint = null;
// foreach (var waypoint in validWaypoints)
// {
// if (waypoint != lastWaypoint)
// {
// deduplicatedWaypoints.Add(waypoint);
// lastWaypoint = waypoint;
// }
// }
// validWaypoints = deduplicatedWaypoints;
// 최종 경로 리스트와 누적 값
var combinedPath = new List<MapNode>();
float totalDistance = 0;
long totalCalculationTime = 0;
// // 최종 경로 리스트와 누적 값
// var combinedPath = new List<MapNode>();
// float totalDistance = 0;
// long totalCalculationTime = 0;
// 현재 시작점
string currentStart = startNodeId;
// // 현재 시작점
// string currentStart = startNodeId;
// 1단계: 각 경유지까지의 경로 계산
for (int i = 0; i < validWaypoints.Count; i++)
{
string waypoint = validWaypoints[i];
// // 1단계: 각 경유지까지의 경로 계산
// for (int i = 0; i < validWaypoints.Count; i++)
// {
// string waypoint = validWaypoints[i];
// 현재 위치에서 경유지까지의 경로 계산
var segmentResult = FindPathAStar(currentStart, waypoint);
// // 현재 위치에서 경유지까지의 경로 계산
// var segmentResult = FindPathAStar(currentStart, waypoint);
if (!segmentResult.Success)
{
return AGVPathResult.CreateFailure(
$"경유지 {i + 1}({waypoint})까지의 경로 계산 실패: {segmentResult.ErrorMessage}",
stopwatch.ElapsedMilliseconds, 0);
}
// 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);
}
// // 경로 합치기 (첫 번째 구간이 아니면 시작점 제거하여 중복 방지)
// 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;
// totalDistance += segmentResult.TotalDistance;
// totalCalculationTime += segmentResult.CalculationTimeMs;
// 다음 경유지의 시작점은 현재 경유지
currentStart = waypoint;
}
// // 다음 경유지의 시작점은 현재 경유지
// currentStart = waypoint;
// }
// 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
var finalSegmentResult = FindPathAStar(currentStart, endNodeId);
// // 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
// var finalSegmentResult = FindPathAStar(currentStart, endNodeId);
if (!finalSegmentResult.Success)
{
return AGVPathResult.CreateFailure(
$"최종 목적지까지의 경로 계산 실패: {finalSegmentResult.ErrorMessage}",
stopwatch.ElapsedMilliseconds, 0);
}
// 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);
}
// // 최종 경로 합치기 (시작점 제거)
// 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;
// totalDistance += finalSegmentResult.TotalDistance;
// totalCalculationTime += finalSegmentResult.CalculationTimeMs;
stopwatch.Stop();
// stopwatch.Stop();
// 결과 생성
return AGVPathResult.CreateSuccess(
combinedPath,
new List<AgvDirection>(),
totalDistance,
totalCalculationTime
);
}
catch (Exception ex)
{
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
}
}
// // 결과 생성
// return AGVPathResult.CreateSuccess(
// combinedPath,
// new List<AgvDirection>(),
// totalDistance,
// totalCalculationTime
// );
// }
// catch (Exception ex)
// {
// return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
// }
//}
/// <summary>
/// 두 경로 결과를 합치기
@@ -425,31 +426,31 @@ namespace AGVNavigationCore.PathFinding.Core
}
/// <summary>
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
/// </summary>
/// <param name="startNodeId">시작 노드 ID</param>
/// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
/// <returns>경로 계산 결과</returns>
public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
{
if (targetNodeIds == null || targetNodeIds.Count == 0)
{
return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
}
///// <summary>
///// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
///// </summary>
///// <param name="startNodeId">시작 노드 ID</param>
///// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
///// <returns>경로 계산 결과</returns>
//public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
//{
// if (targetNodeIds == null || targetNodeIds.Count == 0)
// {
// return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
// }
AGVPathResult bestResult = null;
foreach (var targetId in targetNodeIds)
{
var result = FindPathAStar(startNodeId, targetId);
if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
{
bestResult = result;
}
}
// AGVPathResult bestResult = null;
// foreach (var targetId in targetNodeIds)
// {
// var result = FindPathAStar(startNodeId, targetId);
// if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
// {
// bestResult = result;
// }
// }
return bestResult ?? AGVPathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
}
// return bestResult ?? AGVPathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
//}
/// <summary>
/// 휴리스틱 거리 계산 (유클리드 거리)

322
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/AGVPathfinder.cs
View File

@@ -111,329 +111,13 @@ namespace AGVNavigationCore.PathFinding.Planning
return null;
}
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode)
{
// 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
return FindPath(startNode, targetNode, startNode, AgvDirection.Forward, AgvDirection.Forward, false);
}
public AGVPathResult FindPathAStar(MapNode startNode, MapNode targetNode)
{
// 기본값으로 경로 탐색 (이전 위치 = 현재 위치, 방향 = 전진)
return _basicPathfinder.FindPathAStar(startNode.Id, targetNode.Id);
return _basicPathfinder.FindPathAStar(startNode, targetNode);
}
public AGVPathResult FindPath(MapNode startNode, MapNode targetNode,
MapNode prevNode, AgvDirection prevDirection, AgvDirection currentDirection, bool crossignore = false)
{
// 입력 검증
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 (targetNode.isDockingNode == false && targetNode.Type != NodeType.Normal)
return AGVPathResult.CreateFailure("이동 가능한 노드가 아닙니다", 0, 0);
var tnode = targetNode as MapNode;
//시작노드와 종료노드가 동일위치이고 도킹방향도 맞다면 그대로 OK 한다
if (startNode.Id == targetNode.Id && tnode.DockDirection.MatchAGVDirection(prevDirection))
return AGVPathResult.CreateSuccess(new List<MapNode> { startNode, startNode }, new List<AgvDirection>(), 0, 0);
//반대방향값 지정
var ReverseDirection = (currentDirection == AgvDirection.Forward ? AgvDirection.Backward : AgvDirection.Forward);
//1.목적지까지의 최단거리 경로를 찾는다.(AStar 방식)
var pathResult = _basicPathfinder.FindPathAStar(startNode.Id, targetNode.Id);
pathResult.PrevNode = prevNode;
pathResult.PrevDirection = prevDirection;
if (!pathResult.Success || pathResult.Path == null || pathResult.Path.Count == 0)
return AGVPathResult.CreateFailure("각 노드간 최단 경로 계산이 실패되었습니다", 0, 0);
//정방향/역방향 이동 시 다음 노드 확인
// 경로 계획 단계에서는 마그넷 방향이 미리 알려지지 않으므로 Straight로 기본값 사용
// ✅ 현재 방향 유지: prevDirection = currentDirection (방향 일관성)
var nextNodeForward = DirectionalHelper.GetNextNodeByDirection(
startNode, prevNode, currentDirection, currentDirection, MagnetDirection.Straight, _mapNodes);
// ✅ 방향 전환: prevDirection = currentDirection, direction = ReverseDirection
var nextNodeBackward = DirectionalHelper.GetNextNodeByDirection(
startNode, prevNode, currentDirection, ReverseDirection, MagnetDirection.Straight, _mapNodes);
//2.AGV방향과 목적지에 설정된 방향이 일치하면 그대로 진행하면된다.(목적지에 방향이 없는 경우에도 그대로 진행)
if (tnode.DockDirection == DockingDirection.DontCare ||
(tnode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
(tnode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
{
if ((nextNodeForward?.Id ?? string.Empty) == pathResult.Path[1].Id) //예측경로와 다음진행방향 경로가 일치하면 해당 방향이 맞다
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
for (int i = 0; i < pathResult.DetailedPath.Count; i++)
pathResult.DetailedPath[i].seq = i + 1;
return pathResult;
}
}
//2-1 현재위치의 반대방향과 대상의 방향이 맞는 경우에도 그대로 사용가능하다.
//if (targetNode.DockDirection == DockingDirection.DontCare ||
// (targetNode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Backward) ||
// (targetNode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Forward))
//{
// // 반대 방향으로 이동하여 목적지에 진입해야 함
// // 현재 방향으로 가면서 역방향으로 도킹
// MakeDetailData(pathResult, ReverseDirection);
// MakeMagnetDirection(pathResult);
// return pathResult;
//}
//뒤로 이동시 경로상의 처음 만나는 노드가 같다면 그 방향으로 이동하면 된다.
// ⚠️ 단, 현재 방향과 목적지 도킹 방향이 일치해야 함!
if (nextNodeBackward != null && pathResult.Path.Count > 1 &&
nextNodeBackward.Id == pathResult.Path[1].Id) // ✅ 추가: 현재도 Backward여야 함
{
if (tnode.DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward ||
tnode.DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward)
{
MakeDetailData(pathResult, ReverseDirection);
MakeMagnetDirection(pathResult);
for (int i = 0; i < pathResult.DetailedPath.Count; i++)
pathResult.DetailedPath[i].seq = i + 1;
return pathResult;
}
}
if (nextNodeForward != null && pathResult.Path.Count > 1 &&
nextNodeForward.Id == pathResult.Path[1].Id) // ✅ 추가: 현재도 Forward여야 함
{
if (tnode.DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward ||
tnode.DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward)
{
MakeDetailData(pathResult, currentDirection);
MakeMagnetDirection(pathResult);
for (int i = 0; i < pathResult.DetailedPath.Count; i++)
pathResult.DetailedPath[i].seq = i + 1;
return pathResult;
}
}
//if(nextNodeForward.NodeId == pathResult.Path[1])
//{
// MakeDetailData(pathResult, currentDirection);
// MakeMagnetDirection(pathResult);
// return pathResult;
//}
//현재 내 포인트가 교차로라면.. 무조건 왓던 방향 혹은 그 반대방향으로 이동해서 경로를 계산해야한다.
//교차로에 멈춰있을때에는 바로 방향전환을 할 수없으니. 정/역(straight)로 이동해서 다시 계산을 해야한다
if (crossignore == false && startNode.ConnectedNodes.Count > 2)
{
//진행방향으로 이동했을때 나오는 노드를 사용한다.
if (nextNodeForward != null)
{
var Path0 = _basicPathfinder.FindPathAStar(startNode.Id, nextNodeForward.Id);
Path0.PrevNode = prevNode;
Path0.PrevDirection = prevDirection;
MakeDetailData(Path0, prevDirection);
var Path1 = FindPath(nextNodeForward, targetNode, startNode, prevDirection, currentDirection, true);
Path1.PrevNode = startNode;
Path1.PrevDirection = prevDirection;
//MakeDetailData(Path1, ReverseDirection);
var combinedResult0 = Path0;
combinedResult0 = _basicPathfinder.CombineResults(combinedResult0, Path1);
MakeMagnetDirection(combinedResult0);
for (int i = 0; i < combinedResult0.DetailedPath.Count; i++)
combinedResult0.DetailedPath[i].seq = i + 1;
return combinedResult0;
}
else if (nextNodeBackward != null)
{
return AGVPathResult.CreateFailure("backward 처리코드가 없습니다 오류", 0, 0);
}
else
{
return AGVPathResult.CreateFailure("교차로에서 시작하는 조건중 forward/backrad 노드 검색 실패", 0, 0);
}
}
//3. 도킹방향이 일치하지 않으니 교차로에서 방향을 회전시켜야 한다
//최단거리(=minpath)경로에 속하는 교차로가 있다면 그것을 사용하고 없다면 가장 가까운 교차로를 찾는다.
var JunctionInPath = FindNearestJunctionOnPath(pathResult);
if (JunctionInPath == null)
{
//시작노드로부터 가까운 교차로 검색
JunctionInPath = FindNearestJunction(startNode);
//종료노드로부터 가까운 교차로 검색
if (JunctionInPath == null) JunctionInPath = FindNearestJunction(tnode);
}
if (JunctionInPath == null)
return AGVPathResult.CreateFailure("교차로가 없어 경로계산을 할 수 없습니다", 0, 0);
//경유지를 포함하여 경로를 다시 계산한다.
//1.시작위치 - 교차로(여기까지는 현재 방향으로 그대로 이동을 한다)
var path1 = _basicPathfinder.FindPathAStar(startNode.Id, JunctionInPath.Id);
path1.PrevNode = prevNode;
path1.PrevDirection = prevDirection;
//다음좌표를 보고 교차로가 진행방향인지 반대방향인지 체크한다.
bool ReverseCheck = false;
//if (path1.Path.Count > 1 && nextNodeForward != null && nextNodeForward.NodeId.Equals(path1.Path[1].NodeId))
//{
// ReverseCheck = false; //현재 진행 방향으로 이동해야한다
// MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
//}
if (path1.Path.Count > 1 && nextNodeBackward != null && nextNodeBackward.Id.Equals(path1.Path[1].Id))
{
ReverseCheck = true; //현재 방향의 반대방향으로 이동해야한다
MakeDetailData(path1, ReverseDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
else
{
ReverseCheck = false; //현재 진행 방향으로 이동해야한다
MakeDetailData(path1, currentDirection); // path1의 상세 경로 정보 채우기 (모터 방향 설정)
}
//2.교차로 - 종료위치
var path2 = _basicPathfinder.FindPathAStar(JunctionInPath.Id, targetNode.Id);
path2.PrevNode = prevNode;
path2.PrevDirection = prevDirection;
//2번paths느최종 목적지 이므로 목적지와 도킹방향 확인해서 결정한다
if ((path2.Path.Last().DockDirection == DockingDirection.Forward && ReverseDirection == AgvDirection.Forward) ||
(path2.Path.Last().DockDirection == DockingDirection.Backward && ReverseDirection == AgvDirection.Backward))
{
MakeDetailData(path2, ReverseDirection);
}
else if ((path2.Path.Last().DockDirection == DockingDirection.Forward && currentDirection == AgvDirection.Forward) ||
(path2.Path.Last().DockDirection == DockingDirection.Backward && currentDirection == AgvDirection.Backward))
{
MakeDetailData(path2, currentDirection);
}
MapNode tempNode = null;
//3.방향전환을 위환 대체 노드찾기
tempNode = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.Id,
path1.Path[path1.Path.Count - 2].Id,
path2.Path[1].Id);
//4. path1 + tempnode + path2 가 최종 위치가 된다.
if (tempNode == null)
return AGVPathResult.CreateFailure("방향 전환을 위한 대체 노드를 찾을 수 없습니다.", 0, 0);
// path1 (시작 → 교차로)
var combinedResult = path1;
//교차로 대체노드를 사용한 경우
//if (tempNode != null)
{
// 교차로 → 대체노드 경로 계산
var pathToTemp = _basicPathfinder.FindPathAStar(JunctionInPath.Id, tempNode.Id);
pathToTemp.PrevNode = JunctionInPath;
pathToTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
if (!pathToTemp.Success)
return AGVPathResult.CreateFailure("교차로에서 대체 노드까지의 경로를 찾을 수 없습니다.", 0, 0);
if (ReverseCheck) MakeDetailData(pathToTemp, ReverseDirection);
else MakeDetailData(pathToTemp, currentDirection);
//교차로찍고 원래방향으로 돌어가야한다.
if (pathToTemp.DetailedPath.Count > 1)
pathToTemp.DetailedPath[pathToTemp.DetailedPath.Count - 1].MotorDirection = currentDirection;
// path1 + pathToTemp 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp);
// 대체노드 → 교차로 경로 계산 (역방향)
var pathFromTemp = _basicPathfinder.FindPathAStar(tempNode.Id, JunctionInPath.Id);
pathFromTemp.PrevNode = JunctionInPath;
pathFromTemp.PrevDirection = (ReverseCheck ? ReverseDirection : currentDirection);
if (!pathFromTemp.Success)
return AGVPathResult.CreateFailure("대체 노드에서 교차로까지의 경로를 찾을 수 없습니다.", 0, 0);
if (ReverseCheck) MakeDetailData(pathFromTemp, currentDirection);
else MakeDetailData(pathFromTemp, ReverseDirection);
// (path1 + pathToTemp) + pathFromTemp 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathFromTemp);
//현재까지 노드에서 목적지까지의 방향이 일치하면 그대로 사용한다.
bool temp3ok = false;
var TempCheck3 = _basicPathfinder.FindPathAStar(combinedResult.Path.Last().Id, targetNode.Id);
if (TempCheck3.Path.First().Id.Equals(combinedResult.Path.Last().Id))
{
if (tnode.DockDirection == DockingDirection.Forward && combinedResult.DetailedPath.Last().MotorDirection == AgvDirection.Forward)
{
temp3ok = true;
}
else if (tnode.DockDirection == DockingDirection.Backward && combinedResult.DetailedPath.Last().MotorDirection == AgvDirection.Backward)
{
temp3ok = true;
}
}
//대체노드에서 최종 목적지를 다시 확인한다.
if (temp3ok == false)
{
//목적지와 방향이 맞지 않다. 그러므로 대체노드를 추가로 더 찾아야한다.
var tempNode2 = _basicPathfinder.FindAlternateNodeForDirectionChange(JunctionInPath.Id,
combinedResult.Path[combinedResult.Path.Count - 2].Id,
path2.Path[1].Id);
var pathToTemp2 = _basicPathfinder.FindPathAStar(JunctionInPath.Id, tempNode2.Id);
if (ReverseCheck) MakeDetailData(pathToTemp2, currentDirection);
else MakeDetailData(pathToTemp2, ReverseDirection);
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp2);
//교차로찍고 원래방향으로 돌어가야한다.
if (combinedResult.DetailedPath.Count > 1)
{
if (ReverseCheck)
combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = ReverseDirection;
else
combinedResult.DetailedPath[combinedResult.DetailedPath.Count - 1].MotorDirection = currentDirection;
}
var pathToTemp3 = _basicPathfinder.FindPathAStar(tempNode2.Id, JunctionInPath.Id);
if (ReverseCheck) MakeDetailData(pathToTemp3, ReverseDirection);
else MakeDetailData(pathToTemp3, currentDirection);
combinedResult = _basicPathfinder.CombineResults(combinedResult, pathToTemp3);
}
}
// (path1 + pathToTemp + pathFromTemp) + path2 합치기
combinedResult = _basicPathfinder.CombineResults(combinedResult, path2);
MakeMagnetDirection(combinedResult);
for (int i = 0; i < combinedResult.DetailedPath.Count; i++)
combinedResult.DetailedPath[i].seq = i + 1;
return combinedResult;
}
/// <summary>
/// 이 작업후에 MakeMagnetDirection 를 추가로 실행 하세요
@@ -449,7 +133,7 @@ namespace AGVNavigationCore.PathFinding.Planning
return AGVPathResult.CreateFailure("노드 정보 오류", 0, 0);
// 2. A* 경로 탐색
var pathResult = _basicPathfinder.FindPathAStar(startNode.Id, targetNode.Id);
var pathResult = _basicPathfinder.FindPathAStar(startNode, targetNode);
pathResult.PrevNode = prevNode;
pathResult.PrevDirection = prevDirection;

709
Cs_HMI/AGVLogic/AGVNavigationCore/PathFinding/Planning/DirectionChangePlanner.cs
View File

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

53
Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.Designer.cs generated
View File

@@ -93,8 +93,8 @@ namespace AGVSimulator.Forms
this._agvCountLabel = new System.Windows.Forms.Label();
this._simulationStatusLabel = new System.Windows.Forms.Label();
this._pathGroup = new System.Windows.Forms.GroupBox();
this.btPath2 = new System.Windows.Forms.Button();
this._clearPathButton = new System.Windows.Forms.Button();
this.btPath1 = new System.Windows.Forms.Button();
this._targetCalcButton = new System.Windows.Forms.Button();
this._avoidRotationCheckBox = new System.Windows.Forms.CheckBox();
this._targetNodeCombo = new System.Windows.Forms.ComboBox();
@@ -120,7 +120,6 @@ namespace AGVSimulator.Forms
this._liftDirectionLabel = new System.Windows.Forms.Label();
this._motorDirectionLabel = new System.Windows.Forms.Label();
this.timer1 = new System.Windows.Forms.Timer(this.components);
this.btPath2 = new System.Windows.Forms.Button();
this._menuStrip.SuspendLayout();
this._toolStrip.SuspendLayout();
this._statusStrip.SuspendLayout();
@@ -142,7 +141,7 @@ namespace AGVSimulator.Forms
this.helpToolStripMenuItem});
this._menuStrip.Location = new System.Drawing.Point(0, 0);
this._menuStrip.Name = "_menuStrip";
this._menuStrip.Size = new System.Drawing.Size(1034, 24);
this._menuStrip.Size = new System.Drawing.Size(1248, 24);
this._menuStrip.TabIndex = 0;
this._menuStrip.Text = "menuStrip";
//
@@ -310,7 +309,7 @@ namespace AGVSimulator.Forms
this.btMakeMap});
this._toolStrip.Location = new System.Drawing.Point(0, 24);
this._toolStrip.Name = "_toolStrip";
this._toolStrip.Size = new System.Drawing.Size(1034, 25);
this._toolStrip.Size = new System.Drawing.Size(1248, 25);
this._toolStrip.TabIndex = 1;
this._toolStrip.Text = "toolStrip";
//
@@ -436,7 +435,7 @@ namespace AGVSimulator.Forms
this.prb1});
this._statusStrip.Location = new System.Drawing.Point(0, 689);
this._statusStrip.Name = "_statusStrip";
this._statusStrip.Size = new System.Drawing.Size(1034, 22);
this._statusStrip.Size = new System.Drawing.Size(1248, 22);
this._statusStrip.TabIndex = 2;
this._statusStrip.Text = "statusStrip";
//
@@ -464,7 +463,7 @@ namespace AGVSimulator.Forms
this._controlPanel.Controls.Add(this._pathGroup);
this._controlPanel.Controls.Add(this._agvControlGroup);
this._controlPanel.Dock = System.Windows.Forms.DockStyle.Right;
this._controlPanel.Location = new System.Drawing.Point(801, 49);
this._controlPanel.Location = new System.Drawing.Point(1015, 49);
this._controlPanel.Name = "_controlPanel";
this._controlPanel.Size = new System.Drawing.Size(233, 640);
this._controlPanel.TabIndex = 3;
@@ -532,7 +531,6 @@ namespace AGVSimulator.Forms
//
this._pathGroup.Controls.Add(this.btPath2);
this._pathGroup.Controls.Add(this._clearPathButton);
this._pathGroup.Controls.Add(this.btPath1);
this._pathGroup.Controls.Add(this._targetCalcButton);
this._pathGroup.Controls.Add(this._avoidRotationCheckBox);
this._pathGroup.Controls.Add(this._targetNodeCombo);
@@ -547,6 +545,16 @@ namespace AGVSimulator.Forms
this._pathGroup.TabStop = false;
this._pathGroup.Text = "경로 제어";
//
// btPath2
//
this.btPath2.Location = new System.Drawing.Point(12, 201);
this.btPath2.Name = "btPath2";
this.btPath2.Size = new System.Drawing.Size(106, 25);
this.btPath2.TabIndex = 10;
this.btPath2.Text = "경로 계산2";
this.btPath2.UseVisualStyleBackColor = true;
this.btPath2.Click += new System.EventHandler(this.btPath2_Click);
//
// _clearPathButton
//
this._clearPathButton.Location = new System.Drawing.Point(121, 177);
@@ -557,16 +565,6 @@ namespace AGVSimulator.Forms
this._clearPathButton.UseVisualStyleBackColor = true;
this._clearPathButton.Click += new System.EventHandler(this.OnClearPath_Click);
//
// btPath1
//
this.btPath1.Location = new System.Drawing.Point(12, 174);
this.btPath1.Name = "btPath1";
this.btPath1.Size = new System.Drawing.Size(106, 25);
this.btPath1.TabIndex = 4;
this.btPath1.Text = "경로 계산";
this.btPath1.UseVisualStyleBackColor = true;
this.btPath1.Click += new System.EventHandler(this.OnCalculatePath_Click);
//
// _targetCalcButton
//
this._targetCalcButton.Location = new System.Drawing.Point(10, 148);
@@ -741,7 +739,7 @@ namespace AGVSimulator.Forms
this._canvasPanel.Dock = System.Windows.Forms.DockStyle.Fill;
this._canvasPanel.Location = new System.Drawing.Point(0, 129);
this._canvasPanel.Name = "_canvasPanel";
this._canvasPanel.Size = new System.Drawing.Size(801, 560);
this._canvasPanel.Size = new System.Drawing.Size(1015, 560);
this._canvasPanel.TabIndex = 4;
//
// lbPredict
@@ -749,7 +747,7 @@ namespace AGVSimulator.Forms
this.lbPredict.Dock = System.Windows.Forms.DockStyle.Bottom;
this.lbPredict.Location = new System.Drawing.Point(0, 513);
this.lbPredict.Name = "lbPredict";
this.lbPredict.Size = new System.Drawing.Size(801, 47);
this.lbPredict.Size = new System.Drawing.Size(1015, 47);
this.lbPredict.TabIndex = 0;
this.lbPredict.Text = "";
//
@@ -764,7 +762,7 @@ namespace AGVSimulator.Forms
this._agvInfoPanel.Dock = System.Windows.Forms.DockStyle.Top;
this._agvInfoPanel.Location = new System.Drawing.Point(0, 49);
this._agvInfoPanel.Name = "_agvInfoPanel";
this._agvInfoPanel.Size = new System.Drawing.Size(801, 80);
this._agvInfoPanel.Size = new System.Drawing.Size(1015, 80);
this._agvInfoPanel.TabIndex = 5;
//
// _pathDebugLabel
@@ -775,7 +773,7 @@ namespace AGVSimulator.Forms
this._pathDebugLabel.Location = new System.Drawing.Point(10, 30);
this._pathDebugLabel.Multiline = true;
this._pathDebugLabel.Name = "_pathDebugLabel";
this._pathDebugLabel.Size = new System.Drawing.Size(947, 43);
this._pathDebugLabel.Size = new System.Drawing.Size(947, 45);
this._pathDebugLabel.TabIndex = 4;
this._pathDebugLabel.Text = "경로: 설정되지 않음";
//
@@ -814,21 +812,11 @@ namespace AGVSimulator.Forms
this.timer1.Interval = 500;
this.timer1.Tick += new System.EventHandler(this.timer1_Tick);
//
// btPath2
//
this.btPath2.Location = new System.Drawing.Point(12, 201);
this.btPath2.Name = "btPath2";
this.btPath2.Size = new System.Drawing.Size(106, 25);
this.btPath2.TabIndex = 10;
this.btPath2.Text = "경로 계산2";
this.btPath2.UseVisualStyleBackColor = true;
this.btPath2.Click += new System.EventHandler(this.btPath2_Click);
//
// SimulatorForm
//
this.AutoScaleDimensions = new System.Drawing.SizeF(7F, 12F);
this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font;
this.ClientSize = new System.Drawing.Size(1034, 711);
this.ClientSize = new System.Drawing.Size(1248, 711);
this.Controls.Add(this._canvasPanel);
this.Controls.Add(this._agvInfoPanel);
this.Controls.Add(this._controlPanel);
@@ -901,7 +889,6 @@ namespace AGVSimulator.Forms
private System.Windows.Forms.ComboBox _startNodeCombo;
private System.Windows.Forms.Label targetNodeLabel;
private System.Windows.Forms.ComboBox _targetNodeCombo;
private System.Windows.Forms.Button btPath1;
private System.Windows.Forms.Button _clearPathButton;
private System.Windows.Forms.Button _targetCalcButton;
private System.Windows.Forms.CheckBox _avoidRotationCheckBox;

249
Cs_HMI/AGVLogic/AGVSimulator/Forms/SimulatorForm.cs
View File

@@ -422,8 +422,6 @@ namespace AGVSimulator.Forms
var displayText = GetDisplayName(selectedNode.Id);
_statusLabel.Text = $"타겟계산 - 목적지: {displayText}";
// 자동으로 경로 계산 수행
OnCalculatePath_Click(this, EventArgs.Empty);
}
}
catch (Exception ex)
@@ -1009,8 +1007,8 @@ namespace AGVSimulator.Forms
_stopSimulationButton.Enabled = _simulationState.IsRunning;
_removeAgvButton.Enabled = _agvListCombo.SelectedItem != null;
btPath1.Enabled = _startNodeCombo.SelectedItem != null &&
_targetNodeCombo.SelectedItem != null;
// btPath1.Enabled = _startNodeCombo.SelectedItem != null &&
// _targetNodeCombo.SelectedItem != null;
// RFID 위치 설정 관련
var hasSelectedAGV = _agvListCombo.SelectedItem != null;
@@ -1577,7 +1575,7 @@ namespace AGVSimulator.Forms
MotorDirection = directionName,
CurrentPosition = GetNodeDisplayName(currentNode),
TargetPosition = GetNodeDisplayName(targetNode),
DockingPosition = targetNode.StationType == StationType.Charger ? "충전기" : "장비"
DockingPosition = (targetNode.StationType == StationType.Charger1 || targetNode.StationType == StationType.Charger2) ? "충전기" : "장비"
};
if (calcResult.result)
@@ -1750,19 +1748,19 @@ namespace AGVSimulator.Forms
SetTargetNodeComboBox(dockingTarget.Id);
// 경로 계산 버튼 클릭 (실제 사용자 동작)
var calcResult = CalcPath();
//var calcResult = CalcPath();
// 테스트 결과 생성
testResult = CreateTestResultFromUI(nodeA, dockingTarget, directionName, calcResult);
//// 테스트 결과 생성
//testResult = CreateTestResultFromUI(nodeA, dockingTarget, directionName, calcResult);
// 로그 추가
logForm.AddLogItem(testResult);
//// 로그 추가
//logForm.AddLogItem(testResult);
// 실패한 경우에만 경로를 화면에 표시 (시각적 확인)
if (!testResult.Success && _simulatorCanvas.CurrentPath != null)
{
_simulatorCanvas.Invalidate();
}
//// 실패한 경우에만 경로를 화면에 표시 (시각적 확인)
//if (!testResult.Success && _simulatorCanvas.CurrentPath != null)
//{
// _simulatorCanvas.Invalidate();
//}
Application.DoEvents();
});
@@ -2414,94 +2412,94 @@ namespace AGVSimulator.Forms
return hex.PadLeft(2, '0');
}
(bool result, string message) CalcPath()
{
// 시작 RFID가 없으면 AGV 현재 위치로 설정
if (_startNodeCombo.SelectedItem == null || _startNodeCombo.Text == "선택하세요")
{
SetStartNodeFromAGVPosition();
}
//(bool result, string message) CalcPath()
//{
// // 시작 RFID가 없으면 AGV 현재 위치로 설정
// if (_startNodeCombo.SelectedItem == null || _startNodeCombo.Text == "선택하세요")
// {
// SetStartNodeFromAGVPosition();
// }
if (_startNodeCombo.SelectedItem == null || _targetNodeCombo.SelectedItem == null)
{
return (false, "시작 RFID와 목표 RFID를 선택해주세요.");
}
// if (_startNodeCombo.SelectedItem == null || _targetNodeCombo.SelectedItem == null)
// {
// return (false, "시작 RFID와 목표 RFID를 선택해주세요.");
// }
var startItem = _startNodeCombo.SelectedItem as ComboBoxItem<MapNode>;
var targetItem = _targetNodeCombo.SelectedItem as ComboBoxItem<MapNode>;
var startNode = startItem?.Value;
var targetNode = targetItem?.Value;
// var startItem = _startNodeCombo.SelectedItem as ComboBoxItem<MapNode>;
// var targetItem = _targetNodeCombo.SelectedItem as ComboBoxItem<MapNode>;
// var startNode = startItem?.Value;
// var targetNode = targetItem?.Value;
if (startNode == null || targetNode == null)
{
return (false, "선택한 노드 정보가 올바르지 않습니다.");
}
// if (startNode == null || targetNode == null)
// {
// return (false, "선택한 노드 정보가 올바르지 않습니다.");
// }
if (_advancedPathfinder == null)
{
_advancedPathfinder = new AGVPathfinder(_simulatorCanvas.Nodes);
}
// if (_advancedPathfinder == null)
// {
// _advancedPathfinder = new AGVPathfinder(_simulatorCanvas.Nodes);
// }
// 현재 AGV 방향 가져오기
var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV;
if (selectedAGV == null)
{
return (false, "Virtual AGV 가 없습니다");
}
var currentDirection = selectedAGV.CurrentDirection;
// // 현재 AGV 방향 가져오기
// var selectedAGV = _agvListCombo.SelectedItem as VirtualAGV;
// if (selectedAGV == null)
// {
// return (false, "Virtual AGV 가 없습니다");
// }
// var currentDirection = selectedAGV.CurrentDirection;
// AGV의 이전 위치에서 가장 가까운 노드 찾기
var prevNode = selectedAGV.PrevNode;
var prevDir = selectedAGV.PrevDirection;
// // AGV의 이전 위치에서 가장 가까운 노드 찾기
// var prevNode = selectedAGV.PrevNode;
// var prevDir = selectedAGV.PrevDirection;
// 고급 경로 계획 사용 (노드 객체 직접 전달)
var advancedResult = _advancedPathfinder.FindPath(startNode, targetNode, prevNode, prevDir, currentDirection);
// // 고급 경로 계획 사용 (노드 객체 직접 전달)
// var advancedResult = _advancedPathfinder.FindPath(startNode, targetNode, prevNode, prevDir, currentDirection);
_simulatorCanvas.FitToNodes();
if (advancedResult.Success)
{
// 도킹 검증이 없는 경우 추가 검증 수행
if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired)
{
advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _simulatorCanvas.Nodes);
}
// _simulatorCanvas.FitToNodes();
// if (advancedResult.Success)
// {
// // 도킹 검증이 없는 경우 추가 검증 수행
// if (advancedResult.DockingValidation == null || !advancedResult.DockingValidation.IsValidationRequired)
// {
// advancedResult.DockingValidation = DockingValidator.ValidateDockingDirection(advancedResult, _simulatorCanvas.Nodes);
// }
//마지막대상이 버퍼라면 시퀀스처리를 해야한다
if (targetNode.StationType == StationType.Buffer)
{
var lastDetailPath = advancedResult.DetailedPath.Last();
if (lastDetailPath.NodeId == targetNode.Id) //마지막노드 재확인
{
//버퍼에 도킹할때에는 마지막 노드에서 멈추고 시퀀스를 적용해야한다
advancedResult.DetailedPath = advancedResult.DetailedPath.Take(advancedResult.DetailedPath.Count - 1).ToList();
Console.WriteLine("최종위치가 버퍼이므로 마지막 RFID에서 멈추도록 합니다");
}
}
// //마지막대상이 버퍼라면 시퀀스처리를 해야한다
// if (targetNode.StationType == StationType.Buffer)
// {
// var lastDetailPath = advancedResult.DetailedPath.Last();
// if (lastDetailPath.NodeId == targetNode.Id) //마지막노드 재확인
// {
// //버퍼에 도킹할때에는 마지막 노드에서 멈추고 시퀀스를 적용해야한다
// advancedResult.DetailedPath = advancedResult.DetailedPath.Take(advancedResult.DetailedPath.Count - 1).ToList();
// Console.WriteLine("최종위치가 버퍼이므로 마지막 RFID에서 멈추도록 합니다");
// }
// }
_simulatorCanvas.CurrentPath = advancedResult;
_pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}";
_statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)";
// _simulatorCanvas.CurrentPath = advancedResult;
// _pathLengthLabel.Text = $"경로 길이: {advancedResult.TotalDistance:F1}";
// _statusLabel.Text = $"경로 계산 완료 ({advancedResult.CalculationTimeMs}ms)";
// 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요)
selectedAGV.SetPath(advancedResult);
// // 🔥 VirtualAGV에도 경로 설정 (Predict()가 동작하려면 필요)
// selectedAGV.SetPath(advancedResult);
// 도킹 검증 결과 확인 및 UI 표시
CheckAndDisplayDockingValidation(advancedResult);
// // 도킹 검증 결과 확인 및 UI 표시
// CheckAndDisplayDockingValidation(advancedResult);
// 고급 경로 디버깅 정보 표시
UpdateAdvancedPathDebugInfo(advancedResult);
return (true, string.Empty);
}
else
{
// 경로 실패시 디버깅 정보 초기화
_pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}";
// // 고급 경로 디버깅 정보 표시
// UpdateAdvancedPathDebugInfo(advancedResult);
// return (true, string.Empty);
// }
// else
// {
// // 경로 실패시 디버깅 정보 초기화
// _pathDebugLabel.Text = $"경로: 실패 - {advancedResult.ErrorMessage}";
return (false, $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}");
}
}
// return (false, $"경로를 찾을 수 없습니다:\n{advancedResult.ErrorMessage}");
// }
//}
#endregion
private void btPath2_Click(object sender, EventArgs e)
@@ -2569,17 +2567,31 @@ namespace AGVSimulator.Forms
_simulatorCanvas.HighlightNodeId = gatewayNode.Id; // Gateway 강조 설정
// 4. Start -> Gateway 경로 계산 (A*)
var pathToGateway = _advancedPathfinder.FindPath(startNode, gatewayNode, prevNode, prevDir, currentAgvDir);
var pathToGateway = _advancedPathfinder.FindBasicPath(startNode, gatewayNode, prevNode, prevDir);
if (!pathToGateway.Success) return (false, $"Gateway({gatewayNode.ID2})까지 경로 실패: {pathToGateway.ErrorMessage}");
// 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
var arrivalOrientation = pathToGateway.DetailedPath.Last().MotorDirection;
AGVPathResult finalPath = pathToGateway;
//마지막경로는 게이트웨이이므로 제거하낟.
if(pathToGateway.Path.Count > 1)
{
pathToGateway.Path.RemoveAt(pathToGateway.Path.Count - 1);
pathToGateway.DetailedPath.RemoveAt(pathToGateway.DetailedPath.Count - 1);
}
var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, pathToGateway.Path.Last(), arrivalOrientation);
// 5. Gateway -> Target 경로 계산 (회차 패턴 및 최종 진입 포함)
MapNode GateprevNode = pathToGateway.Path.Last();
NodeMotorInfo GatePrevDetail = pathToGateway.DetailedPath.Last();
var arrivalOrientation = GatePrevDetail.MotorDirection;
//아래코드오류발생함
var gatewayPathResult = GetPathFromGateway(gatewayNode, targetNode, GateprevNode, arrivalOrientation);
if (!gatewayPathResult.Success) return (false, $"{gatewayPathResult.ErrorMessage}");
AGVPathResult finalPath = pathToGateway;
finalPath = CombinePaths(finalPath, gatewayPathResult);
@@ -2617,52 +2629,50 @@ namespace AGVSimulator.Forms
/// <summary>
/// Gateway 도착 후, Target까지의 경로(회차 및 최종진입 포함)를 계산합니다.
/// </summary>
/// <param name="gatewayNode">게이트웨이 노드값</param>
/// <param name="GTNode">게이트웨이 노드값(현재노드값)</param>
/// <param name="targetNode">최종 목표값</param>
/// <param name="GTprevNode">게이트웨이 진입 전 노드</param>
/// <param name="GTprevDirection">게이트웨이 진입 전 모터방향</param>
/// <param name="PrevNode">게이트웨이 진입 전 노드</param>
/// <param name="PrevDirection">게이트웨이 진입 전 모터방향</param>
/// <returns></returns>
private AGVPathResult GetPathFromGateway(MapNode gatewayNode, MapNode targetNode, MapNode GTprevNode, AgvDirection GTprevDirection)
private AGVPathResult GetPathFromGateway(MapNode GTNode, MapNode targetNode, MapNode PrevNode, AgvDirection PrevDirection)
{
AGVPathResult resultPath = null;
MapNode currentNode = gatewayNode;
MapNode currentPrev = GTprevNode; // Gateway 바로 이전 노드 (방향 계산용)
AgvDirection currentDir = GTprevDirection;
//게이트웨이 진입 한 방향을 보고. 목적지와 도킹방향이 일치하는지 결정한다.
var deltaX = gatewayNode.Position.X - GTprevNode.Position.X;
var deltaX = GTNode.Position.X - PrevNode.Position.X;
var isMonitorLeft = false;
bool requiredDir = false;
switch (targetNode.StationType)
{
case StationType.Charger1:
case StationType.UnLoader:
case StationType.Clearner:
case StationType.Buffer:
//버퍼는 게이트웨이가 6번이고 좌/우로 판단한다
if (deltaX > 0) //게이트웨이가 더 오른쪽에있으니 좌->우 이동을 한경우이다. 이떄 모터방향이 후진이라면 모니터는 왼쪽이고, 반대는 오른쪽이다
if (deltaX > 0) //게이트웨이가 우측에 있다
{
isMonitorLeft = GTprevDirection == AgvDirection.Backward;
//이떄 모터방향이 후진이라면 모니터는 왼쪽이고, 반대는 오른쪽이다
isMonitorLeft = PrevDirection == AgvDirection.Backward;
}
else
{
isMonitorLeft = GTprevDirection == AgvDirection.Forward;
isMonitorLeft = PrevDirection == AgvDirection.Forward;
}
//버퍼는 모니터가 왼쪽에 있으면 안된다.
//충전기1만 전진 도킹을 한다.
List<string> turnPatterns = new List<string>();
AGVPathResult rlt1 = new AGVPathResult();
rlt1.Success = true;
//목적지까지 바로 계산한다
var pathtarget = _advancedPathfinder.FindBasicPath(gatewayNode, targetNode, GTprevNode, AgvDirection.Backward);
var pathtarget = _advancedPathfinder.FindBasicPath(GTNode, targetNode, PrevNode, AgvDirection.Backward);
if (isMonitorLeft)
if (targetNode.DockDirection == DockingDirection.Backward && isMonitorLeft)
{
//턴을 하는
turnPatterns = GetTurnaroundPattern(gatewayNode, targetNode);
if (turnPatterns == null || turnPatterns.Any() == false) return new AGVPathResult { Success = false, ErrorMessage = $"회차 패턴 없음: Dir {currentDir}" };
turnPatterns = GetTurnaroundPattern(GTNode, targetNode);
if (turnPatterns == null || turnPatterns.Any() == false) return new AGVPathResult { Success = false, ErrorMessage = $"회차 패턴 없음: Dir {PrevDirection}" };
foreach (var item in turnPatterns)
{
var rfidvalue = ushort.Parse(item.Substring(0, 4));
@@ -2685,7 +2695,7 @@ namespace AGVSimulator.Forms
//시작위치가 겹치므로 제거해줘야하낟.
if (pathtarget.DetailedPath.First().NodeId != rlt1.DetailedPath.Last().NodeId ||
pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection )
pathtarget.DetailedPath.First().MotorDirection != rlt1.DetailedPath.Last().MotorDirection)
{
new AGVPathResult { Success = false, ErrorMessage = $"게이트웨이 턴 마지막 주소와, 이 후 주소의 시작 노드ID가 일치하지 않습니다" };
}
@@ -2817,7 +2827,7 @@ namespace AGVSimulator.Forms
//게이트웨이까지 후진으로 이동했다면 모니터방향이 오른쪽이다 => 방향전환필요
var gateToTarget = GetPathFromGateway(GateWayNode, target, lastPrev, lastDir);
escPath.Path.RemoveAt(escPath.Path.Count-1);
escPath.Path.RemoveAt(escPath.Path.Count - 1);
escPath.DetailedPath.RemoveAt(escPath.DetailedPath.Count - 1);
@@ -2888,11 +2898,11 @@ namespace AGVSimulator.Forms
var res = new AGVPathResult();
res.Success = true;
foreach(var item in p1.Path)
foreach (var item in p1.Path)
{
res.Path.Add(item);
}
foreach(var item in p2.Path)
foreach (var item in p2.Path)
{
res.Path.Add(item);
}
@@ -2905,7 +2915,7 @@ namespace AGVSimulator.Forms
}
foreach (var item in p2.DetailedPath)
{
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
var maxseq = res.DetailedPath.Count == 0 ? 0 : res.DetailedPath.Max(t => t.seq);
item.seq = maxseq + 1;
res.DetailedPath.Add(item);
}
@@ -2922,13 +2932,6 @@ namespace AGVSimulator.Forms
_simulatorCanvas.FitToNodes();
}
private void OnCalculatePath_Click(object sender, EventArgs e)
{
var rlt = CalcPath();
if (rlt.result == false) MessageBox.Show(rlt.message, "알림", MessageBoxButtons.OK, MessageBoxIcon.Information);
}
}
}

3
Cs_HMI/Project/StateMachine/Step/_SM_RUN.cs
View File

@@ -138,7 +138,8 @@ namespace Project
PUB.sm.SetNewRunStep(ERunStep.ERROR);
}
break;
case AGVNavigationCore.Models.StationType.Charger:
case AGVNavigationCore.Models.StationType.Charger1:
case AGVNavigationCore.Models.StationType.Charger2:
break;

20
Cs_HMI/Project/ViewForm/fAuto.cs
View File

@@ -72,7 +72,7 @@ namespace Project.ViewForm
menu.Items.Add(pickOff);
// Charge
if (mapnode.StationType == StationType.Charger)
if (mapnode.StationType == StationType.Charger1 || mapnode.StationType == StationType.Charger2)
{
var charge = new ToolStripMenuItem("Charge (Move & Charge)");
charge.Click += (s, args) => ExecuteManualCommand(mapnode, ENIGProtocol.AGVCommandHE.Charger);
@@ -105,14 +105,14 @@ namespace Project.ViewForm
// 1. 경로 생성
var pathFinder = new AGVNavigationCore.PathFinding.Planning.AGVPathfinder(PUB._mapCanvas.Nodes);
// 현재위치에서 목표위치까지
var result = pathFinder.FindPath(PUB._virtualAGV.CurrentNode, targetNode);
//// 현재위치에서 목표위치까지
//var result = pathFinder.FindBasicPath(PUB._virtualAGV.CurrentNode, targetNode);
if (!result.Success || result.Path == null || result.Path.Count == 0)
{
MessageBox.Show("경로를 찾을 수 없습니다.");
return;
}
//if (!result.Success || result.Path == null || result.Path.Count == 0)
//{
// MessageBox.Show("경로를 찾을 수 없습니다.");
// return;
//}
// 2. 상태 설정
@@ -123,7 +123,7 @@ namespace Project.ViewForm
PUB.log.AddI($"[Manual Command] {cmd} to ({targetNode.Id})");
// FindPathResult contains DetailedPath already.
PUB._virtualAGV.SetPath(result);
// PUB._virtualAGV.SetPath(result);
PUB._virtualAGV.TargetNode = targetNode as MapNode;
// 3. 작업 설정
@@ -243,7 +243,7 @@ namespace Project.ViewForm
ENIGProtocol.AGVCommandHE targetCmd = ENIGProtocol.AGVCommandHE.Goto;
string confirmMsg = "";
if (targetNode.StationType == StationType.Charger)
if (targetNode.StationType == StationType.Charger1 || targetNode.StationType == StationType.Charger2)
{
if (MessageBox.Show($"[{targetNode.Id}] 충전기로 이동하여 충전을 진행하시겠습니까?", "작업 확인", MessageBoxButtons.YesNo) == DialogResult.Yes)
{