refactor: Consolidate RFID mapping and add bidirectional pathfinding
Major improvements to AGV navigation system: • Consolidated RFID management into MapNode, removing duplicate RfidMapping class • Enhanced MapNode with RFID metadata fields (RfidStatus, RfidDescription) • Added automatic bidirectional connection generation in pathfinding algorithms • Updated all components to use unified MapNode-based RFID system • Added command line argument support for AGVMapEditor auto-loading files • Fixed pathfinding failures by ensuring proper node connectivity Technical changes: - Removed RfidMapping class and dependencies across all projects - Updated AStarPathfinder with EnsureBidirectionalConnections() method - Modified MapLoader to use AssignAutoRfidIds() for RFID automation - Enhanced UnifiedAGVCanvas, SimulatorForm, and MainForm for MapNode integration - Improved data consistency and reduced memory footprint 🤖 Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
ChiKyun Kim
@@ -3,467 +3,266 @@ using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using AGVNavigationCore.Models;
|
||||
using AGVNavigationCore.PathFinding;
|
||||
|
||||
namespace AGVMapEditor.Models
|
||||
{
|
||||
/// <summary>
|
||||
/// AGV 전용 경로 계산기 (A* 알고리즘 기반)
|
||||
/// AGV의 방향성, 도킹 제약, 회전 제약을 고려한 경로 계산
|
||||
/// AGV 전용 경로 계산기 (AGVNavigationCore 래퍼)
|
||||
/// AGVMapEditor와 AGVNavigationCore 간의 호환성 제공
|
||||
/// RFID 기반 경로 계산을 우선 사용
|
||||
/// </summary>
|
||||
public class PathCalculator
|
||||
{
|
||||
#region Constants
|
||||
|
||||
private const float BASE_MOVE_COST = 1.0f; // 기본 이동 비용
|
||||
private const float ROTATION_COST = 0.5f; // 회전 비용
|
||||
private const float DOCKING_APPROACH_COST = 0.2f; // 도킹 접근 추가 비용
|
||||
private const float HEURISTIC_WEIGHT = 1.0f; // 휴리스틱 가중치
|
||||
|
||||
#endregion
|
||||
|
||||
#region Fields
|
||||
|
||||
private List<MapNode> _mapNodes;
|
||||
private NodeResolver _nodeResolver;
|
||||
|
||||
#endregion
|
||||
|
||||
#region Constructor
|
||||
private AGVPathfinder _agvPathfinder;
|
||||
private AStarPathfinder _astarPathfinder;
|
||||
private RfidBasedPathfinder _rfidPathfinder;
|
||||
|
||||
/// <summary>
|
||||
/// 생성자
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
/// <param name="nodeResolver">노드 해결기</param>
|
||||
public PathCalculator(List<MapNode> mapNodes, NodeResolver nodeResolver)
|
||||
public PathCalculator()
|
||||
{
|
||||
_mapNodes = mapNodes ?? throw new ArgumentNullException(nameof(mapNodes));
|
||||
_nodeResolver = nodeResolver ?? throw new ArgumentNullException(nameof(nodeResolver));
|
||||
_agvPathfinder = new AGVPathfinder();
|
||||
_astarPathfinder = new AStarPathfinder();
|
||||
_rfidPathfinder = new RfidBasedPathfinder();
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Methods
|
||||
/// <summary>
|
||||
/// 맵 노드 설정
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
public void SetMapNodes(List<MapNode> mapNodes)
|
||||
{
|
||||
_agvPathfinder.SetMapNodes(mapNodes);
|
||||
_astarPathfinder.SetMapNodes(mapNodes);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 계산 (메인 메서드)
|
||||
/// 맵 데이터 설정
|
||||
/// </summary>
|
||||
/// <param name="mapNodes">맵 노드 목록</param>
|
||||
public void SetMapData(List<MapNode> mapNodes)
|
||||
{
|
||||
_agvPathfinder.SetMapNodes(mapNodes);
|
||||
_astarPathfinder.SetMapNodes(mapNodes);
|
||||
// RfidPathfinder는 MapNode의 RFID 정보를 직접 사용
|
||||
_rfidPathfinder.SetMapNodes(mapNodes);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// AGV 경로 계산
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="targetNodeId">목표 노드 ID</param>
|
||||
/// <param name="currentDirection">현재 AGV 방향</param>
|
||||
/// <param name="endNodeId">목적지 노드 ID</param>
|
||||
/// <param name="targetDirection">목적지 도착 방향</param>
|
||||
/// <returns>AGV 경로 계산 결과</returns>
|
||||
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection = null)
|
||||
{
|
||||
return _agvPathfinder.FindAGVPath(startNodeId, endNodeId, targetDirection);
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 충전 스테이션으로의 경로 찾기
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <returns>AGV 경로 계산 결과</returns>
|
||||
public AGVPathResult FindPathToChargingStation(string startNodeId)
|
||||
{
|
||||
return _agvPathfinder.FindPathToChargingStation(startNodeId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 스테이션으로의 경로 찾기
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="stationType">장비 타입</param>
|
||||
/// <returns>AGV 경로 계산 결과</returns>
|
||||
public AGVPathResult FindPathToDockingStation(string startNodeId, StationType stationType)
|
||||
{
|
||||
return _agvPathfinder.FindPathToDockingStation(startNodeId, stationType);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 여러 목적지 중 가장 가까운 노드로의 경로 찾기
|
||||
/// </summary>
|
||||
/// <param name="startNodeId">시작 노드 ID</param>
|
||||
/// <param name="targetNodeIds">목적지 후보 노드 ID 목록</param>
|
||||
/// <returns>경로 계산 결과</returns>
|
||||
public PathResult CalculatePath(string startNodeId, string targetNodeId, AgvDirection currentDirection)
|
||||
public PathResult FindNearestPath(string startNodeId, List<string> targetNodeIds)
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
|
||||
try
|
||||
{
|
||||
// 입력 검증
|
||||
var validationResult = ValidateInput(startNodeId, targetNodeId, currentDirection);
|
||||
if (!validationResult.Success)
|
||||
{
|
||||
stopwatch.Stop();
|
||||
validationResult.CalculationTime = stopwatch.ElapsedMilliseconds;
|
||||
return validationResult;
|
||||
}
|
||||
|
||||
// A* 알고리즘 실행
|
||||
var result = ExecuteAStar(startNodeId, targetNodeId, currentDirection);
|
||||
|
||||
stopwatch.Stop();
|
||||
result.CalculationTime = stopwatch.ElapsedMilliseconds;
|
||||
|
||||
return result;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
stopwatch.Stop();
|
||||
return new PathResult($"경로 계산 중 오류 발생: {ex.Message}")
|
||||
{
|
||||
CalculationTime = stopwatch.ElapsedMilliseconds
|
||||
};
|
||||
}
|
||||
return _astarPathfinder.FindNearestPath(startNodeId, targetNodeIds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 유효성 검증 (RFID 이탈 감지시 사용)
|
||||
/// 두 노드가 연결되어 있는지 확인
|
||||
/// </summary>
|
||||
/// <param name="currentPath">현재 경로</param>
|
||||
/// <param name="currentRfidId">현재 감지된 RFID</param>
|
||||
/// <returns>경로 유효성 여부</returns>
|
||||
public bool ValidateCurrentPath(PathResult currentPath, string currentRfidId)
|
||||
/// <param name="nodeId1">노드 1 ID</param>
|
||||
/// <param name="nodeId2">노드 2 ID</param>
|
||||
/// <returns>연결 여부</returns>
|
||||
public bool AreNodesConnected(string nodeId1, string nodeId2)
|
||||
{
|
||||
if (currentPath == null || !currentPath.Success)
|
||||
return false;
|
||||
|
||||
var currentNode = _nodeResolver.GetNodeByRfid(currentRfidId);
|
||||
if (currentNode == null)
|
||||
return false;
|
||||
|
||||
// 현재 노드가 계획된 경로에 포함되어 있는지 확인
|
||||
return currentPath.NodeSequence.Contains(currentNode.NodeId);
|
||||
return _astarPathfinder.AreNodesConnected(nodeId1, nodeId2);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 동적 경로 재계산 (경로 이탈시 사용)
|
||||
/// 경로 유효성 검증
|
||||
/// </summary>
|
||||
/// <param name="currentRfidId">현재 RFID 위치</param>
|
||||
/// <param name="targetNodeId">목표 노드 ID</param>
|
||||
/// <param name="currentDirection">현재 방향</param>
|
||||
/// <param name="originalPath">원래 경로 (참고용)</param>
|
||||
/// <returns>새로운 경로</returns>
|
||||
public PathResult RecalculatePath(string currentRfidId, string targetNodeId,
|
||||
AgvDirection currentDirection, PathResult originalPath = null)
|
||||
/// <param name="path">검증할 경로</param>
|
||||
/// <returns>유효성 검증 결과</returns>
|
||||
public bool ValidatePath(List<string> path)
|
||||
{
|
||||
var currentNode = _nodeResolver.GetNodeByRfid(currentRfidId);
|
||||
if (currentNode == null)
|
||||
{
|
||||
return new PathResult("현재 위치를 확인할 수 없습니다.");
|
||||
}
|
||||
|
||||
// 새로운 경로 계산
|
||||
var result = CalculatePath(currentNode.NodeId, targetNodeId, currentDirection);
|
||||
|
||||
// 원래 경로와 비교 (로그용)
|
||||
if (originalPath != null && result.Success)
|
||||
{
|
||||
// TODO: 경로 변경 로그 기록
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Methods - Input Validation
|
||||
|
||||
/// <summary>
|
||||
/// 입력 값 검증
|
||||
/// </summary>
|
||||
private PathResult ValidateInput(string startNodeId, string targetNodeId, AgvDirection currentDirection)
|
||||
{
|
||||
if (string.IsNullOrEmpty(startNodeId))
|
||||
return new PathResult("시작 노드가 지정되지 않았습니다.");
|
||||
|
||||
if (string.IsNullOrEmpty(targetNodeId))
|
||||
return new PathResult("목표 노드가 지정되지 않았습니다.");
|
||||
|
||||
var startNode = _mapNodes.FirstOrDefault(n => n.NodeId == startNodeId);
|
||||
if (startNode == null)
|
||||
return new PathResult($"시작 노드를 찾을 수 없습니다: {startNodeId}");
|
||||
|
||||
var targetNode = _mapNodes.FirstOrDefault(n => n.NodeId == targetNodeId);
|
||||
if (targetNode == null)
|
||||
return new PathResult($"목표 노드를 찾을 수 없습니다: {targetNodeId}");
|
||||
|
||||
if (startNodeId == targetNodeId)
|
||||
return new PathResult("시작점과 목표점이 동일합니다.");
|
||||
|
||||
return new PathResult { Success = true };
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Private Methods - A* Algorithm
|
||||
|
||||
/// <summary>
|
||||
/// A* 알고리즘 실행
|
||||
/// </summary>
|
||||
private PathResult ExecuteAStar(string startNodeId, string targetNodeId, AgvDirection currentDirection)
|
||||
{
|
||||
var openSet = new SortedSet<PathNode>();
|
||||
var closedSet = new HashSet<string>();
|
||||
var gScore = new Dictionary<string, float>();
|
||||
|
||||
// 시작 노드 설정
|
||||
var startPathNode = new PathNode(startNodeId, currentDirection)
|
||||
{
|
||||
GCost = 0,
|
||||
HCost = CalculateHeuristic(startNodeId, targetNodeId)
|
||||
};
|
||||
|
||||
openSet.Add(startPathNode);
|
||||
gScore[startPathNode.GetKey()] = 0;
|
||||
|
||||
while (openSet.Count > 0)
|
||||
{
|
||||
// 가장 낮은 F 비용을 가진 노드 선택
|
||||
var current = openSet.Min;
|
||||
openSet.Remove(current);
|
||||
|
||||
// 목표 도달 확인
|
||||
if (current.NodeId == targetNodeId)
|
||||
{
|
||||
// 도킹 방향 검증
|
||||
if (IsValidDockingApproach(targetNodeId, current.Direction))
|
||||
{
|
||||
return ReconstructPath(current, startNodeId, targetNodeId, currentDirection);
|
||||
}
|
||||
// 도킹 방향이 맞지 않으면 계속 탐색
|
||||
}
|
||||
|
||||
closedSet.Add(current.GetKey());
|
||||
|
||||
// 인접 노드들 처리
|
||||
ProcessNeighbors(current, targetNodeId, openSet, closedSet, gScore);
|
||||
}
|
||||
|
||||
return new PathResult("경로를 찾을 수 없습니다.");
|
||||
return _agvPathfinder.ValidatePath(path);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 인접 노드들 처리
|
||||
/// 네비게이션 가능한 노드 목록 반환
|
||||
/// </summary>
|
||||
private void ProcessNeighbors(PathNode current, string targetNodeId,
|
||||
SortedSet<PathNode> openSet, HashSet<string> closedSet,
|
||||
Dictionary<string, float> gScore)
|
||||
/// <returns>노드 ID 목록</returns>
|
||||
public List<string> GetNavigationNodes()
|
||||
{
|
||||
var currentMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == current.NodeId);
|
||||
if (currentMapNode == null) return;
|
||||
|
||||
foreach (var neighborId in currentMapNode.ConnectedNodes)
|
||||
{
|
||||
var neighborMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == neighborId);
|
||||
if (neighborMapNode == null) continue;
|
||||
|
||||
// 가능한 모든 방향으로 이웃 노드 방문
|
||||
foreach (var direction in GetPossibleDirections(current, neighborMapNode))
|
||||
{
|
||||
var neighborPathNode = new PathNode(neighborId, direction);
|
||||
var neighborKey = neighborPathNode.GetKey();
|
||||
|
||||
if (closedSet.Contains(neighborKey))
|
||||
continue;
|
||||
|
||||
// 이동 비용 계산
|
||||
var moveCost = CalculateMoveCost(current, neighborPathNode, neighborMapNode);
|
||||
var tentativeGScore = current.GCost + moveCost;
|
||||
|
||||
// 더 좋은 경로인지 확인
|
||||
if (!gScore.ContainsKey(neighborKey) || tentativeGScore < gScore[neighborKey])
|
||||
{
|
||||
// 경로 정보 업데이트
|
||||
neighborPathNode.Parent = current;
|
||||
neighborPathNode.GCost = tentativeGScore;
|
||||
neighborPathNode.HCost = CalculateHeuristic(neighborId, targetNodeId);
|
||||
neighborPathNode.RotationCount = current.RotationCount +
|
||||
(current.Direction != direction ? 1 : 0);
|
||||
|
||||
// 이동 명령 시퀀스 구성
|
||||
neighborPathNode.MovementSequence = GenerateMovementSequence(current, neighborPathNode);
|
||||
|
||||
gScore[neighborKey] = tentativeGScore;
|
||||
|
||||
// openSet에 추가 (중복 제거)
|
||||
openSet.RemoveWhere(n => n.GetKey() == neighborKey);
|
||||
openSet.Add(neighborPathNode);
|
||||
}
|
||||
}
|
||||
}
|
||||
return _astarPathfinder.GetNavigationNodes();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 가능한 방향들 계산
|
||||
/// AGV 현재 방향 설정
|
||||
/// </summary>
|
||||
private List<AgvDirection> GetPossibleDirections(PathNode current, MapNode neighborNode)
|
||||
/// <param name="direction">현재 방향</param>
|
||||
public void SetCurrentDirection(AgvDirection direction)
|
||||
{
|
||||
var directions = new List<AgvDirection>();
|
||||
|
||||
// 기본적으로 전진/후진 가능
|
||||
directions.Add(AgvDirection.Forward);
|
||||
directions.Add(AgvDirection.Backward);
|
||||
|
||||
// 회전 가능한 노드에서만 방향 전환 가능
|
||||
if (CanRotateAt(current.NodeId))
|
||||
{
|
||||
// 현재 방향과 다른 방향도 고려
|
||||
if (current.Direction == AgvDirection.Forward)
|
||||
directions.Add(AgvDirection.Backward);
|
||||
else if (current.Direction == AgvDirection.Backward)
|
||||
directions.Add(AgvDirection.Forward);
|
||||
}
|
||||
|
||||
return directions;
|
||||
_agvPathfinder.CurrentDirection = direction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이동 비용 계산
|
||||
/// 회전 비용 가중치 설정
|
||||
/// </summary>
|
||||
private float CalculateMoveCost(PathNode from, PathNode to, MapNode toMapNode)
|
||||
/// <param name="weight">회전 비용 가중치</param>
|
||||
public void SetRotationCostWeight(float weight)
|
||||
{
|
||||
float cost = BASE_MOVE_COST;
|
||||
|
||||
// 방향 전환 비용
|
||||
if (from.Direction != to.Direction)
|
||||
{
|
||||
cost += ROTATION_COST;
|
||||
}
|
||||
|
||||
// 도킹 스테이션 접근 비용
|
||||
if (toMapNode.Type == NodeType.Docking || toMapNode.Type == NodeType.Charging)
|
||||
{
|
||||
cost += DOCKING_APPROACH_COST;
|
||||
}
|
||||
|
||||
// 실제 거리 기반 비용 (좌표가 있는 경우)
|
||||
var fromMapNode = _mapNodes.FirstOrDefault(n => n.NodeId == from.NodeId);
|
||||
if (fromMapNode != null && toMapNode != null)
|
||||
{
|
||||
var distance = CalculateDistance(fromMapNode.Position, toMapNode.Position);
|
||||
cost *= (distance / 100.0f); // 좌표 단위를 거리 단위로 조정
|
||||
}
|
||||
|
||||
return cost;
|
||||
_agvPathfinder.RotationCostWeight = weight;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 휴리스틱 함수 (목표까지의 추정 거리)
|
||||
/// 휴리스틱 가중치 설정
|
||||
/// </summary>
|
||||
private float CalculateHeuristic(string fromNodeId, string toNodeId)
|
||||
/// <param name="weight">휴리스틱 가중치</param>
|
||||
public void SetHeuristicWeight(float weight)
|
||||
{
|
||||
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
|
||||
var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);
|
||||
|
||||
if (fromNode == null || toNode == null)
|
||||
return 0;
|
||||
|
||||
// 유클리드 거리 계산
|
||||
var distance = CalculateDistance(fromNode.Position, toNode.Position);
|
||||
return distance * HEURISTIC_WEIGHT / 100.0f; // 좌표 단위 조정
|
||||
_astarPathfinder.HeuristicWeight = weight;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 두 점 사이의 거리 계산
|
||||
/// 최대 탐색 노드 수 설정
|
||||
/// </summary>
|
||||
private float CalculateDistance(Point from, Point to)
|
||||
/// <param name="maxNodes">최대 탐색 노드 수</param>
|
||||
public void SetMaxSearchNodes(int maxNodes)
|
||||
{
|
||||
var dx = to.X - from.X;
|
||||
var dy = to.Y - from.Y;
|
||||
return (float)Math.Sqrt(dx * dx + dy * dy);
|
||||
_astarPathfinder.MaxSearchNodes = maxNodes;
|
||||
}
|
||||
|
||||
// ==================== RFID 기반 경로 계산 메서드들 ====================
|
||||
|
||||
/// <summary>
|
||||
/// RFID 기반 AGV 경로 계산
|
||||
/// </summary>
|
||||
/// <param name="startRfidId">시작 RFID</param>
|
||||
/// <param name="endRfidId">목적지 RFID</param>
|
||||
/// <param name="targetDirection">목적지 도착 방향</param>
|
||||
/// <returns>RFID 기반 AGV 경로 계산 결과</returns>
|
||||
public RfidPathResult FindAGVPathByRfid(string startRfidId, string endRfidId, AgvDirection? targetDirection = null)
|
||||
{
|
||||
return _rfidPathfinder.FindAGVPath(startRfidId, endRfidId, targetDirection);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 회전 가능한 위치인지 확인
|
||||
/// RFID 기반 충전소 경로 찾기
|
||||
/// </summary>
|
||||
private bool CanRotateAt(string nodeId)
|
||||
/// <param name="startRfidId">시작 RFID</param>
|
||||
/// <returns>RFID 기반 경로 계산 결과</returns>
|
||||
public RfidPathResult FindPathToChargingStationByRfid(string startRfidId)
|
||||
{
|
||||
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
|
||||
return node != null && (node.CanRotate || node.Type == NodeType.Rotation);
|
||||
return _rfidPathfinder.FindPathToChargingStation(startRfidId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 도킹 접근 방향이 유효한지 확인
|
||||
/// RFID 기반 도킹 스테이션 경로 찾기
|
||||
/// </summary>
|
||||
private bool IsValidDockingApproach(string nodeId, AgvDirection approachDirection)
|
||||
/// <param name="startRfidId">시작 RFID</param>
|
||||
/// <param name="stationType">장비 타입</param>
|
||||
/// <returns>RFID 기반 경로 계산 결과</returns>
|
||||
public RfidPathResult FindPathToDockingStationByRfid(string startRfidId, StationType stationType)
|
||||
{
|
||||
var node = _mapNodes.FirstOrDefault(n => n.NodeId == nodeId);
|
||||
if (node == null) return true;
|
||||
|
||||
// 도킹/충전 스테이션이 아니면 방향 제약 없음
|
||||
if (node.Type != NodeType.Docking && node.Type != NodeType.Charging)
|
||||
return true;
|
||||
|
||||
// 도킹 방향 확인
|
||||
if (node.DockDirection == null)
|
||||
return true;
|
||||
|
||||
// 충전기는 전진으로만, 다른 장비는 후진으로만
|
||||
if (node.Type == NodeType.Charging)
|
||||
return approachDirection == AgvDirection.Forward;
|
||||
else
|
||||
return approachDirection == AgvDirection.Backward;
|
||||
return _rfidPathfinder.FindPathToDockingStation(startRfidId, stationType);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 이동 명령 시퀀스 생성
|
||||
/// 여러 RFID 목적지 중 가장 가까운 곳으로의 경로 찾기
|
||||
/// </summary>
|
||||
private List<AgvDirection> GenerateMovementSequence(PathNode from, PathNode to)
|
||||
/// <param name="startRfidId">시작 RFID</param>
|
||||
/// <param name="targetRfidIds">목적지 후보 RFID 목록</param>
|
||||
/// <returns>RFID 기반 경로 계산 결과</returns>
|
||||
public RfidPathResult FindNearestPathByRfid(string startRfidId, List<string> targetRfidIds)
|
||||
{
|
||||
var sequence = new List<AgvDirection>();
|
||||
|
||||
// 방향 전환이 필요한 경우
|
||||
if (from.Direction != to.Direction)
|
||||
{
|
||||
if (from.Direction == AgvDirection.Forward && to.Direction == AgvDirection.Backward)
|
||||
{
|
||||
sequence.Add(AgvDirection.Right); // 180도 회전 (마크센서까지)
|
||||
}
|
||||
else if (from.Direction == AgvDirection.Backward && to.Direction == AgvDirection.Forward)
|
||||
{
|
||||
sequence.Add(AgvDirection.Right); // 180도 회전 (마크센서까지)
|
||||
}
|
||||
}
|
||||
|
||||
// 이동 명령
|
||||
sequence.Add(to.Direction);
|
||||
|
||||
return sequence;
|
||||
return _rfidPathfinder.FindNearestPath(startRfidId, targetRfidIds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 재구성
|
||||
/// RFID 매핑 정보 조회 (MapNode 반환)
|
||||
/// </summary>
|
||||
private PathResult ReconstructPath(PathNode goalNode, string startNodeId, string targetNodeId, AgvDirection startDirection)
|
||||
/// <param name="rfidId">RFID</param>
|
||||
/// <returns>MapNode 또는 null</returns>
|
||||
public MapNode GetRfidMapping(string rfidId)
|
||||
{
|
||||
var path = new List<PathNode>();
|
||||
var current = goalNode;
|
||||
|
||||
// 역순으로 경로 구성
|
||||
while (current != null)
|
||||
{
|
||||
path.Insert(0, current);
|
||||
current = current.Parent;
|
||||
}
|
||||
|
||||
return new PathResult(path, startNodeId, targetNodeId, startDirection);
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Public Utility Methods
|
||||
|
||||
/// <summary>
|
||||
/// 노드 간 직선 거리 계산
|
||||
/// </summary>
|
||||
public float GetDistance(string fromNodeId, string toNodeId)
|
||||
{
|
||||
var fromNode = _mapNodes.FirstOrDefault(n => n.NodeId == fromNodeId);
|
||||
var toNode = _mapNodes.FirstOrDefault(n => n.NodeId == toNodeId);
|
||||
|
||||
if (fromNode == null || toNode == null)
|
||||
return float.MaxValue;
|
||||
|
||||
return CalculateDistance(fromNode.Position, toNode.Position);
|
||||
return _rfidPathfinder.GetRfidMapping(rfidId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 특정 노드에서 가능한 다음 노드들 조회
|
||||
/// RFID로 NodeId 조회
|
||||
/// </summary>
|
||||
public List<string> GetPossibleNextNodes(string currentNodeId, AgvDirection currentDirection)
|
||||
/// <param name="rfidId">RFID</param>
|
||||
/// <returns>NodeId 또는 null</returns>
|
||||
public string GetNodeIdByRfid(string rfidId)
|
||||
{
|
||||
var currentNode = _mapNodes.FirstOrDefault(n => n.NodeId == currentNodeId);
|
||||
if (currentNode == null)
|
||||
return new List<string>();
|
||||
|
||||
return currentNode.ConnectedNodes.ToList();
|
||||
return _rfidPathfinder.GetNodeIdByRfid(rfidId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 경로 최적화 (선택적 기능)
|
||||
/// NodeId로 RFID 조회
|
||||
/// </summary>
|
||||
public PathResult OptimizePath(PathResult originalPath)
|
||||
/// <param name="nodeId">NodeId</param>
|
||||
/// <returns>RFID 또는 null</returns>
|
||||
public string GetRfidByNodeId(string nodeId)
|
||||
{
|
||||
if (originalPath == null || !originalPath.Success)
|
||||
return originalPath;
|
||||
|
||||
// TODO: 경로 최적화 로직 구현
|
||||
// - 불필요한 중간 정지점 제거
|
||||
// - 회전 최소화
|
||||
// - 경로 단순화
|
||||
|
||||
return originalPath;
|
||||
return _rfidPathfinder.GetRfidByNodeId(nodeId);
|
||||
}
|
||||
|
||||
#endregion
|
||||
/// <summary>
|
||||
/// 활성화된 RFID 목록 반환
|
||||
/// </summary>
|
||||
/// <returns>활성화된 RFID 목록</returns>
|
||||
public List<string> GetActiveRfidList()
|
||||
{
|
||||
return _rfidPathfinder.GetActiveRfidList();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// RFID pathfinder의 AGV 현재 방향 설정
|
||||
/// </summary>
|
||||
/// <param name="direction">현재 방향</param>
|
||||
public void SetRfidPathfinderCurrentDirection(AgvDirection direction)
|
||||
{
|
||||
_rfidPathfinder.CurrentDirection = direction;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// RFID pathfinder의 회전 비용 가중치 설정
|
||||
/// </summary>
|
||||
/// <param name="weight">회전 비용 가중치</param>
|
||||
public void SetRfidPathfinderRotationCostWeight(float weight)
|
||||
{
|
||||
_rfidPathfinder.RotationCostWeight = weight;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user