TaskService.cs
5.01 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
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
using System.Text.Json;
using Rcs.Application.Services;
using Rcs.Application.Services.BLL;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Application.Shared;
using Rcs.Domain.Models.VDA5050;
namespace Rcs.Infrastructure.BLL;
public class TaskService : ITaskService
{
private readonly IAgvPathService _agvPathService;
private readonly IMqttClientService _mqttClientService;
public TaskService(
IAgvPathService agvPathService,
IMqttClientService mqttClientService)
{
_agvPathService = agvPathService;
_mqttClientService = mqttClientService;
}
/// <summary>
/// 发送下一段路径指令
/// @author zzy
/// 2026-02-05 更新：支持两层切割结构
/// </summary>
/// <param name="robotBasic">机器人基础信息</param>
/// <param name="pathCache">路径缓存</param>
/// <param name="junctionIndex">路口段索引</param>
/// <param name="resourceIndex">资源段索引</param>
/// <param name="segmentSequenceId">分段序号</param>
public async Task SendNextSegmentOrderAsync(
RobotBasicCache robotBasic,
VdaSegmentedPathCache pathCache,
int junctionIndex,
int resourceIndex,
int segmentSequenceId)
{
// 获取地图图结构
var graph = await _agvPathService.GetOrBuildGraphAsync(pathCache.MapId);
if (graph == null)
{
throw new Exception($"无法加载地图图结构: MapId={pathCache.MapId}");
}
// 构建VDA5050订单
var order = new Domain.Models.VDA5050.Order
{
HeaderId = int.Parse(robotBasic.RobotVersion) + segmentSequenceId * 2,
Timestamp = DateTime.Now.ToString("O"),
Version = robotBasic.ProtocolVersion,
Manufacturer = robotBasic.RobotManufacturer,
SerialNumber = robotBasic.RobotSerialNumber,
OrderId = pathCache.TaskCode,
OrderUpdateId = segmentSequenceId,
Nodes = new List<Node>(),
Edges = new List<Edge>()
};
// 获取指定的路径段
var segments = pathCache.JunctionSegments[junctionIndex].ResourceSegments[resourceIndex].Segments;
// 填充订单节点和边
FillOrderWithSegments(order, segments, graph);
// 发送订单
var payload = JsonSerializer.Serialize(order);
await _mqttClientService.PublishOrderAsync(
robotBasic.ProtocolName,
robotBasic.ProtocolVersion,
robotBasic.RobotManufacturer,
robotBasic.RobotSerialNumber,
payload);
}
/// <summary>
/// 将路径段转换为VDA5050的Nodes与Edges并填充到订单中
/// @author zzy
/// </summary>
private void FillOrderWithSegments(Domain.Models.VDA5050.Order order, List<PathSegmentWithCode> segments, PathGraph graph)
{
if (segments.Count == 0) return;
var nodeLookup = graph.Nodes;
var mapCode = !string.IsNullOrWhiteSpace(graph.MapCode) ? graph.MapCode : graph.MapId.ToString();
var sequenceId = 0;
var firstSeg = segments[0];
order.Nodes.Add(BuildNode(firstSeg.FromNodeId, firstSeg.FromNodeCode, nodeLookup, mapCode, sequenceId));
foreach (var seg in segments)
{
order.Edges.Add(BuildEdge(seg, sequenceId + 1));
sequenceId += 2;
order.Nodes.Add(BuildNode(seg.ToNodeId, seg.ToNodeCode, nodeLookup, mapCode, sequenceId));
}
}
/// <summary>
/// 构建VDA5050节点信息
/// @author zzy
/// </summary>
private static Node BuildNode(Guid nodeId, string nodeCode, IReadOnlyDictionary<Guid, PathNode> lookup, string mapCode, int sequenceId)
{
var node = new Node
{
NodeId = string.IsNullOrEmpty(nodeCode) ? nodeId.ToString() : nodeCode,
SequenceId = sequenceId,
Released = false
};
if (lookup.TryGetValue(nodeId, out var pathNode))
{
node.NodePosition = new NodePosition
{
X = pathNode.X,
Y = pathNode.Y,
Theta = pathNode.Theta,
MapId = mapCode
};
}
return node;
}
/// <summary>
/// 构建VDA5050边信息
/// @author zzy
/// </summary>
private static Edge BuildEdge(PathSegmentWithCode seg, int sequenceId)
{
return new Edge
{
EdgeId = string.IsNullOrEmpty(seg.EdgeCode) ? seg.EdgeId.ToString() : seg.EdgeCode,
SequenceId = sequenceId,
Released = false,
StartNodeId = seg.FromNodeCode,
EndNodeId = seg.ToNodeCode,
MaxSpeed = seg.MaxSpeed,
Length = seg.Length
};
}
}