862 lines
36 KiB
C#
862 lines
36 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using AGVNavigationCore.Models;
|
|
|
|
namespace AGVNavigationCore.PathFinding
|
|
{
|
|
/// <summary>
|
|
/// AGV 특화 경로 탐색기 (방향성 및 도킹 제약 고려)
|
|
/// </summary>
|
|
public class AGVPathfinder
|
|
{
|
|
private AStarPathfinder _pathfinder;
|
|
private Dictionary<string, MapNode> _nodeMap;
|
|
|
|
/// <summary>
|
|
/// AGV 현재 방향
|
|
/// </summary>
|
|
public AgvDirection CurrentDirection { get; set; } = AgvDirection.Forward;
|
|
|
|
/// <summary>
|
|
/// 회전 비용 가중치 (회전이 비싼 동작임을 반영)
|
|
/// </summary>
|
|
public float RotationCostWeight { get; set; } = 50.0f;
|
|
|
|
/// <summary>
|
|
/// 도킹 접근 거리 (픽셀 단위)
|
|
/// </summary>
|
|
public float DockingApproachDistance { get; set; } = 100.0f;
|
|
|
|
/// <summary>
|
|
/// 경로 탐색 옵션
|
|
/// </summary>
|
|
public PathfindingOptions Options { get; set; } = PathfindingOptions.Default;
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
public AGVPathfinder()
|
|
{
|
|
_pathfinder = new AStarPathfinder();
|
|
_nodeMap = new Dictionary<string, MapNode>();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 맵 노드 설정
|
|
/// </summary>
|
|
/// <param name="mapNodes">맵 노드 목록</param>
|
|
public void SetMapNodes(List<MapNode> mapNodes)
|
|
{
|
|
_pathfinder.SetMapNodes(mapNodes);
|
|
_nodeMap.Clear();
|
|
|
|
foreach (var node in mapNodes ?? new List<MapNode>())
|
|
{
|
|
_nodeMap[node.NodeId] = node;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// AGV 경로 계산 (방향성 및 도킹 제약 고려)
|
|
/// </summary>
|
|
/// <param name="startNodeId">시작 노드 ID</param>
|
|
/// <param name="endNodeId">목적지 노드 ID</param>
|
|
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
|
|
/// <returns>AGV 경로 계산 결과</returns>
|
|
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection = null)
|
|
{
|
|
return FindAGVPath(startNodeId, endNodeId, targetDirection, Options);
|
|
}
|
|
|
|
/// <summary>
|
|
/// AGV 경로 계산 (현재 방향 및 옵션 지정 가능)
|
|
/// </summary>
|
|
/// <param name="startNodeId">시작 노드 ID</param>
|
|
/// <param name="endNodeId">목적지 노드 ID</param>
|
|
/// <param name="currentDirection">현재 AGV 방향</param>
|
|
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
|
|
/// <param name="options">경로 탐색 옵션</param>
|
|
/// <returns>AGV 경로 계산 결과</returns>
|
|
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? currentDirection, AgvDirection? targetDirection, PathfindingOptions options)
|
|
{
|
|
var result = FindAGVPath(startNodeId, endNodeId, targetDirection, options);
|
|
|
|
if (!result.Success || currentDirection == null || result.Commands.Count == 0)
|
|
return result;
|
|
|
|
// 경로의 첫 번째 방향과 현재 방향 비교
|
|
var firstDirection = result.Commands[0];
|
|
|
|
if (RequiresDirectionChange(currentDirection.Value, firstDirection))
|
|
{
|
|
return InsertDirectionChangeCommands(result, currentDirection.Value, firstDirection);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// AGV 경로 계산 (옵션 지정 가능)
|
|
/// </summary>
|
|
/// <param name="startNodeId">시작 노드 ID</param>
|
|
/// <param name="endNodeId">목적지 노드 ID</param>
|
|
/// <param name="targetDirection">목적지 도착 방향 (null이면 자동 결정)</param>
|
|
/// <param name="options">경로 탐색 옵션</param>
|
|
/// <returns>AGV 경로 계산 결과</returns>
|
|
public AGVPathResult FindAGVPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options)
|
|
{
|
|
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
|
|
|
try
|
|
{
|
|
if (!_nodeMap.ContainsKey(startNodeId))
|
|
{
|
|
return AGVPathResult.CreateFailure($"시작 노드를 찾을 수 없습니다: {startNodeId}", stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
if (!_nodeMap.ContainsKey(endNodeId))
|
|
{
|
|
return AGVPathResult.CreateFailure($"목적지 노드를 찾을 수 없습니다: {endNodeId}", stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
var endNode = _nodeMap[endNodeId];
|
|
|
|
if (IsSpecialNode(endNode))
|
|
{
|
|
return FindPathToSpecialNode(startNodeId, endNode, targetDirection, options, stopwatch);
|
|
}
|
|
else
|
|
{
|
|
return FindNormalPath(startNodeId, endNodeId, targetDirection, options, stopwatch);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return AGVPathResult.CreateFailure($"AGV 경로 계산 중 오류: {ex.Message}", stopwatch.ElapsedMilliseconds);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 충전 스테이션으로의 경로 찾기
|
|
/// </summary>
|
|
/// <param name="startNodeId">시작 노드 ID</param>
|
|
/// <returns>AGV 경로 계산 결과</returns>
|
|
public AGVPathResult FindPathToChargingStation(string startNodeId)
|
|
{
|
|
var chargingStations = _nodeMap.Values
|
|
.Where(n => n.Type == NodeType.Charging && n.IsActive)
|
|
.Select(n => n.NodeId)
|
|
.ToList();
|
|
|
|
if (chargingStations.Count == 0)
|
|
{
|
|
return AGVPathResult.CreateFailure("사용 가능한 충전 스테이션이 없습니다", 0);
|
|
}
|
|
|
|
var nearestResult = _pathfinder.FindNearestPath(startNodeId, chargingStations);
|
|
if (!nearestResult.Success)
|
|
{
|
|
return AGVPathResult.CreateFailure("충전 스테이션으로의 경로를 찾을 수 없습니다", nearestResult.CalculationTimeMs);
|
|
}
|
|
|
|
var targetNodeId = nearestResult.Path.Last();
|
|
return FindAGVPath(startNodeId, targetNodeId, AgvDirection.Forward);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 특정 타입의 도킹 스테이션으로의 경로 찾기
|
|
/// </summary>
|
|
/// <param name="startNodeId">시작 노드 ID</param>
|
|
/// <param name="stationType">장비 타입</param>
|
|
/// <returns>AGV 경로 계산 결과</returns>
|
|
public AGVPathResult FindPathToDockingStation(string startNodeId, StationType stationType)
|
|
{
|
|
var dockingStations = _nodeMap.Values
|
|
.Where(n => n.Type == NodeType.Docking && n.StationType == stationType && n.IsActive)
|
|
.Select(n => n.NodeId)
|
|
.ToList();
|
|
|
|
if (dockingStations.Count == 0)
|
|
{
|
|
return AGVPathResult.CreateFailure($"{stationType} 타입의 사용 가능한 도킹 스테이션이 없습니다", 0);
|
|
}
|
|
|
|
var nearestResult = _pathfinder.FindNearestPath(startNodeId, dockingStations);
|
|
if (!nearestResult.Success)
|
|
{
|
|
return AGVPathResult.CreateFailure($"{stationType} 도킹 스테이션으로의 경로를 찾을 수 없습니다", nearestResult.CalculationTimeMs);
|
|
}
|
|
|
|
var targetNodeId = nearestResult.Path.Last();
|
|
return FindAGVPath(startNodeId, targetNodeId, AgvDirection.Backward);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 일반 노드로의 경로 계산
|
|
/// </summary>
|
|
private AGVPathResult FindNormalPath(string startNodeId, string endNodeId, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
|
|
{
|
|
PathResult result;
|
|
|
|
// 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
|
|
if (options.AvoidRotationNodes)
|
|
{
|
|
result = FindPathAvoidingRotation(startNodeId, endNodeId, options);
|
|
|
|
// 회전 회피 경로를 찾지 못하면 일반 경로 계산
|
|
if (!result.Success)
|
|
{
|
|
result = _pathfinder.FindPath(startNodeId, endNodeId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = _pathfinder.FindPath(startNodeId, endNodeId);
|
|
}
|
|
|
|
if (!result.Success)
|
|
{
|
|
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
var agvCommands = GenerateAGVCommands(result.Path, targetDirection ?? AgvDirection.Forward);
|
|
var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
|
|
return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 특수 노드(도킹/충전)로의 경로 계산
|
|
/// </summary>
|
|
private AGVPathResult FindPathToSpecialNode(string startNodeId, MapNode endNode, AgvDirection? targetDirection, PathfindingOptions options, System.Diagnostics.Stopwatch stopwatch)
|
|
{
|
|
var requiredDirection = GetRequiredDirectionForNode(endNode);
|
|
var actualTargetDirection = targetDirection ?? requiredDirection;
|
|
|
|
PathResult result;
|
|
|
|
// 회전 회피 옵션이 활성화되어 있으면 회전 노드 회피 시도
|
|
if (options.AvoidRotationNodes)
|
|
{
|
|
result = FindPathAvoidingRotation(startNodeId, endNode.NodeId, options);
|
|
|
|
// 회전 회피 경로를 찾지 못하면 일반 경로 계산
|
|
if (!result.Success)
|
|
{
|
|
result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result = _pathfinder.FindPath(startNodeId, endNode.NodeId);
|
|
}
|
|
|
|
if (!result.Success)
|
|
{
|
|
return AGVPathResult.CreateFailure(result.ErrorMessage, stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
if (actualTargetDirection != requiredDirection)
|
|
{
|
|
return AGVPathResult.CreateFailure($"{endNode.NodeId}는 {requiredDirection} 방향으로만 접근 가능합니다", stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
var agvCommands = GenerateAGVCommands(result.Path, actualTargetDirection);
|
|
var nodeMotorInfos = GenerateNodeMotorInfos(result.Path);
|
|
return AGVPathResult.CreateSuccess(result.Path, agvCommands, nodeMotorInfos, result.TotalDistance, stopwatch.ElapsedMilliseconds);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 노드가 특수 노드(도킹/충전)인지 확인
|
|
/// </summary>
|
|
private bool IsSpecialNode(MapNode node)
|
|
{
|
|
return node.Type == NodeType.Docking || node.Type == NodeType.Charging;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 노드에 필요한 접근 방향 반환
|
|
/// </summary>
|
|
private AgvDirection GetRequiredDirectionForNode(MapNode node)
|
|
{
|
|
switch (node.Type)
|
|
{
|
|
case NodeType.Charging:
|
|
return AgvDirection.Forward;
|
|
case NodeType.Docking:
|
|
return node.DockDirection == DockingDirection.Forward ? AgvDirection.Forward : AgvDirection.Backward;
|
|
default:
|
|
return AgvDirection.Forward;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 경로에서 AGV 명령어 생성
|
|
/// </summary>
|
|
private List<AgvDirection> GenerateAGVCommands(List<string> path, AgvDirection targetDirection)
|
|
{
|
|
var commands = new List<AgvDirection>();
|
|
if (path.Count < 2) return commands;
|
|
|
|
var currentDir = CurrentDirection;
|
|
|
|
for (int i = 0; i < path.Count - 1; i++)
|
|
{
|
|
var currentNodeId = path[i];
|
|
var nextNodeId = path[i + 1];
|
|
|
|
if (_nodeMap.ContainsKey(currentNodeId) && _nodeMap.ContainsKey(nextNodeId))
|
|
{
|
|
var currentNode = _nodeMap[currentNodeId];
|
|
var nextNode = _nodeMap[nextNodeId];
|
|
|
|
if (currentNode.CanRotate && ShouldRotate(currentDir, targetDirection))
|
|
{
|
|
commands.Add(GetRotationCommand(currentDir, targetDirection));
|
|
currentDir = targetDirection;
|
|
}
|
|
|
|
commands.Add(currentDir);
|
|
}
|
|
}
|
|
|
|
return commands;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 회전이 필요한지 판단
|
|
/// </summary>
|
|
private bool ShouldRotate(AgvDirection current, AgvDirection target)
|
|
{
|
|
return current != target && (current == AgvDirection.Forward && target == AgvDirection.Backward ||
|
|
current == AgvDirection.Backward && target == AgvDirection.Forward);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 회전 명령어 반환
|
|
/// </summary>
|
|
private AgvDirection GetRotationCommand(AgvDirection from, AgvDirection to)
|
|
{
|
|
if (from == AgvDirection.Forward && to == AgvDirection.Backward)
|
|
return AgvDirection.Right;
|
|
if (from == AgvDirection.Backward && to == AgvDirection.Forward)
|
|
return AgvDirection.Right;
|
|
|
|
return AgvDirection.Right;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 노드별 모터방향 정보 생성 (방향 전환 로직 개선)
|
|
/// </summary>
|
|
/// <param name="path">경로 노드 목록</param>
|
|
/// <returns>노드별 모터방향 정보 목록</returns>
|
|
private List<NodeMotorInfo> GenerateNodeMotorInfos(List<string> path)
|
|
{
|
|
var nodeMotorInfos = new List<NodeMotorInfo>();
|
|
if (path.Count < 2) return nodeMotorInfos;
|
|
|
|
// 전체 경로에 대한 방향 전환 계획 수립
|
|
var directionPlan = PlanDirectionChanges(path);
|
|
|
|
for (int i = 0; i < path.Count; i++)
|
|
{
|
|
var currentNodeId = path[i];
|
|
string nextNodeId = i < path.Count - 1 ? path[i + 1] : null;
|
|
|
|
// 계획된 방향 사용
|
|
var motorDirection = directionPlan.ContainsKey(currentNodeId)
|
|
? directionPlan[currentNodeId]
|
|
: AgvDirection.Forward;
|
|
|
|
// 노드 특성 정보 수집
|
|
bool canRotate = false;
|
|
bool isDirectionChangePoint = false;
|
|
bool requiresSpecialAction = false;
|
|
string specialActionDescription = "";
|
|
|
|
if (_nodeMap.ContainsKey(currentNodeId))
|
|
{
|
|
var currentNode = _nodeMap[currentNodeId];
|
|
canRotate = currentNode.CanRotate;
|
|
|
|
// 방향 전환 감지
|
|
if (i > 0 && directionPlan.ContainsKey(path[i - 1]))
|
|
{
|
|
var prevDirection = directionPlan[path[i - 1]];
|
|
isDirectionChangePoint = prevDirection != motorDirection;
|
|
}
|
|
|
|
// 특수 동작 필요 여부 감지
|
|
if (!canRotate && isDirectionChangePoint)
|
|
{
|
|
requiresSpecialAction = true;
|
|
specialActionDescription = "갈림길 전진/후진 반복";
|
|
}
|
|
else if (canRotate && isDirectionChangePoint)
|
|
{
|
|
specialActionDescription = "회전 노드 방향전환";
|
|
}
|
|
}
|
|
|
|
var nodeMotorInfo = new NodeMotorInfo(currentNodeId, motorDirection, nextNodeId,
|
|
canRotate, isDirectionChangePoint, MagnetDirection.Straight, requiresSpecialAction, specialActionDescription);
|
|
nodeMotorInfos.Add(nodeMotorInfo);
|
|
}
|
|
|
|
return nodeMotorInfos;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 경로 전체에 대한 방향 전환 계획 수립
|
|
/// </summary>
|
|
/// <param name="path">경로 노드 목록</param>
|
|
/// <returns>노드별 모터 방향 계획</returns>
|
|
private Dictionary<string, AgvDirection> PlanDirectionChanges(List<string> path)
|
|
{
|
|
var directionPlan = new Dictionary<string, AgvDirection>();
|
|
if (path.Count < 2) return directionPlan;
|
|
|
|
// 1단계: 목적지 노드의 요구사항 분석
|
|
var targetNodeId = path[path.Count - 1];
|
|
var targetDirection = AgvDirection.Forward;
|
|
|
|
if (_nodeMap.ContainsKey(targetNodeId))
|
|
{
|
|
var targetNode = _nodeMap[targetNodeId];
|
|
targetDirection = GetRequiredDirectionForNode(targetNode);
|
|
}
|
|
|
|
// 2단계: 역방향으로 방향 전환점 찾기
|
|
var currentDirection = targetDirection;
|
|
directionPlan[targetNodeId] = currentDirection;
|
|
|
|
// 마지막에서 두 번째부터 역순으로 처리
|
|
for (int i = path.Count - 2; i >= 0; i--)
|
|
{
|
|
var currentNodeId = path[i];
|
|
var nextNodeId = path[i + 1];
|
|
|
|
if (_nodeMap.ContainsKey(currentNodeId))
|
|
{
|
|
var currentNode = _nodeMap[currentNodeId];
|
|
|
|
// 방법1: 회전 가능 노드에서 방향 전환
|
|
if (currentNode.CanRotate && NeedDirectionChange(currentNodeId, nextNodeId, currentDirection))
|
|
{
|
|
// 회전 가능한 노드에서 방향 전환 수행
|
|
var optimalDirection = CalculateOptimalDirection(currentNodeId, nextNodeId, path, i);
|
|
directionPlan[currentNodeId] = optimalDirection;
|
|
currentDirection = optimalDirection;
|
|
}
|
|
else
|
|
{
|
|
// 일반 노드: 연속성 유지
|
|
directionPlan[currentNodeId] = currentDirection;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
directionPlan[currentNodeId] = currentDirection;
|
|
}
|
|
}
|
|
|
|
// 3단계: 방법2 적용 - 불가능한 방향 전환 감지 및 수정
|
|
ApplyDirectionChangeCorrection(path, directionPlan);
|
|
|
|
return directionPlan;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 방향 전환이 필요한지 판단
|
|
/// </summary>
|
|
private bool NeedDirectionChange(string currentNodeId, string nextNodeId, AgvDirection currentDirection)
|
|
{
|
|
if (!_nodeMap.ContainsKey(nextNodeId)) return false;
|
|
|
|
var nextNode = _nodeMap[nextNodeId];
|
|
var requiredDirection = GetRequiredDirectionForNode(nextNode);
|
|
|
|
return currentDirection != requiredDirection;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 회전 가능 노드에서 최적 방향 계산
|
|
/// </summary>
|
|
private AgvDirection CalculateOptimalDirection(string nodeId, string nextNodeId, List<string> path, int nodeIndex)
|
|
{
|
|
if (!_nodeMap.ContainsKey(nextNodeId)) return AgvDirection.Forward;
|
|
|
|
var nextNode = _nodeMap[nextNodeId];
|
|
|
|
// 목적지까지의 경로를 고려한 최적 방향 결정
|
|
if (nextNode.Type == NodeType.Charging)
|
|
{
|
|
return AgvDirection.Forward; // 충전기는 전진 접근
|
|
}
|
|
else if (nextNode.Type == NodeType.Docking)
|
|
{
|
|
return AgvDirection.Backward; // 도킹은 후진 접근
|
|
}
|
|
else
|
|
{
|
|
// 경로 패턴 분석을 통한 방향 결정
|
|
return AnalyzePathPattern(path, nodeIndex);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 경로 패턴 분석을 통한 방향 결정
|
|
/// </summary>
|
|
private AgvDirection AnalyzePathPattern(List<string> path, int startIndex)
|
|
{
|
|
// 남은 경로에서 도킹/충전 스테이션이 있는지 확인
|
|
for (int i = startIndex + 1; i < path.Count; i++)
|
|
{
|
|
if (_nodeMap.ContainsKey(path[i]))
|
|
{
|
|
var node = _nodeMap[path[i]];
|
|
if (node.Type == NodeType.Docking)
|
|
{
|
|
return AgvDirection.Backward; // 도킹 준비를 위해 후진 방향
|
|
}
|
|
else if (node.Type == NodeType.Charging)
|
|
{
|
|
return AgvDirection.Forward; // 충전 준비를 위해 전진 방향
|
|
}
|
|
}
|
|
}
|
|
|
|
return AgvDirection.Forward; // 기본값
|
|
}
|
|
|
|
/// <summary>
|
|
/// 방법2: 불가능한 방향 전환 감지 및 보정 (갈림길 전진/후진 반복)
|
|
/// </summary>
|
|
private void ApplyDirectionChangeCorrection(List<string> path, Dictionary<string, AgvDirection> directionPlan)
|
|
{
|
|
for (int i = 0; i < path.Count - 1; i++)
|
|
{
|
|
var currentNodeId = path[i];
|
|
var nextNodeId = path[i + 1];
|
|
|
|
if (directionPlan.ContainsKey(currentNodeId) && directionPlan.ContainsKey(nextNodeId))
|
|
{
|
|
var currentDir = directionPlan[currentNodeId];
|
|
var nextDir = directionPlan[nextNodeId];
|
|
|
|
// 급격한 방향 전환 감지 (전진→후진, 후진→전진)
|
|
if (IsImpossibleDirectionChange(currentDir, nextDir))
|
|
{
|
|
var currentNode = _nodeMap.ContainsKey(currentNodeId) ? _nodeMap[currentNodeId] : null;
|
|
|
|
if (currentNode != null && !currentNode.CanRotate)
|
|
{
|
|
// 회전 불가능한 노드에서 방향 전환 시도 → 특수 동작 추가
|
|
AddTurnAroundSequence(currentNodeId, directionPlan);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 불가능한 방향 전환인지 확인
|
|
/// </summary>
|
|
private bool IsImpossibleDirectionChange(AgvDirection current, AgvDirection next)
|
|
{
|
|
return (current == AgvDirection.Forward && next == AgvDirection.Backward) ||
|
|
(current == AgvDirection.Backward && next == AgvDirection.Forward);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 회전 불가능한 노드에서 방향 전환을 위한 특수 동작 시퀀스 추가
|
|
/// </summary>
|
|
private void AddTurnAroundSequence(string nodeId, Dictionary<string, AgvDirection> directionPlan)
|
|
{
|
|
// 갈림길에서 전진/후진 반복 작업으로 방향 변경
|
|
// 실제 구현시에는 NodeMotorInfo에 특수 플래그나 추가 동작 정보 포함 필요
|
|
// 현재는 안전한 방향으로 보정
|
|
if (directionPlan.ContainsKey(nodeId))
|
|
{
|
|
// 일단 연속성을 유지하도록 보정 (실제로는 특수 회전 동작 필요)
|
|
directionPlan[nodeId] = AgvDirection.Forward;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 노드에서 다음 노드로 이동할 때의 모터방향 계산 (레거시 - 새로운 PlanDirectionChanges 사용)
|
|
/// </summary>
|
|
/// <param name="currentNodeId">현재 노드 ID</param>
|
|
/// <param name="nextNodeId">다음 노드 ID</param>
|
|
/// <returns>모터방향</returns>
|
|
private AgvDirection CalculateMotorDirection(string currentNodeId, string nextNodeId)
|
|
{
|
|
if (!_nodeMap.ContainsKey(currentNodeId) || !_nodeMap.ContainsKey(nextNodeId))
|
|
{
|
|
return AgvDirection.Forward;
|
|
}
|
|
|
|
var nextNode = _nodeMap[nextNodeId];
|
|
|
|
// 다음 노드가 특수 노드인지 확인
|
|
if (nextNode.Type == NodeType.Charging)
|
|
{
|
|
// 충전기: 전진으로 도킹
|
|
return AgvDirection.Forward;
|
|
}
|
|
else if (nextNode.Type == NodeType.Docking)
|
|
{
|
|
// 도킹 스테이션: 후진으로 도킹
|
|
return AgvDirection.Backward;
|
|
}
|
|
else
|
|
{
|
|
// 일반 이동: 기본적으로 전진 (실제로는 PlanDirectionChanges에서 결정됨)
|
|
return AgvDirection.Forward;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 회전 노드를 회피하는 경로 탐색
|
|
/// </summary>
|
|
/// <param name="startNodeId">시작 노드 ID</param>
|
|
/// <param name="endNodeId">목적지 노드 ID</param>
|
|
/// <param name="options">경로 탐색 옵션</param>
|
|
/// <returns>회전 회피 경로 결과</returns>
|
|
private PathResult FindPathAvoidingRotation(string startNodeId, string endNodeId, PathfindingOptions options)
|
|
{
|
|
try
|
|
{
|
|
// 회전 가능한 노드들을 필터링하여 임시로 비활성화
|
|
var rotationNodes = _nodeMap.Values.Where(n => n.CanRotate).Select(n => n.NodeId).ToList();
|
|
var originalConnections = new Dictionary<string, List<string>>();
|
|
|
|
// 시작과 끝 노드가 회전 노드인 경우는 제외
|
|
var nodesToDisable = rotationNodes.Where(nodeId => nodeId != startNodeId && nodeId != endNodeId).ToList();
|
|
|
|
foreach (var nodeId in nodesToDisable)
|
|
{
|
|
// 임시로 연결 해제 (실제로는 pathfinder에서 해당 노드를 높은 비용으로 설정해야 함)
|
|
originalConnections[nodeId] = _nodeMap[nodeId].ConnectedNodes.ToList();
|
|
}
|
|
|
|
// 회전 노드 회피 경로 계산 시도
|
|
var result = _pathfinder.FindPath(startNodeId, endNodeId);
|
|
|
|
// 결과에서 회전 노드가 포함되어 있으면 실패로 간주
|
|
if (result.Success && result.Path != null)
|
|
{
|
|
var pathContainsRotation = result.Path.Any(nodeId => nodesToDisable.Contains(nodeId));
|
|
if (pathContainsRotation)
|
|
{
|
|
return PathResult.CreateFailure("회전 노드를 회피하는 경로를 찾을 수 없습니다.", 0, 0);
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return PathResult.CreateFailure($"회전 회피 경로 계산 중 오류: {ex.Message}", 0, 0);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 경로 유효성 검증
|
|
/// </summary>
|
|
/// <param name="path">검증할 경로</param>
|
|
/// <returns>유효성 검증 결과</returns>
|
|
public bool ValidatePath(List<string> path)
|
|
{
|
|
if (path == null || path.Count < 2) return true;
|
|
|
|
for (int i = 0; i < path.Count - 1; i++)
|
|
{
|
|
if (!_pathfinder.AreNodesConnected(path[i], path[i + 1]))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 방향에서 목표 방향으로 변경하기 위해 방향 전환이 필요한지 판단
|
|
/// </summary>
|
|
/// <param name="currentDirection">현재 방향</param>
|
|
/// <param name="targetDirection">목표 방향</param>
|
|
/// <returns>방향 전환 필요 여부</returns>
|
|
private bool RequiresDirectionChange(AgvDirection currentDirection, AgvDirection targetDirection)
|
|
{
|
|
// 같은 방향이면 변경 불필요
|
|
if (currentDirection == targetDirection)
|
|
return false;
|
|
|
|
// 전진 <-> 후진 전환은 방향 전환 필요
|
|
return (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward) ||
|
|
(currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 방향 전환을 위한 명령어를 기존 경로에 삽입
|
|
/// </summary>
|
|
/// <param name="originalResult">원래 경로 결과</param>
|
|
/// <param name="currentDirection">현재 AGV 방향</param>
|
|
/// <param name="targetDirection">목표 방향</param>
|
|
/// <returns>방향 전환 명령이 포함된 경로 결과</returns>
|
|
private AGVPathResult InsertDirectionChangeCommands(AGVPathResult originalResult, AgvDirection currentDirection, AgvDirection targetDirection)
|
|
{
|
|
if (originalResult.Path.Count < 1)
|
|
return originalResult;
|
|
|
|
// 시작 노드를 찾아서 회전 가능한지 확인
|
|
var startNodeId = originalResult.Path[0];
|
|
if (!_nodeMap.ContainsKey(startNodeId))
|
|
return originalResult;
|
|
|
|
var startNode = _nodeMap[startNodeId];
|
|
|
|
// 방향 전환을 위한 가장 가까운 교차로 찾기
|
|
var targetNodeId = originalResult.Path.LastOrDefault();
|
|
var junctionNode = FindNearestJunctionForDirectionChange(startNodeId, targetNodeId);
|
|
if (junctionNode == null)
|
|
{
|
|
return AGVPathResult.CreateFailure("방향 전환을 위한 교차로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
|
|
}
|
|
|
|
// 교차로로의 경로 추가
|
|
var pathToJunction = _pathfinder.FindPath(startNodeId, junctionNode.NodeId);
|
|
if (!pathToJunction.Success)
|
|
{
|
|
return AGVPathResult.CreateFailure("방향 전환 교차로로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
|
|
}
|
|
|
|
// 교차로에서 원래 목적지로의 경로 계산
|
|
var pathFromJunction = _pathfinder.FindPath(junctionNode.NodeId, originalResult.Path.Last());
|
|
if (!pathFromJunction.Success)
|
|
{
|
|
return AGVPathResult.CreateFailure("방향 전환 후 목적지로의 경로를 찾을 수 없습니다.", originalResult.CalculationTimeMs);
|
|
}
|
|
|
|
// 전체 경로 조합
|
|
var combinedPath = new List<string>();
|
|
combinedPath.AddRange(pathToJunction.Path);
|
|
combinedPath.AddRange(pathFromJunction.Path.Skip(1)); // 중복 노드 제거
|
|
|
|
var combinedDistance = pathToJunction.TotalDistance + pathFromJunction.TotalDistance;
|
|
var combinedCommands = GenerateAGVCommandsWithDirectionChange(combinedPath, currentDirection, targetDirection, junctionNode.NodeId);
|
|
var nodeMotorInfos = GenerateNodeMotorInfos(combinedPath);
|
|
|
|
return AGVPathResult.CreateSuccess(combinedPath, combinedCommands, nodeMotorInfos, combinedDistance, originalResult.CalculationTimeMs);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 방향 전환을 위한 가장 가까운 교차로 찾기
|
|
/// </summary>
|
|
/// <param name="fromNodeId">시작 노드 ID</param>
|
|
/// <param name="targetNodeId">목적지 노드 ID (경로상 교차로 우선 검색용)</param>
|
|
/// <returns>가장 가까운 교차로 노드</returns>
|
|
private MapNode FindNearestJunctionForDirectionChange(string fromNodeId, string targetNodeId = null)
|
|
{
|
|
// 1단계: 시작점에서 목적지까지 직접 경로상의 교차로 우선 검색
|
|
if (!string.IsNullOrEmpty(targetNodeId))
|
|
{
|
|
var directPath = _pathfinder.FindPath(fromNodeId, targetNodeId);
|
|
if (directPath.Success)
|
|
{
|
|
foreach (var nodeId in directPath.Path)
|
|
{
|
|
var node = _nodeMap.ContainsKey(nodeId) ? _nodeMap[nodeId] : null;
|
|
if (node != null && IsJunction(node))
|
|
{
|
|
return node;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// 2단계: 경로상에 교차로가 없으면 거리가 가장 가까운 교차로 찾기
|
|
var junctionNodes = _nodeMap.Values.Where(n => n.IsActive && IsJunction(n)).ToList();
|
|
|
|
MapNode nearestNode = null;
|
|
var shortestDistance = float.MaxValue;
|
|
|
|
foreach (var junctionNode in junctionNodes)
|
|
{
|
|
var pathResult = _pathfinder.FindPath(fromNodeId, junctionNode.NodeId);
|
|
if (pathResult.Success && pathResult.TotalDistance < shortestDistance)
|
|
{
|
|
shortestDistance = pathResult.TotalDistance;
|
|
nearestNode = junctionNode;
|
|
}
|
|
}
|
|
|
|
return nearestNode;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 노드가 교차로인지 판단 (3개 이상 연결된 노드)
|
|
/// </summary>
|
|
/// <param name="node">검사할 노드</param>
|
|
/// <returns>교차로이면 true</returns>
|
|
private bool IsJunction(MapNode node)
|
|
{
|
|
// 연결된 노드 수 계산 (단방향 + 양방향)
|
|
var connectedCount = node.ConnectedNodes.Count;
|
|
|
|
// 역방향 연결도 고려
|
|
foreach (var otherNode in _nodeMap.Values)
|
|
{
|
|
if (otherNode.NodeId != node.NodeId && otherNode.ConnectedNodes.Contains(node.NodeId))
|
|
{
|
|
connectedCount++;
|
|
}
|
|
}
|
|
|
|
// 3개 이상 연결되어 있으면 교차로
|
|
return connectedCount >= 3;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 방향 전환을 포함한 AGV 명령어 생성
|
|
/// </summary>
|
|
/// <param name="path">경로</param>
|
|
/// <param name="currentDirection">현재 방향</param>
|
|
/// <param name="targetDirection">목표 방향</param>
|
|
/// <param name="rotationNodeId">방향 전환 노드 ID</param>
|
|
/// <returns>AGV 명령어 목록</returns>
|
|
private List<AgvDirection> GenerateAGVCommandsWithDirectionChange(List<string> path, AgvDirection currentDirection, AgvDirection targetDirection, string rotationNodeId)
|
|
{
|
|
var commands = new List<AgvDirection>();
|
|
|
|
int rotationIndex = path.IndexOf(rotationNodeId);
|
|
|
|
// 회전 노드까지는 현재 방향으로 이동
|
|
for (int i = 0; i < rotationIndex; i++)
|
|
{
|
|
commands.Add(currentDirection);
|
|
}
|
|
|
|
// 회전 명령 추가 (전진->후진 또는 후진->전진)
|
|
if (currentDirection == AgvDirection.Forward && targetDirection == AgvDirection.Backward)
|
|
{
|
|
commands.Add(AgvDirection.Left);
|
|
commands.Add(AgvDirection.Left); // 180도 회전
|
|
}
|
|
else if (currentDirection == AgvDirection.Backward && targetDirection == AgvDirection.Forward)
|
|
{
|
|
commands.Add(AgvDirection.Right);
|
|
commands.Add(AgvDirection.Right); // 180도 회전
|
|
}
|
|
|
|
// 회전 노드부터 목적지까지는 목표 방향으로 이동
|
|
for (int i = rotationIndex; i < path.Count - 1; i++)
|
|
{
|
|
commands.Add(targetDirection);
|
|
}
|
|
|
|
return commands;
|
|
}
|
|
}
|
|
} |