파일정리
This commit is contained in:
ChiKyun Kim
314
AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
Normal file
314
AGVLogic/AGVNavigationCore/PathFinding/Core/AGVPathResult.cs
Normal file
@@ -0,0 +1,314 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
using AGVNavigationCore.PathFinding.Validation;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 경로 계산 결과 (방향성 및 명령어 포함)
|
||||
/// </summary>
|
||||
public class AGVPathResult
|
||||
{
|
||||
/// <summary>
|
||||
/// 경로 찾기 성공 여부
|
||||
/// </summary>
|
||||
public bool Success { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 경로 노드 목록 (시작 → 목적지 순서)
|
||||
/// </summary>
|
||||
public List<MapNode> Path { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// AGV 명령어 목록 (이동 방향 시퀀스)
|
||||
/// </summary>
|
||||
public List<AgvDirection> Commands { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 총 거리
|
||||
/// </summary>
|
||||
public float TotalDistance { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 계산 소요 시간 (밀리초)
|
||||
/// </summary>
|
||||
public long CalculationTimeMs { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 탐색된 노드 수
|
||||
/// </summary>
|
||||
public int ExploredNodeCount { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 탐색된 노드 수 (호환성용)
|
||||
/// </summary>
|
||||
public int ExploredNodes
|
||||
{
|
||||
get => ExploredNodeCount;
|
||||
set => ExploredNodeCount = value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 예상 소요 시간 (초)
|
||||
/// </summary>
|
||||
public float EstimatedTimeSeconds { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 회전 횟수
|
||||
/// </summary>
|
||||
public int RotationCount { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 오류 메시지 (실패시)
|
||||
/// </summary>
|
||||
public string Message { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 검증 결과
|
||||
/// </summary>
|
||||
public DockingValidationResult DockingValidation { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 상세 경로 정보 (NodeMotorInfo 목록)
|
||||
/// </summary>
|
||||
public List<NodeMotorInfo> DetailedPath { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 계획 설명
|
||||
/// </summary>
|
||||
public string PlanDescription { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 필요 여부
|
||||
/// </summary>
|
||||
public bool RequiredDirectionChange { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환 노드 ID
|
||||
/// </summary>
|
||||
public string DirectionChangeNode { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 경로계산시 사용했던 최초 이전 포인트 이전의 노드
|
||||
/// </summary>
|
||||
public MapNode PrevNode { get; set; }
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// PrevNode 에서 현재위치까지 이동한 모터의 방향값
|
||||
/// </summary>
|
||||
public AgvDirection PrevDirection { get; set; }
|
||||
|
||||
public MapNode Gateway { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 기본 생성자
|
||||
/// </summary>
|
||||
public AGVPathResult()
|
||||
{
|
||||
Success = false;
|
||||
Path = new List<MapNode>();
|
||||
Commands = new List<AgvDirection>();
|
||||
DetailedPath = new List<NodeMotorInfo>();
|
||||
TotalDistance = 0;
|
||||
CalculationTimeMs = 0;
|
||||
ExploredNodes = 0;
|
||||
EstimatedTimeSeconds = 0;
|
||||
RotationCount = 0;
|
||||
Message = string.Empty;
|
||||
PlanDescription = string.Empty;
|
||||
RequiredDirectionChange = false;
|
||||
DirectionChangeNode = string.Empty;
|
||||
DockingValidation = DockingValidationResult.CreateNotRequired();
|
||||
PrevNode = null;
|
||||
PrevDirection = AgvDirection.Stop;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 성공 결과 생성
|
||||
/// </summary>
|
||||
/// <param name="path">경로</param>
|
||||
/// <param name="commands">AGV 명령어 목록</param>
|
||||
/// <param name="totalDistance">총 거리</param>
|
||||
/// <param name="calculationTimeMs">계산 시간</param>
|
||||
/// <returns>성공 결과</returns>
|
||||
public static AGVPathResult CreateSuccess(List<MapNode> path, List<AgvDirection> commands, float totalDistance, long calculationTimeMs)
|
||||
{
|
||||
var result = new AGVPathResult
|
||||
{
|
||||
Success = true,
|
||||
Path = new List<MapNode>(path),
|
||||
Commands = new List<AgvDirection>(commands),
|
||||
TotalDistance = totalDistance,
|
||||
CalculationTimeMs = calculationTimeMs
|
||||
};
|
||||
|
||||
result.CalculateMetrics();
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 실패 결과 생성
|
||||
/// </summary>
|
||||
/// <param name="errorMessage">오류 메시지</param>
|
||||
/// <param name="calculationTimeMs">계산 시간</param>
|
||||
/// <param name="exploredNodes">탐색된 노드 수</param>
|
||||
/// <returns>실패 결과</returns>
|
||||
public static AGVPathResult CreateFailure(string errorMessage, long calculationTimeMs = 0, int exploredNodes = 0)
|
||||
{
|
||||
return new AGVPathResult
|
||||
{
|
||||
Success = false,
|
||||
Message = errorMessage,
|
||||
CalculationTimeMs = calculationTimeMs,
|
||||
ExploredNodes = exploredNodes
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 경로 메트릭 계산
|
||||
/// </summary>
|
||||
private void CalculateMetrics()
|
||||
{
|
||||
RotationCount = CountRotations();
|
||||
EstimatedTimeSeconds = CalculateEstimatedTime();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 회전 횟수 계산
|
||||
/// </summary>
|
||||
private int CountRotations()
|
||||
{
|
||||
int count = 0;
|
||||
foreach (var command in Commands)
|
||||
{
|
||||
if (command == AgvDirection.Left || command == AgvDirection.Right)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 예상 소요 시간 계산
|
||||
/// </summary>
|
||||
/// <param name="agvSpeed">AGV 속도 (픽셀/초, 기본값: 100)</param>
|
||||
/// <param name="rotationTime">회전 시간 (초, 기본값: 3)</param>
|
||||
/// <returns>예상 소요 시간 (초)</returns>
|
||||
private float CalculateEstimatedTime(float agvSpeed = 100.0f, float rotationTime = 3.0f)
|
||||
{
|
||||
float moveTime = TotalDistance / agvSpeed;
|
||||
float totalRotationTime = RotationCount * rotationTime;
|
||||
return moveTime + totalRotationTime;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 명령어 요약 생성
|
||||
/// </summary>
|
||||
/// <returns>명령어 요약 문자열</returns>
|
||||
public string GetCommandSummary()
|
||||
{
|
||||
if (!Success) return "실패";
|
||||
|
||||
var summary = new List<string>();
|
||||
var currentCommand = AgvDirection.Stop;
|
||||
var count = 0;
|
||||
|
||||
foreach (var command in Commands)
|
||||
{
|
||||
if (command == currentCommand)
|
||||
{
|
||||
count++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (count > 0)
|
||||
{
|
||||
summary.Add($"{GetCommandText(currentCommand)}×{count}");
|
||||
}
|
||||
currentCommand = command;
|
||||
count = 1;
|
||||
}
|
||||
}
|
||||
|
||||
if (count > 0)
|
||||
{
|
||||
summary.Add($"{GetCommandText(currentCommand)}×{count}");
|
||||
}
|
||||
|
||||
return string.Join(" → ", summary);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 명령어 텍스트 반환
|
||||
/// </summary>
|
||||
private string GetCommandText(AgvDirection command)
|
||||
{
|
||||
switch (command)
|
||||
{
|
||||
case AgvDirection.Forward: return "전진";
|
||||
case AgvDirection.Backward: return "후진";
|
||||
case AgvDirection.Left: return "좌회전";
|
||||
case AgvDirection.Right: return "우회전";
|
||||
case AgvDirection.Stop: return "정지";
|
||||
default: return command.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로의 노드 정보를 포함
|
||||
/// </summary>
|
||||
/// <returns></returns>
|
||||
public string GetDetailedPathInfo(bool shortmessage = false)
|
||||
{
|
||||
if (!Success)
|
||||
{
|
||||
return $"경로 계산 실패: {Message} (계산시간: {CalculationTimeMs}ms)";
|
||||
}
|
||||
|
||||
var data = DetailedPath.Select(t =>
|
||||
{
|
||||
if (shortmessage)
|
||||
return $"{t.RfidId:00}{t.MotorDirection.ToString().Substring(0, 1)}{t.MagnetDirection.ToString().Substring(0, 1)}";
|
||||
else
|
||||
return $"{t.RfidId}[{t.NodeId}] {t.MotorDirection.ToString().Substring(0, 1)}-{t.MagnetDirection.ToString().Substring(0, 1)}";
|
||||
});
|
||||
return string.Join(" → ", data);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 단순 경로 목록 반환 (호환성용 - 노드 ID 문자열 목록)
|
||||
/// </summary>
|
||||
/// <returns>노드 ID 목록</returns>
|
||||
public List<string> GetSimplePath()
|
||||
{
|
||||
if (DetailedPath != null && DetailedPath.Count > 0)
|
||||
{
|
||||
return DetailedPath.Select(n => n.NodeId).ToList();
|
||||
}
|
||||
return Path?.Select(n => n.Id).ToList() ?? new List<string>();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 문자열 표현
|
||||
/// </summary>
|
||||
public override string ToString()
|
||||
{
|
||||
if (Success)
|
||||
{
|
||||
return $"Success: {Path.Count} nodes, {TotalDistance:F1}px, {RotationCount} rotations, {EstimatedTimeSeconds:F1}s";
|
||||
}
|
||||
else
|
||||
{
|
||||
return $"Failed: {Message}";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
622
AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
Normal file
622
AGVLogic/AGVNavigationCore/PathFinding/Core/AStarPathfinder.cs
Normal file
@@ -0,0 +1,622 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding.Planning;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
/// <summary>
|
||||
/// A* 알고리즘 기반 경로 탐색기
|
||||
/// </summary>
|
||||
public class AStarPathfinder
|
||||
{
|
||||
private Dictionary<string, PathNode> _nodeMap;
|
||||
private List<MapNode> _mapNodes;
|
||||
private Dictionary<string, MapNode> _mapNodeLookup; // Quick lookup for node ID -> MapNode
|
||||
|
||||
/// <summary>
|
||||
/// 휴리스틱 가중치 (기본값: 1.0)
|
||||
/// 값이 클수록 목적지 방향을 우선시하나 최적 경로를 놓칠 수 있음
|
||||
/// </summary>
|
||||
public float HeuristicWeight { get; set; } = 1.0f;
|
||||
|
||||
/// <summary>
|
||||
/// 최대 탐색 노드 수 (무한 루프 방지)
|
||||
/// </summary>
|
||||
public int MaxSearchNodes { get; set; } = 1000;
|
||||
|
||||
/// <summary>
|
||||
/// 생성자
|
||||
/// </summary>
|
||||
public AStarPathfinder()
|
||||
{
|
||||
_nodeMap = new Dictionary<string, PathNode>();
|
||||
_mapNodes = new List<MapNode>();
|
||||
_mapNodeLookup = new Dictionary<string, MapNode>();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 맵 노드 설정
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
public void SetMapNodes(List<MapNode> mapNodes)
|
||||
{
|
||||
_mapNodes = mapNodes ?? new List<MapNode>();
|
||||
_nodeMap.Clear();
|
||||
_mapNodeLookup.Clear();
|
||||
|
||||
// 모든 네비게이션 노드를 PathNode로 변환하고 양방향 연결 생성
|
||||
foreach (var mapNode in _mapNodes)
|
||||
{
|
||||
_mapNodeLookup[mapNode.Id] = mapNode; // Add to lookup table
|
||||
|
||||
if (mapNode.IsNavigationNode())
|
||||
{
|
||||
var pathNode = new PathNode(mapNode.Id, mapNode.Position);
|
||||
_nodeMap[mapNode.Id] = pathNode;
|
||||
}
|
||||
}
|
||||
|
||||
// 단일 연결을 양방향으로 확장
|
||||
foreach (var mapNode in _mapNodes)
|
||||
{
|
||||
if (mapNode.IsNavigationNode() && _nodeMap.ContainsKey(mapNode.Id))
|
||||
{
|
||||
var pathNode = _nodeMap[mapNode.Id];
|
||||
|
||||
foreach (var connectedNode in mapNode.ConnectedMapNodes)
|
||||
{
|
||||
if (connectedNode != null && _nodeMap.ContainsKey(connectedNode.Id))
|
||||
{
|
||||
// 양방향 연결 생성 (단일 연결이 양방향을 의미)
|
||||
if (!pathNode.ConnectedNodes.Contains(connectedNode.Id))
|
||||
{
|
||||
pathNode.ConnectedNodes.Add(connectedNode.Id);
|
||||
}
|
||||
|
||||
var connectedPathNode = _nodeMap[connectedNode.Id];
|
||||
if (!connectedPathNode.ConnectedNodes.Contains(mapNode.Id))
|
||||
{
|
||||
connectedPathNode.ConnectedNodes.Add(mapNode.Id);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드 ID로 MapNode 가져오기 (헬퍼 메서드)
|
||||
/// </summary>
|
||||
private MapNode GetMapNode(string nodeId)
|
||||
{
|
||||
return _mapNodeLookup.ContainsKey(nodeId) ? _mapNodeLookup[nodeId] : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 찾기 (A* 알고리즘)
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="endNodeId">목적지 노드 ID</param>
|
||||
/// <returns>경로 계산 결과</returns>
|
||||
public AGVPathResult FindPathAStar(MapNode start, MapNode end)
|
||||
{
|
||||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
|
||||
try
|
||||
{
|
||||
if (!_nodeMap.ContainsKey(start.Id))
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {start.Id}", stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
if (!_nodeMap.ContainsKey(end.Id))
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {end.Id}", stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
|
||||
//출발지와 목적지가 동일한 경우
|
||||
if (start.Id == end.Id)
|
||||
{
|
||||
//var startMapNode = GetMapNode(start);
|
||||
var singlePath = new List<MapNode> { start };
|
||||
return AGVPathResult.CreateSuccess(singlePath, new List<AgvDirection>(), 0, stopwatch.ElapsedMilliseconds);
|
||||
}
|
||||
|
||||
var startNode = _nodeMap[start.Id];
|
||||
var endNode = _nodeMap[end.Id];
|
||||
var openSet = new List<PathNode>();
|
||||
var closedSet = new HashSet<string>();
|
||||
var exploredCount = 0;
|
||||
|
||||
startNode.GCost = 0;
|
||||
startNode.HCost = CalculateHeuristic(startNode, endNode);
|
||||
startNode.Parent = null;
|
||||
openSet.Add(startNode);
|
||||
|
||||
while (openSet.Count > 0 && exploredCount < MaxSearchNodes)
|
||||
{
|
||||
var currentNode = GetLowestFCostNode(openSet);
|
||||
openSet.Remove(currentNode);
|
||||
closedSet.Add(currentNode.NodeId);
|
||||
exploredCount++;
|
||||
|
||||
if (currentNode.NodeId == end.Id)
|
||||
{
|
||||
var path = ReconstructPath(currentNode);
|
||||
var totalDistance = CalculatePathDistance(path);
|
||||
return AGVPathResult.CreateSuccess(path, new List<AgvDirection>(), totalDistance, stopwatch.ElapsedMilliseconds);
|
||||
}
|
||||
|
||||
foreach (var neighborId in currentNode.ConnectedNodes)
|
||||
{
|
||||
if (closedSet.Contains(neighborId) || !_nodeMap.ContainsKey(neighborId))
|
||||
continue;
|
||||
|
||||
var neighbor = _nodeMap[neighborId];
|
||||
var tentativeGCost = currentNode.GCost + currentNode.DistanceTo(neighbor);
|
||||
|
||||
if (!openSet.Contains(neighbor))
|
||||
{
|
||||
neighbor.Parent = currentNode;
|
||||
neighbor.GCost = tentativeGCost;
|
||||
neighbor.HCost = CalculateHeuristic(neighbor, endNode);
|
||||
openSet.Add(neighbor);
|
||||
}
|
||||
else if (tentativeGCost < neighbor.GCost)
|
||||
{
|
||||
neighbor.Parent = currentNode;
|
||||
neighbor.GCost = tentativeGCost;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return AGVPathResult.CreateFailure("경로를 찾을 수 없습니다", stopwatch.ElapsedMilliseconds, exploredCount);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
|
||||
}
|
||||
}
|
||||
|
||||
///// <summary>
|
||||
///// 경유지를 거쳐 경로 찾기 (오버로드)
|
||||
///// 여러 경유지를 순차적으로 거쳐서 최종 목적지까지의 경로를 계산합니다.
|
||||
///// 기존 FindPath를 여러 번 호출하여 각 구간의 경로를 합칩니다.
|
||||
///// </summary>
|
||||
///// <param name="startNodeId">시작 노드 ID</param>
|
||||
///// <param name="endNodeId">최종 목적지 노드 ID</param>
|
||||
///// <param name="waypointNodeIds">경유지 노드 ID 배열 (선택사항)</param>
|
||||
///// <returns>경로 계산 결과 (모든 경유지를 거친 전체 경로)</returns>
|
||||
//public AGVPathResult FindPath(string startNodeId, string endNodeId, params string[] waypointNodeIds)
|
||||
//{
|
||||
// var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||||
|
||||
// try
|
||||
// {
|
||||
// // 경유지가 없으면 기본 FindPath 호출
|
||||
// if (waypointNodeIds == null || waypointNodeIds.Length == 0)
|
||||
// {
|
||||
// return FindPathAStar(startNodeId, endNodeId);
|
||||
// }
|
||||
|
||||
// // 경유지 유효성 검증
|
||||
// var validWaypoints = new List<string>();
|
||||
// foreach (var waypointId in waypointNodeIds)
|
||||
// {
|
||||
// if (string.IsNullOrEmpty(waypointId))
|
||||
// continue;
|
||||
|
||||
// if (!_nodeMap.ContainsKey(waypointId))
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure($"경유지 노드를 찾을 수 없습니다: {waypointId}", stopwatch.ElapsedMilliseconds, 0);
|
||||
// }
|
||||
|
||||
// validWaypoints.Add(waypointId);
|
||||
// }
|
||||
|
||||
// // 경유지가 없으면 기본 경로 계산
|
||||
// if (validWaypoints.Count == 0)
|
||||
// {
|
||||
// return FindPathAStar(startNodeId, endNodeId);
|
||||
// }
|
||||
|
||||
// // 첫 번째 경유지가 시작노드와 같은지 검사
|
||||
// if (validWaypoints[0] == startNodeId)
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure(
|
||||
// $"첫 번째 경유지({validWaypoints[0]})가 시작 노드({startNodeId})와 동일합니다. 경유지는 시작노드와 달라야 합니다.",
|
||||
// stopwatch.ElapsedMilliseconds, 0);
|
||||
// }
|
||||
|
||||
// // 마지막 경유지가 목적지노드와 같은지 검사
|
||||
// if (validWaypoints[validWaypoints.Count - 1] == endNodeId)
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure(
|
||||
// $"마지막 경유지({validWaypoints[validWaypoints.Count - 1]})가 목적지 노드({endNodeId})와 동일합니다. 경유지는 목적지노드와 달라야 합니다.",
|
||||
// stopwatch.ElapsedMilliseconds, 0);
|
||||
// }
|
||||
|
||||
// // 연속된 중복만 제거 (순서 유지)
|
||||
// // 예: [1, 2, 2, 3, 2] -> [1, 2, 3, 2] (연속 중복만 제거)
|
||||
// var deduplicatedWaypoints = new List<string>();
|
||||
// string lastWaypoint = null;
|
||||
// foreach (var waypoint in validWaypoints)
|
||||
// {
|
||||
// if (waypoint != lastWaypoint)
|
||||
// {
|
||||
// deduplicatedWaypoints.Add(waypoint);
|
||||
// lastWaypoint = waypoint;
|
||||
// }
|
||||
// }
|
||||
// validWaypoints = deduplicatedWaypoints;
|
||||
|
||||
// // 최종 경로 리스트와 누적 값
|
||||
// var combinedPath = new List<MapNode>();
|
||||
// float totalDistance = 0;
|
||||
// long totalCalculationTime = 0;
|
||||
|
||||
// // 현재 시작점
|
||||
// string currentStart = startNodeId;
|
||||
|
||||
// // 1단계: 각 경유지까지의 경로 계산
|
||||
// for (int i = 0; i < validWaypoints.Count; i++)
|
||||
// {
|
||||
// string waypoint = validWaypoints[i];
|
||||
|
||||
// // 현재 위치에서 경유지까지의 경로 계산
|
||||
// var segmentResult = FindPathAStar(currentStart, waypoint);
|
||||
|
||||
// if (!segmentResult.Success)
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure(
|
||||
// $"경유지 {i + 1}({waypoint})까지의 경로 계산 실패: {segmentResult.ErrorMessage}",
|
||||
// stopwatch.ElapsedMilliseconds, 0);
|
||||
// }
|
||||
|
||||
// // 경로 합치기 (첫 번째 구간이 아니면 시작점 제거하여 중복 방지)
|
||||
// if (combinedPath.Count > 0 && segmentResult.Path.Count > 0)
|
||||
// {
|
||||
// // 시작 노드 제거 (이전 경로의 마지막 노드와 동일)
|
||||
// combinedPath.AddRange(segmentResult.Path.Skip(1));
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// combinedPath.AddRange(segmentResult.Path);
|
||||
// }
|
||||
|
||||
// totalDistance += segmentResult.TotalDistance;
|
||||
// totalCalculationTime += segmentResult.CalculationTimeMs;
|
||||
|
||||
// // 다음 경유지의 시작점은 현재 경유지
|
||||
// currentStart = waypoint;
|
||||
// }
|
||||
|
||||
// // 2단계: 마지막 경유지에서 최종 목적지까지의 경로 계산
|
||||
// var finalSegmentResult = FindPathAStar(currentStart, endNodeId);
|
||||
|
||||
// if (!finalSegmentResult.Success)
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure(
|
||||
// $"최종 목적지까지의 경로 계산 실패: {finalSegmentResult.ErrorMessage}",
|
||||
// stopwatch.ElapsedMilliseconds, 0);
|
||||
// }
|
||||
|
||||
// // 최종 경로 합치기 (시작점 제거)
|
||||
// if (combinedPath.Count > 0 && finalSegmentResult.Path.Count > 0)
|
||||
// {
|
||||
// combinedPath.AddRange(finalSegmentResult.Path.Skip(1));
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// combinedPath.AddRange(finalSegmentResult.Path);
|
||||
// }
|
||||
|
||||
// totalDistance += finalSegmentResult.TotalDistance;
|
||||
// totalCalculationTime += finalSegmentResult.CalculationTimeMs;
|
||||
|
||||
// stopwatch.Stop();
|
||||
|
||||
// // 결과 생성
|
||||
// return AGVPathResult.CreateSuccess(
|
||||
// combinedPath,
|
||||
// new List<AgvDirection>(),
|
||||
// totalDistance,
|
||||
// totalCalculationTime
|
||||
// );
|
||||
// }
|
||||
// catch (Exception ex)
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure($"경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds, 0);
|
||||
// }
|
||||
//}
|
||||
|
||||
/// <summary>
|
||||
/// 두 경로 결과를 합치기
|
||||
/// 이전 경로의 마지막 노드와 현재 경로의 시작 노드가 같으면 시작 노드를 제거하고 합침
|
||||
/// </summary>
|
||||
/// <param name="previousResult">이전 경로 결과</param>
|
||||
/// <param name="currentResult">현재 경로 결과</param>
|
||||
/// <returns>합쳐진 경로 결과</returns>
|
||||
public AGVPathResult CombineResults( AGVPathResult previousResult, AGVPathResult currentResult)
|
||||
{
|
||||
// 입력 검증
|
||||
if (previousResult == null)
|
||||
return currentResult;
|
||||
|
||||
if (currentResult == null)
|
||||
return previousResult;
|
||||
|
||||
if (!previousResult.Success)
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"이전 경로 결과 실패: {previousResult.Message}",
|
||||
previousResult.CalculationTimeMs);
|
||||
|
||||
if (!currentResult.Success)
|
||||
return AGVPathResult.CreateFailure(
|
||||
$"현재 경로 결과 실패: {currentResult.Message}",
|
||||
currentResult.CalculationTimeMs);
|
||||
|
||||
// 경로가 비어있는 경우 처리
|
||||
if (previousResult.Path == null || previousResult.Path.Count == 0)
|
||||
return currentResult;
|
||||
|
||||
if (currentResult.Path == null || currentResult.Path.Count == 0)
|
||||
return previousResult;
|
||||
|
||||
// 합친 경로 생성
|
||||
var combinedPath = new List<MapNode>(previousResult.Path);
|
||||
var combinedCommands = new List<AgvDirection>(previousResult.Commands);
|
||||
var combinedDetailedPath = new List<NodeMotorInfo>(previousResult.DetailedPath ?? new List<NodeMotorInfo>());
|
||||
|
||||
// 이전 경로의 마지막 노드와 현재 경로의 시작 노드 비교
|
||||
string lastNodeOfPrevious = previousResult.Path[previousResult.Path.Count - 1].Id;
|
||||
string firstNodeOfCurrent = currentResult.Path[0].Id;
|
||||
|
||||
if (lastNodeOfPrevious == firstNodeOfCurrent)
|
||||
{
|
||||
// 첫 번째 노드 제거 (중복 제거)
|
||||
combinedPath.RemoveAt(combinedPath.Count - 1);
|
||||
combinedPath.AddRange(currentResult.Path);
|
||||
|
||||
// DetailedPath도 첫 번째 노드 제거
|
||||
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
|
||||
{
|
||||
combinedDetailedPath.RemoveAt(combinedDetailedPath.Count - 1);
|
||||
combinedDetailedPath.AddRange(currentResult.DetailedPath);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 그대로 붙임
|
||||
combinedPath.AddRange(currentResult.Path);
|
||||
|
||||
// DetailedPath도 그대로 붙임
|
||||
if (currentResult.DetailedPath != null && currentResult.DetailedPath.Count > 0)
|
||||
{
|
||||
combinedDetailedPath.AddRange(currentResult.DetailedPath);
|
||||
}
|
||||
}
|
||||
|
||||
// 명령어 합치기
|
||||
combinedCommands.AddRange(currentResult.Commands);
|
||||
|
||||
// 총 거리 합산
|
||||
float combinedDistance = previousResult.TotalDistance + currentResult.TotalDistance;
|
||||
|
||||
// 계산 시간 합산
|
||||
long combinedCalculationTime = previousResult.CalculationTimeMs + currentResult.CalculationTimeMs;
|
||||
|
||||
// 합쳐진 결과 생성
|
||||
var result = AGVPathResult.CreateSuccess(
|
||||
combinedPath,
|
||||
combinedCommands,
|
||||
combinedDistance,
|
||||
combinedCalculationTime
|
||||
);
|
||||
|
||||
// DetailedPath 설정
|
||||
result.DetailedPath = combinedDetailedPath;
|
||||
result.PrevNode = previousResult.PrevNode;
|
||||
result.PrevDirection = previousResult.PrevDirection;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
///// <summary>
|
||||
///// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
|
||||
///// </summary>
|
||||
///// <param name="startNodeId">시작 노드 ID</param>
|
||||
///// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
|
||||
///// <returns>경로 계산 결과</returns>
|
||||
//public AGVPathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
|
||||
//{
|
||||
// if (targetNodeIds == null || targetNodeIds.Count == 0)
|
||||
// {
|
||||
// return AGVPathResult.CreateFailure("목적지 노드가 지정되지 않았습니다", 0, 0);
|
||||
// }
|
||||
|
||||
// AGVPathResult bestResult = null;
|
||||
// foreach (var targetId in targetNodeIds)
|
||||
// {
|
||||
// var result = FindPathAStar(startNodeId, targetId);
|
||||
// if (result.Success && (bestResult == null || result.TotalDistance < bestResult.TotalDistance))
|
||||
// {
|
||||
// bestResult = result;
|
||||
// }
|
||||
// }
|
||||
|
||||
// return bestResult ?? AGVPathResult.CreateFailure("모든 목적지로의 경로를 찾을 수 없습니다", 0, 0);
|
||||
//}
|
||||
|
||||
/// <summary>
|
||||
/// 휴리스틱 거리 계산 (유클리드 거리)
|
||||
/// </summary>
|
||||
private float CalculateHeuristic(PathNode from, PathNode to)
|
||||
{
|
||||
return from.DistanceTo(to) * HeuristicWeight;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// F cost가 가장 낮은 노드 선택
|
||||
/// </summary>
|
||||
private PathNode GetLowestFCostNode(List<PathNode> nodes)
|
||||
{
|
||||
PathNode lowest = nodes[0];
|
||||
foreach (var node in nodes)
|
||||
{
|
||||
if (node.FCost < lowest.FCost ||
|
||||
(Math.Abs(node.FCost - lowest.FCost) < 0.001f && node.HCost < lowest.HCost))
|
||||
{
|
||||
lowest = node;
|
||||
}
|
||||
}
|
||||
return lowest;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 재구성 (부모 노드를 따라 역추적)
|
||||
/// </summary>
|
||||
private List<MapNode> ReconstructPath(PathNode endNode)
|
||||
{
|
||||
var path = new List<MapNode>();
|
||||
var current = endNode;
|
||||
|
||||
while (current != null)
|
||||
{
|
||||
var mapNode = GetMapNode(current.NodeId);
|
||||
if (mapNode != null)
|
||||
{
|
||||
path.Add(mapNode);
|
||||
}
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
return path;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로의 총 거리 계산
|
||||
/// </summary>
|
||||
private float CalculatePathDistance(List<MapNode> path)
|
||||
{
|
||||
if (path.Count < 2) return 0;
|
||||
|
||||
float totalDistance = 0;
|
||||
for (int i = 0; i < path.Count - 1; i++)
|
||||
{
|
||||
var nodeId1 = path[i].Id;
|
||||
var nodeId2 = path[i + 1].Id;
|
||||
|
||||
if (_nodeMap.ContainsKey(nodeId1) && _nodeMap.ContainsKey(nodeId2))
|
||||
{
|
||||
totalDistance += _nodeMap[nodeId1].DistanceTo(_nodeMap[nodeId2]);
|
||||
}
|
||||
}
|
||||
|
||||
return totalDistance;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 노드가 연결되어 있는지 확인
|
||||
/// </summary>
|
||||
/// <param name="nodeId1">노드 1 ID</param>
|
||||
/// <param name="nodeId2">노드 2 ID</param>
|
||||
/// <returns>연결 여부</returns>
|
||||
public bool AreNodesConnected(string nodeId1, string nodeId2)
|
||||
{
|
||||
if (!_nodeMap.ContainsKey(nodeId1) || !_nodeMap.ContainsKey(nodeId2))
|
||||
return false;
|
||||
|
||||
return _nodeMap[nodeId1].ConnectedNodes.Contains(nodeId2);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 네비게이션 가능한 노드 목록 반환
|
||||
/// </summary>
|
||||
/// <returns>노드 ID 목록</returns>
|
||||
public List<string> GetNavigationNodes()
|
||||
{
|
||||
return _nodeMap.Keys.ToList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 노드 정보 반환
|
||||
/// </summary>
|
||||
/// <param name="nodeId">노드 ID</param>
|
||||
/// <returns>노드 정보 또는 null</returns>
|
||||
public PathNode GetNode(string nodeId)
|
||||
{
|
||||
return _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 방향 전환을 위한 대체 노드 찾기
|
||||
/// 교차로에 연결된 노드 중에서 왔던 길과 갈 길이 아닌 다른 노드를 찾음
|
||||
/// 방향 전환 시 왕복 경로에 사용될 노드
|
||||
/// </summary>
|
||||
/// <param name="junctionNodeId">교차로 노드 ID (B)</param>
|
||||
/// <param name="previousNodeId">이전 노드 ID (A - 왔던 길)</param>
|
||||
/// <param name="targetNodeId">목표 노드 ID (C - 갈 길)</param>
|
||||
/// <param name="mapNodes">전체 맵 노드 목록</param>
|
||||
/// <returns>방향 전환에 사용할 대체 노드, 없으면 null</returns>
|
||||
public MapNode FindAlternateNodeForDirectionChange(
|
||||
string junctionNodeId,
|
||||
string previousNodeId,
|
||||
string targetNodeId)
|
||||
{
|
||||
// 입력 검증
|
||||
if (string.IsNullOrEmpty(junctionNodeId) || string.IsNullOrEmpty(previousNodeId) || string.IsNullOrEmpty(targetNodeId))
|
||||
return null;
|
||||
|
||||
if (_mapNodes == null || _mapNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 교차로 노드 찾기
|
||||
var junctionNode = _mapNodes.FirstOrDefault(n => n.Id == junctionNodeId);
|
||||
if (junctionNode == null || junctionNode.ConnectedNodes == null || junctionNode.ConnectedNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 교차로에 연결된 모든 노드 중에서 조건을 만족하는 노드 찾기
|
||||
// 조건:
|
||||
// 1. 이전 노드(왔던 길)가 아님
|
||||
// 2. 목표 노드(갈 길)가 아님
|
||||
// 3. 실제로 존재하는 노드
|
||||
// 4. 활성 상태인 노드
|
||||
// 5. 네비게이션 가능한 노드
|
||||
|
||||
var alternateNodes = new List<MapNode>();
|
||||
|
||||
foreach (var connectedNodeId in junctionNode.ConnectedNodes)
|
||||
{
|
||||
if (connectedNodeId == null) continue;
|
||||
|
||||
// 조건 1: 왔던 길이 아님
|
||||
if (connectedNodeId == previousNodeId) continue;
|
||||
|
||||
// 조건 2: 갈 길이 아님
|
||||
if (connectedNodeId == targetNodeId) continue;
|
||||
|
||||
// 조건 3, 4, 5: 존재하고, 활성 상태이고, 네비게이션 가능
|
||||
var connectedNode = _mapNodes.FirstOrDefault(n => n.Id == connectedNodeId);
|
||||
if (connectedNode != null && connectedNode.IsActive && connectedNode.IsNavigationNode())
|
||||
{
|
||||
alternateNodes.Add(connectedNode);
|
||||
}
|
||||
}
|
||||
|
||||
// 찾은 노드가 없으면 null 반환
|
||||
if (alternateNodes.Count == 0)
|
||||
return null;
|
||||
|
||||
// 여러 개 찾았으면 첫 번째 노드 반환
|
||||
// (필요시 거리 기반으로 가장 가까운 노드를 선택할 수도 있음)
|
||||
return alternateNodes[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
101
AGVLogic/AGVNavigationCore/PathFinding/Core/PathNode.cs
Normal file
101
AGVLogic/AGVNavigationCore/PathFinding/Core/PathNode.cs
Normal file
@@ -0,0 +1,101 @@
|
||||
using System;
|
||||
using System.Drawing;
|
||||
|
||||
namespace AGVNavigationCore.PathFinding.Core
|
||||
{
|
||||
/// <summary>
|
||||
/// A* 알고리즘에서 사용하는 경로 노드
|
||||
/// </summary>
|
||||
public class PathNode
|
||||
{
|
||||
/// <summary>
|
||||
/// 노드 ID
|
||||
/// </summary>
|
||||
public string NodeId { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 노드 위치
|
||||
/// </summary>
|
||||
public Point Position { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 시작점으로부터의 실제 거리 (G cost)
|
||||
/// </summary>
|
||||
public float GCost { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 목적지까지의 추정 거리 (H cost - 휴리스틱)
|
||||
/// </summary>
|
||||
public float HCost { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 총 비용 (F cost = G cost + H cost)
|
||||
/// </summary>
|
||||
public float FCost => GCost + HCost;
|
||||
|
||||
/// <summary>
|
||||
/// 부모 노드 (경로 추적용)
|
||||
/// </summary>
|
||||
public PathNode Parent { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 연결된 노드 ID 목록
|
||||
/// </summary>
|
||||
public System.Collections.Generic.List<string> ConnectedNodes { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 생성자
|
||||
/// </summary>
|
||||
/// <param name="nodeId">노드 ID</param>
|
||||
/// <param name="position">위치</param>
|
||||
public PathNode(string nodeId, Point position)
|
||||
{
|
||||
NodeId = nodeId;
|
||||
Position = position;
|
||||
GCost = 0;
|
||||
HCost = 0;
|
||||
Parent = null;
|
||||
ConnectedNodes = new System.Collections.Generic.List<string>();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 다른 노드까지의 유클리드 거리 계산
|
||||
/// </summary>
|
||||
/// <param name="other">대상 노드</param>
|
||||
/// <returns>거리</returns>
|
||||
public float DistanceTo(PathNode other)
|
||||
{
|
||||
float dx = Position.X - other.Position.X;
|
||||
float dy = Position.Y - other.Position.Y;
|
||||
return (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 문자열 표현
|
||||
/// </summary>
|
||||
public override string ToString()
|
||||
{
|
||||
return $"{NodeId} - F:{FCost:F1} G:{GCost:F1} H:{HCost:F1}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 같음 비교 (NodeId 기준)
|
||||
/// </summary>
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (obj is PathNode other)
|
||||
{
|
||||
return NodeId == other.NodeId;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 해시코드 (NodeId 기준)
|
||||
/// </summary>
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return NodeId?.GetHashCode() ?? 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user