NavigationService.cs
4.63 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
using Microsoft.Extensions.Logging;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind;
using Rcs.Application.Services.PathFind.Models;
namespace Rcs.Infrastructure.PathFinding.Services;
/// <summary>
/// 导航服务（统一锁模式）
/// @author zzy
/// </summary>
public class NavigationService
{
private readonly IAgvPathService _agvPathService;
private readonly IUnifiedTrafficControlService _unifiedTrafficControl;
private readonly ILogger<NavigationService> _logger;
private readonly AStarPathFinder _pathFinder;
public NavigationService(
IAgvPathService agvPathService,
IUnifiedTrafficControlService unifiedTrafficControl,
ILoggerFactory loggerFactory,
ILogger<NavigationService> logger)
{
_agvPathService = agvPathService;
_unifiedTrafficControl = unifiedTrafficControl;
_logger = logger;
_pathFinder = new AStarPathFinder(loggerFactory.CreateLogger<AStarPathFinder>());
}
/// <summary>
/// 核心调度循环：每 100-200ms 调用一次
/// </summary>
/// <param name="robotId">机器人ID</param>
/// <param name="mapId">地图Id</param>
/// <param name="mapCode">地图编码（用于统一锁）</param>
/// <param name="currentEdgeId">当前边ID</param>
/// <param name="currentNodeId">当前节点ID</param>
/// <param name="targetNodeId">目标节点ID</param>
/// <param name="currentTheta">当前航向角</param>
public async Task<NavigationResult> ExecuteNavigationCycleAsync(
Guid robotId,
Guid mapId,
string mapCode,
Guid currentEdgeId,
Guid currentNodeId,
Guid targetNodeId,
double currentTheta,
double currentRadius)
{
// 1. 构造寻路请求
var request = new PathRequest
{
RobotId = robotId,
StartNodeId = currentNodeId,
EndNodeId = targetNodeId,
CurrentTheta = currentTheta,
VehicleOrientationAngles = new List<double> { 0, Math.PI },
ForkRadOffsets = new List<double>()
};
var graph = await _agvPathService.GetOrBuildGraphAsync(mapId);
var globalContext = _agvPathService.BuildGlobalPathContext(robotId);
// 2. 调用 A* 算法规划路径 (软约束规划)
var pathResult = _pathFinder.FindPath(request, graph, globalContext);
if (!pathResult.Success || pathResult.Segments.Count == 0)
{
return NavigationResult.Wait("无可行路径");
}
// 3. 获取下一步要走的边 (Next Step)
var nextSegment = pathResult.Segments[0];
// 获取边的Code信息（用于统一锁）
var edgeCode = graph.Edges.TryGetValue(nextSegment.EdgeId, out var edge) ? edge.EdgeCode : string.Empty;
var toNodeCode = graph.Nodes.TryGetValue(nextSegment.ToNodeId, out var toNode) ? toNode.NodeCode : string.Empty;
if (string.IsNullOrEmpty(edgeCode) || string.IsNullOrEmpty(toNodeCode))
{
_logger.LogWarning("边或节点缺少Code信息，EdgeId={EdgeId}, ToNodeId={ToNodeId}", nextSegment.EdgeId, nextSegment.ToNodeId);
return NavigationResult.Wait("边或节点缺少Code信息");
}
// 4. 向统一锁服务申请硬锁（使用 MapCode:EdgeCode）
bool edgeLockAcquired = await _unifiedTrafficControl.TryAcquireEdgeLockAsync(mapCode, edgeCode, robotId);
bool nodeLockAcquired = await _unifiedTrafficControl.TryAcquireNodeLockAsync(mapCode, toNodeCode, robotId);
if (edgeLockAcquired && nodeLockAcquired)
{
// 5. 成功锁定，返回指令给机器人
return NavigationResult.Move(nextSegment);
}
else
{
// 锁定失败，释放已获取的锁
if (edgeLockAcquired)
{
await _unifiedTrafficControl.ReleaseEdgeLockAsync(mapCode, edgeCode, robotId);
}
if (nodeLockAcquired)
{
await _unifiedTrafficControl.ReleaseNodeLockAsync(mapCode, toNodeCode, robotId);
}
// 6. 锁定失败 - 等待或触发重规划
return NavigationResult.Wait("资源竞争/物理干涉，等待中...");
}
}
}
// 简单的结果封装
public class NavigationResult
{
public bool ShouldMove { get; set; }
public PathSegment? NextSegment { get; set; }
public string Message { get; set; }
public static NavigationResult Move(PathSegment segment) => new() { ShouldMove = true, NextSegment = segment };
public static NavigationResult Wait(string msg) => new() { ShouldMove = false, Message = msg };
}