PathGraph.cs
2.14 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
namespace Rcs.Application.Services.PathFind.Models;
/// <summary>
/// 寻路图结构
/// @author zzy
/// </summary>
public class PathGraph
{
public Guid MapId { get; set; }
public string MapCode { get; set; }
public Dictionary<Guid, PathNode> Nodes { get; set; } = new();
public Dictionary<Guid, PathEdge> Edges { get; set; } = new();
public List<PathResource> Resources { get; set; } = new();
public List<Guid> GetWaitingAreaNodes()
{
return Resources
.Where(r => r.Type == (int)Rcs.Domain.Entities.MapResourcesTYPE.waitingArea)
.SelectMany(r => r.NodeIds)
.Distinct()
.ToList();
}
/// <summary>
/// 查找最近的空闲等待区节点（使用统一锁服务）
/// @author zzy
/// </summary>
/// <param name="fromNodeId">起始节点ID</param>
/// <param name="mapCode">地图编码</param>
/// <param name="trafficControlService">统一交通管制服务</param>
/// <param name="robotId">机器人ID</param>
/// <returns>最近的空闲等待区节点ID，如果没有则返回null</returns>
public Guid? FindNearestWaitingNode(Guid fromNodeId, string mapCode, IUnifiedTrafficControlService trafficControlService, Guid robotId)
{
if (!Nodes.TryGetValue(fromNodeId, out var fromNode)) return null;
var waitingNodes = GetWaitingAreaNodes();
if (waitingNodes.Count == 0) return null;
return waitingNodes
.Where(n =>
{
if (!Nodes.TryGetValue(n, out var node)) return false;
// 检查节点是否被其他机器人锁定（统一锁返回Guid?）
var holder = trafficControlService.GetNodeLockHolderAsync(mapCode, node.NodeCode).GetAwaiter().GetResult();
return !holder.HasValue || holder.Value == robotId;
})
.Where(n => Nodes.ContainsKey(n))
.OrderBy(n =>
{
var node = Nodes[n];
var dx = node.X - fromNode.X;
var dy = node.Y - fromNode.Y;
return dx * dx + dy * dy;
})
.FirstOrDefault();
}
}