RobotNavigationNodeResolver.cs
3.07 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
using System;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
namespace Rcs.Infrastructure.PathFinding.Services;
/// <summary>
/// 基于位置缓存、路径缓存和图结构解析机器人当前导航锚点节点编码。
/// </summary>
public static class RobotNavigationNodeResolver
{
/// <summary>
/// 尝试解析机器人当前导航锚点节点编码。
/// </summary>
public static async Task<string?> ResolveAsync(
IAgvPathService agvPathService,
Guid mapId,
RobotLocationCache? locationCache,
VdaSegmentedPathCache? pathCache = null,
string? fallbackLastNodeCode = null,
CancellationToken ct = default)
{
ct.ThrowIfCancellationRequested();
if (locationCache?.NodeId is Guid locationNodeId && locationNodeId != Guid.Empty)
{
if (pathCache != null &&
TryResolveNodeCodeFromPathCache(pathCache, locationNodeId, out var nodeCodeFromCache))
{
return nodeCodeFromCache;
}
var graph = await agvPathService.GetOrBuildGraphAsync(mapId);
if (graph?.Nodes.TryGetValue(locationNodeId, out var pathNode) == true &&
!string.IsNullOrWhiteSpace(pathNode.NodeCode))
{
return pathNode.NodeCode;
}
if (!string.IsNullOrWhiteSpace(fallbackLastNodeCode) &&
graph?.Nodes.Values.Any(n => string.Equals(n.NodeCode, fallbackLastNodeCode, StringComparison.OrdinalIgnoreCase)) == true)
{
return fallbackLastNodeCode;
}
return null;
}
if (string.IsNullOrWhiteSpace(fallbackLastNodeCode))
{
return null;
}
var fallbackGraph = await agvPathService.GetOrBuildGraphAsync(mapId);
return fallbackGraph?.Nodes.Values.Any(n => string.Equals(n.NodeCode, fallbackLastNodeCode, StringComparison.OrdinalIgnoreCase)) == true
? fallbackLastNodeCode
: null;
}
private static bool TryResolveNodeCodeFromPathCache(
VdaSegmentedPathCache cache,
Guid nodeId,
out string? nodeCode)
{
nodeCode = null;
foreach (var junction in cache.JunctionSegments)
{
foreach (var resource in junction.ResourceSegments)
{
foreach (var segment in resource.Segments)
{
if (segment.FromNodeId == nodeId && !string.IsNullOrWhiteSpace(segment.FromNodeCode))
{
nodeCode = segment.FromNodeCode;
return true;
}
if (segment.ToNodeId == nodeId && !string.IsNullOrWhiteSpace(segment.ToNodeCode))
{
nodeCode = segment.ToNodeCode;
return true;
}
}
}
}
return false;
}
}