Initial commit
This commit is contained in:
@@ -0,0 +1,923 @@
|
||||
using RobotNet.VDA5050.Order;
|
||||
using RobotNet.VDA5050.State;
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.FleetManager.Data;
|
||||
using RobotNet10.FleetManager.Services.ConfigManager;
|
||||
using RobotNet10.FleetManager.Services.OpenACS;
|
||||
using RobotNet10.FleetManager.Services.RobotController;
|
||||
using RobotNet10.FleetManager.Services.RobotManager;
|
||||
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
||||
using RobotNet10.MapManager.Services;
|
||||
using System.Collections.Concurrent;
|
||||
|
||||
namespace RobotNet10.FleetManager.Services.TrafficControl.ACS;
|
||||
|
||||
public class OrderACSControl : IOrderControlService, IDisposable
|
||||
{
|
||||
private readonly IServiceScopeFactory _serviceScopeFactory;
|
||||
private readonly IACSTrafficConfig _acsTrafficConfig;
|
||||
private readonly TrafficACS _trafficACS;
|
||||
private readonly Logger<OrderACSControl> _logger;
|
||||
private readonly ILogger<OrderACSControl> _loggerTimer;
|
||||
|
||||
// Store order state per robot
|
||||
private readonly ConcurrentDictionary<string, OrderACSState> _orderStates = new();
|
||||
|
||||
private WatchTimerAsync<OrderACSControl>? _processingTimer;
|
||||
private readonly Lock _timerLock = new();
|
||||
private bool _disposed = false;
|
||||
|
||||
public OrderACSControl(
|
||||
IServiceScopeFactory serviceScopeFactory,
|
||||
IACSTrafficConfig acsTrafficConfig,
|
||||
TrafficACS trafficACS,
|
||||
Logger<OrderACSControl> logger,
|
||||
ILoggerFactory loggerFactory)
|
||||
{
|
||||
_serviceScopeFactory = serviceScopeFactory ?? throw new ArgumentNullException(nameof(serviceScopeFactory));
|
||||
_acsTrafficConfig = acsTrafficConfig ?? throw new ArgumentNullException(nameof(acsTrafficConfig));
|
||||
_trafficACS = trafficACS ?? throw new ArgumentNullException(nameof(trafficACS));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
_loggerTimer = loggerFactory?.CreateLogger<OrderACSControl>() ?? throw new ArgumentNullException(nameof(loggerFactory));
|
||||
|
||||
// Subscribe to config changes
|
||||
_acsTrafficConfig.ConfigChanged += OnConfigChanged;
|
||||
|
||||
_logger.Info($"Started OrderACSControl processing timer with interval {_acsTrafficConfig.TrafficInterval}ms");
|
||||
}
|
||||
|
||||
private void OnConfigChanged(object? sender, EventArgs e)
|
||||
{
|
||||
try
|
||||
{
|
||||
var newInterval = _acsTrafficConfig.TrafficInterval;
|
||||
if (newInterval == _processingTimer?.Interval) return;
|
||||
_logger.Info($"ACSTrafficConfig changed, updating timer interval to {newInterval}ms");
|
||||
|
||||
lock (_timerLock)
|
||||
{
|
||||
// Stop old timer
|
||||
_processingTimer?.Stop();
|
||||
_processingTimer?.Dispose();
|
||||
|
||||
// Start new timer with new interval
|
||||
StartProcessingTimer();
|
||||
}
|
||||
|
||||
_logger.Info($"OrderACSControl processing timer updated to interval {newInterval}ms");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"Error updating timer interval: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private void StartProcessingTimer()
|
||||
{
|
||||
var interval = _acsTrafficConfig.TrafficInterval;
|
||||
_processingTimer = new WatchTimerAsync<OrderACSControl>(
|
||||
interval,
|
||||
ProcessAllOrdersAsync,
|
||||
_loggerTimer
|
||||
);
|
||||
_processingTimer.Start();
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed) return;
|
||||
|
||||
// Unsubscribe from config changes
|
||||
_acsTrafficConfig.ConfigChanged -= OnConfigChanged;
|
||||
|
||||
lock (_timerLock)
|
||||
{
|
||||
_processingTimer?.Stop();
|
||||
_processingTimer?.Dispose();
|
||||
}
|
||||
|
||||
_disposed = true;
|
||||
_logger.Info("Stopped OrderACSControl processing timer");
|
||||
GC.SuppressFinalize(this);
|
||||
|
||||
}
|
||||
|
||||
private async Task ProcessAllOrdersAsync()
|
||||
{
|
||||
try
|
||||
{
|
||||
// Process all active orders
|
||||
foreach (var kvp in _orderStates)
|
||||
{
|
||||
var robotId = kvp.Key;
|
||||
var orderState = kvp.Value;
|
||||
|
||||
if (orderState.Status != OrderStatus.IsProccessing)
|
||||
{
|
||||
// Skip non-processing orders
|
||||
continue;
|
||||
}
|
||||
|
||||
// Get robot controller and data using service scope to avoid circular dependency
|
||||
IRobotController? robotController = null;
|
||||
try
|
||||
{
|
||||
using var scope = _serviceScopeFactory.CreateScope();
|
||||
var robotManagerService = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
||||
robotController = robotManagerService.GetRobotController(robotId);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"ProcessAllOrdersAsync: Error getting robot controller for {robotId}: {ex.Message}");
|
||||
continue;
|
||||
}
|
||||
|
||||
if (robotController == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check if robot is online
|
||||
if (!robotController.IsOnline)
|
||||
{
|
||||
// Check if robot has been offline for more than 1 minute
|
||||
var offlineDuration = DateTime.UtcNow - orderState.LastUpdated;
|
||||
if (offlineDuration.TotalMinutes > 1)
|
||||
{
|
||||
orderState.Status = OrderStatus.IsError;
|
||||
orderState.Error = $"Robot {robotId} has been offline for more than 1 minute";
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
_logger.Warning($"CheckRobotOnline: Order for robot {robotId} marked as Error due to offline timeout ({offlineDuration.TotalMinutes:F1} minutes)");
|
||||
continue;
|
||||
}
|
||||
// Robot is offline but less than 1 minute, skip processing
|
||||
continue;
|
||||
}
|
||||
|
||||
var robotData = robotController.Data;
|
||||
var stateMsg = robotData?.State;
|
||||
|
||||
if (stateMsg == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Update last updated time
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
|
||||
// Check if robot has reached a mapped node
|
||||
var currentLastNodeId = stateMsg.LastNodeId;
|
||||
await CheckAndProcessMappedNodesAsync(robotId, currentLastNodeId, orderState, stateMsg);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"ProcessAllOrdersAsync: Error processing orders: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public OrderStatus GetRobotOrderStatus(string robotId)
|
||||
{
|
||||
if (_orderStates.TryGetValue(robotId, out var state))
|
||||
{
|
||||
return state.Status;
|
||||
}
|
||||
return OrderStatus.Empty; // No order found
|
||||
}
|
||||
|
||||
public async Task<bool> CreateRobotOrderAsync(string robotId, RobotRoute route)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (string.IsNullOrEmpty(robotId))
|
||||
{
|
||||
_logger.Warning("CreateRobotOrderAsync: robotId is null or empty");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (route == null || route.FullRoute == null || route.FullRoute.Count == 0)
|
||||
{
|
||||
_logger.Warning($"CreateRobotOrderAsync: Invalid route for robot {robotId}");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Find all nodes that are mapped to ACS zones (IN and OUT separately)
|
||||
var (inMappedNodes, outMappedNodes) = await FindMappedNodesAsync(route);
|
||||
|
||||
// Find first IN mapped node index (only IN nodes affect Base/Horizon calculation)
|
||||
var firstMappedNodeIndex = FindFirstMappedNodeIndex(route, inMappedNodes);
|
||||
|
||||
int baseSegmentCount;
|
||||
if (inMappedNodes.Count == 0 || firstMappedNodeIndex < 0)
|
||||
{
|
||||
// No IN mapped nodes, base = full route
|
||||
baseSegmentCount = route.FullRoute.Count;
|
||||
_logger.Info($"CreateRobotOrderAsync: No IN mapped nodes found for robot {robotId}, base = full route");
|
||||
}
|
||||
else
|
||||
{
|
||||
// Calculate Base: segments from start to first mapped node + 1 segment
|
||||
baseSegmentCount = firstMappedNodeIndex + 3;
|
||||
if (baseSegmentCount > route.FullRoute.Count)
|
||||
{
|
||||
baseSegmentCount = route.FullRoute.Count;
|
||||
}
|
||||
}
|
||||
|
||||
// Split route into Base and Horizon
|
||||
route.Base = [.. route.FullRoute.Take(baseSegmentCount)];
|
||||
route.Horizon = [.. route.FullRoute.Skip(baseSegmentCount)];
|
||||
|
||||
// Mark base segments as released
|
||||
foreach (var segment in route.Base)
|
||||
{
|
||||
segment.Released = true;
|
||||
}
|
||||
|
||||
_logger.Info($"CreateRobotOrderAsync: Order created for robot {robotId}, Base segments: {route.Base.Count}, Horizon segments: {route.Horizon.Count}, IN mapped nodes: {inMappedNodes.Count}, OUT mapped nodes: {outMappedNodes.Count}");
|
||||
|
||||
// Send order to robot
|
||||
var send = await SendOrderToRobotAsync(robotId, route, isInitial: true);
|
||||
if (send)
|
||||
{
|
||||
// Create order state
|
||||
var orderState = new OrderACSState
|
||||
{
|
||||
RobotId = robotId,
|
||||
Status = OrderStatus.IsProccessing,
|
||||
Route = route,
|
||||
InMappedNodes = inMappedNodes,
|
||||
OutMappedNodes = outMappedNodes,
|
||||
CurrentInMappedNodeIndex = 0,
|
||||
CreatedAt = DateTime.UtcNow,
|
||||
LastUpdated = DateTime.UtcNow,
|
||||
ZoneRequestInCompleting = [],
|
||||
ZoneRequestOutCompleting = [],
|
||||
};
|
||||
|
||||
_orderStates.AddOrUpdate(robotId, orderState, (key, old) => orderState);
|
||||
|
||||
}
|
||||
|
||||
return send;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"CreateRobotOrderAsync: Error creating order for robot {robotId}: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private async Task<(List<(string NodeIdString, string ZoneId)> InMappedNodes, List<(string NodeIdString, string ZoneId)> OutMappedNodes)> FindMappedNodesAsync(RobotRoute route)
|
||||
{
|
||||
var inMappedNodes = new List<(string NodeIdString, string ZoneId)>();
|
||||
var outMappedNodes = new List<(string NodeIdString, string ZoneId)>();
|
||||
var acsZoneMapping = _acsTrafficConfig.ACSZoneMaping;
|
||||
var acsOutMapping = _acsTrafficConfig.ACSOutMaping;
|
||||
|
||||
// Get all unique NodeIds (Guid) from route segments
|
||||
var nodeIds = route.FullRoute
|
||||
.Where(s => s.VdaNode != null)
|
||||
.Select(s => s.NodeId)
|
||||
.Distinct()
|
||||
.ToList();
|
||||
|
||||
if (nodeIds.Count == 0)
|
||||
{
|
||||
return (inMappedNodes, outMappedNodes);
|
||||
}
|
||||
|
||||
// Create mapping: NodeId (Guid) -> NodeName
|
||||
var nodeIdToNodeName = new Dictionary<Guid, string?>();
|
||||
try
|
||||
{
|
||||
using var scope = _serviceScopeFactory.CreateScope();
|
||||
var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
||||
|
||||
foreach (var nodeId in nodeIds)
|
||||
{
|
||||
var node = await nodeService.GetByIdAsync(nodeId, includeVehicleProperties: false);
|
||||
if (node != null && !string.IsNullOrEmpty(node.NodeName))
|
||||
{
|
||||
nodeIdToNodeName[nodeId] = node.NodeName;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"FindMappedNodesAsync: Error getting NodeName from database: {ex.Message}");
|
||||
// Continue with empty mapping - will skip nodes without NodeName
|
||||
}
|
||||
|
||||
foreach (var segment in route.FullRoute)
|
||||
{
|
||||
if (segment.VdaNode != null)
|
||||
{
|
||||
var nodeIdString = segment.VdaNode.NodeId;
|
||||
|
||||
// Get NodeName from mapping (using NodeId Guid)
|
||||
if (!nodeIdToNodeName.TryGetValue(segment.NodeId, out var nodeName) || string.IsNullOrEmpty(nodeName))
|
||||
{
|
||||
// Skip if NodeName not found
|
||||
continue;
|
||||
}
|
||||
|
||||
// Check if node is in ACSZoneMapping (RequestIn) using NodeName
|
||||
if (acsZoneMapping.TryGetValue(nodeName, out var zoneId))
|
||||
{
|
||||
inMappedNodes.Add((nodeIdString, zoneId));
|
||||
}
|
||||
|
||||
// Check if node is in ACSOutMapping (RequestOut) using NodeName
|
||||
if (acsOutMapping.TryGetValue(nodeName, out var outZoneId))
|
||||
{
|
||||
outMappedNodes.Add((nodeIdString, outZoneId));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (inMappedNodes, outMappedNodes);
|
||||
}
|
||||
|
||||
private static int FindFirstMappedNodeIndex(RobotRoute route, List<(string NodeIdString, string ZoneId)> mappedNodes)
|
||||
{
|
||||
if (mappedNodes.Count == 0) return -1;
|
||||
|
||||
(string NodeIdString, _) = mappedNodes[0];
|
||||
|
||||
for (int i = 0; i < route.FullRoute.Count; i++)
|
||||
{
|
||||
var segment = route.FullRoute[i];
|
||||
if (segment.VdaNode != null && segment.VdaNode.NodeId == NodeIdString)
|
||||
{
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
return -1;
|
||||
}
|
||||
|
||||
private async Task CheckAndProcessMappedNodesAsync(string robotId, string lastNodeId, OrderACSState orderState, StateMsg stateMsg)
|
||||
{
|
||||
try
|
||||
{
|
||||
foreach (var (NodeIdString, ZoneId) in orderState.OutMappedNodes)
|
||||
{
|
||||
if (lastNodeId == NodeIdString && !string.IsNullOrEmpty(ZoneId) && !orderState.ZoneRequestOutCompleting.Contains(ZoneId) && !orderState.ZoneRequestOutCompleted.Contains(ZoneId))
|
||||
{
|
||||
orderState.ZoneRequestOutCompleting.Add(ZoneId);
|
||||
}
|
||||
}
|
||||
var requestOut = ProcessRequestOutAsync(robotId, orderState);
|
||||
|
||||
foreach (var (NodeIdString, ZoneId) in orderState.InMappedNodes)
|
||||
{
|
||||
if (lastNodeId == NodeIdString && !string.IsNullOrEmpty(ZoneId) && !orderState.ZoneRequestInCompleting.Contains(ZoneId) && !orderState.ZoneRequestInCompleted.Contains(ZoneId))
|
||||
{
|
||||
orderState.ZoneRequestInCompleting.Add(ZoneId);
|
||||
}
|
||||
}
|
||||
await ProcessRequestInAsync(robotId, orderState);
|
||||
|
||||
await requestOut.WaitAsync(CancellationToken.None);
|
||||
|
||||
// Check if order is completed
|
||||
CheckOrderCompletion(robotId, orderState, stateMsg);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"CheckAndProcessMappedNodes: Error for robot {robotId}: {ex.Message}");
|
||||
orderState.Status = OrderStatus.IsError;
|
||||
orderState.Error = ex.Message;
|
||||
}
|
||||
}
|
||||
|
||||
private async Task ProcessRequestInAsync(string robotId, OrderACSState orderState)
|
||||
{
|
||||
string[] zoneIdsIn = [.. orderState.ZoneRequestInCompleting];
|
||||
if (zoneIdsIn.Length == 0) return;
|
||||
foreach (var zoneId in zoneIdsIn)
|
||||
{
|
||||
try
|
||||
{
|
||||
|
||||
if (orderState.ZoneRequestInCompleted.Contains(zoneId)) continue;
|
||||
_logger.Info($"ProcessRequestInAsync: Robot {robotId} requesting into zone {zoneId}");
|
||||
|
||||
var result = await _trafficACS.RequestIn(robotId, zoneId);
|
||||
|
||||
if (result.IsSuccess && result.Data)
|
||||
{
|
||||
// RequestIn successful - save zone to cache for future RequestOut validation
|
||||
orderState.ZoneRequestInCompleted.Add(zoneId);
|
||||
orderState.ZoneRequestInCompleting.Remove(zoneId);
|
||||
orderState.ZoneRequestOutCompleted.Remove(zoneId);
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
|
||||
_logger.Info($"ProcessRequestInAsync: Robot {robotId} successfully requested into zone {zoneId}");
|
||||
}
|
||||
else
|
||||
{
|
||||
// RequestIn failed, just log warning - will retry on next timer cycle
|
||||
_logger.Warning($"ProcessRequestInAsync: Robot {robotId} failed to request into zone {zoneId}: {result.Message}. Will retry on next cycle.");
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"ProcessRequestInAsync: Error for robot {robotId}, zone {zoneId}: {ex.Message}. Will retry on next cycle.");
|
||||
}
|
||||
}
|
||||
if (orderState.ZoneRequestInCompleting.Count == 0)
|
||||
{
|
||||
orderState.CurrentInMappedNodeIndex++;
|
||||
await TryReleaseNextHorizonSegmentAsync(robotId, orderState);
|
||||
}
|
||||
}
|
||||
|
||||
private async Task ProcessRequestOutAsync(string robotId, OrderACSState orderState)
|
||||
{
|
||||
string[] zoneIdsOut = [.. orderState.ZoneRequestOutCompleting];
|
||||
if (zoneIdsOut.Length == 0) return;
|
||||
foreach (var zoneId in zoneIdsOut)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (orderState.ZoneRequestOutCompleted.Contains(zoneId)) continue;
|
||||
_logger.Info($"ProcessRequestOutAsync: Robot {robotId} requesting out of zone {zoneId}");
|
||||
|
||||
var result = await _trafficACS.RequestOut(robotId, zoneId);
|
||||
|
||||
if (result.IsSuccess && result.Data)
|
||||
{
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
orderState.ZoneRequestOutCompleted.Add(zoneId);
|
||||
orderState.ZoneRequestOutCompleting.Remove(zoneId);
|
||||
orderState.ZoneRequestInCompleted.Remove(zoneId);
|
||||
|
||||
_logger.Info($"ProcessRequestOutAsync: Robot {robotId} successfully requested out of zone {zoneId}");
|
||||
// Note: OUT does NOT affect Base/Horizon, so we don't call TryReleaseNextHorizonSegmentAsync
|
||||
}
|
||||
else
|
||||
{
|
||||
// RequestOut failed - will retry on next timer cycle until successful
|
||||
// OUT must retry until successful (unlike IN which can be skipped)
|
||||
_logger.Warning($"ProcessRequestOutAsync: Robot {robotId} failed to request out of zone {zoneId}: {result.Message}. Will retry on next cycle until successful.");
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// RequestOut error - will retry on next timer cycle until successful
|
||||
_logger.Error($"ProcessRequestOutAsync: Error for robot {robotId}, zone {zoneId}: {ex.Message}. Will retry on next cycle until successful.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private async Task TryReleaseNextHorizonSegmentAsync(string robotId, OrderACSState orderState)
|
||||
{
|
||||
try
|
||||
{
|
||||
bool horizonUpdated = false;
|
||||
int baseCountBeforeRelease = orderState.Route.Base.Count; // Store base count before release
|
||||
|
||||
// Check if there are more IN mapped nodes to process (only IN affects Base/Horizon)
|
||||
if (orderState.CurrentInMappedNodeIndex >= orderState.InMappedNodes.Count)
|
||||
{
|
||||
// All mapped nodes processed, check if we can release all remaining horizon
|
||||
if (orderState.Route.Horizon.Count > 0)
|
||||
{
|
||||
// Move all remaining horizon to base
|
||||
var segmentsToMove = orderState.Route.Horizon.ToList();
|
||||
orderState.Route.Base.AddRange(segmentsToMove);
|
||||
orderState.Route.Horizon.Clear();
|
||||
|
||||
// Only set Released = true for newly released segments
|
||||
foreach (var segment in segmentsToMove)
|
||||
{
|
||||
segment.Released = true;
|
||||
}
|
||||
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
horizonUpdated = true;
|
||||
_logger.Info($"TryReleaseNextHorizonSegmentAsync: Released all remaining horizon segments for robot {robotId}");
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Find next IN mapped node
|
||||
var (NodeIdString, _) = orderState.InMappedNodes[orderState.CurrentInMappedNodeIndex];
|
||||
|
||||
// Find index of next mapped node in full route
|
||||
int nextMappedNodeIndex = -1;
|
||||
for (int i = 0; i < orderState.Route.FullRoute.Count; i++)
|
||||
{
|
||||
var segment = orderState.Route.FullRoute[i];
|
||||
if (segment.VdaNode != null && segment.VdaNode.NodeId == NodeIdString)
|
||||
{
|
||||
nextMappedNodeIndex = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (nextMappedNodeIndex >= 0)
|
||||
{
|
||||
// Calculate how many segments to release: from current base end to next mapped node + 1
|
||||
var currentBaseEndIndex = orderState.Route.Base.Count;
|
||||
var segmentsToRelease = nextMappedNodeIndex - currentBaseEndIndex + 3;
|
||||
|
||||
if (segmentsToRelease > 0 && segmentsToRelease <= orderState.Route.Horizon.Count)
|
||||
{
|
||||
// Release segments to base
|
||||
var segmentsToMove = orderState.Route.Horizon.Take(segmentsToRelease).ToList();
|
||||
orderState.Route.Base.AddRange(segmentsToMove);
|
||||
orderState.Route.Horizon.RemoveRange(0, segmentsToRelease);
|
||||
|
||||
foreach (var segment in segmentsToMove)
|
||||
{
|
||||
segment.Released = true;
|
||||
}
|
||||
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
horizonUpdated = true;
|
||||
_logger.Info($"TryReleaseNextHorizonSegmentAsync: Released {segmentsToRelease} segments to base for robot {robotId}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// If horizon was updated, send OrderUpdate to robot
|
||||
if (horizonUpdated)
|
||||
{
|
||||
await SendOrderToRobotAsync(robotId, orderState.Route, isInitial: false, baseCountBeforeRelease);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"TryReleaseNextHorizonSegmentAsync: Error for robot {robotId}: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
private async Task<bool> SendOrderToRobotAsync(string robotId, RobotRoute route, bool isInitial, int baseCountBeforeRelease = -1)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Get robot controller using service scope to avoid circular dependency
|
||||
IRobotController? robotController = null;
|
||||
try
|
||||
{
|
||||
using var scope = _serviceScopeFactory.CreateScope();
|
||||
var robotManagerService = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
||||
robotController = robotManagerService.GetRobotController(robotId);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"SendOrderToRobotAsync: Error getting robot controller for {robotId}: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (robotController == null)
|
||||
{
|
||||
_logger.Warning($"SendOrderToRobotAsync: Robot controller not found for robot {robotId}");
|
||||
return false;
|
||||
}
|
||||
|
||||
// Build nodes and edges
|
||||
var nodes = new List<Node>();
|
||||
var edges = new List<Edge>();
|
||||
|
||||
if (isInitial)
|
||||
{
|
||||
// Initial order: send ALL Base + Horizon segments
|
||||
// Add Base segments (released = true)
|
||||
foreach (var segment in route.Base)
|
||||
{
|
||||
if (segment.VdaNode != null)
|
||||
{
|
||||
var node = CloneNode(segment.VdaNode);
|
||||
node.Released = true; // Base segments are always released
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
if (segment.VdaEdge != null)
|
||||
{
|
||||
var edge = CloneEdge(segment.VdaEdge);
|
||||
edge.Released = true; // Base segments are always released
|
||||
edges.Add(edge);
|
||||
}
|
||||
}
|
||||
|
||||
// Add Horizon segments (released = false)
|
||||
foreach (var segment in route.Horizon)
|
||||
{
|
||||
if (segment.VdaNode != null)
|
||||
{
|
||||
var node = CloneNode(segment.VdaNode);
|
||||
node.Released = false; // Horizon segments are not released
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
if (segment.VdaEdge != null)
|
||||
{
|
||||
var edge = CloneEdge(segment.VdaEdge);
|
||||
edge.Released = false; // Horizon segments are not released
|
||||
edges.Add(edge);
|
||||
}
|
||||
}
|
||||
|
||||
// Create initial Order
|
||||
var order = new OrderMsg
|
||||
{
|
||||
OrderId = route.OrderId,
|
||||
OrderUpdateId = 0,
|
||||
SerialNumber = robotId,
|
||||
Timestamp = DateTime.UtcNow,
|
||||
Nodes = [.. nodes],
|
||||
Edges = [.. edges]
|
||||
};
|
||||
|
||||
var result = await robotController.SendOrderAsync(order);
|
||||
if (result.IsSuccess)
|
||||
{
|
||||
route.OrderUpdateId = order.OrderUpdateId;
|
||||
_logger.Info($"SendOrderToRobotAsync: Successfully sent initial Order (ID: {route.OrderId}, UpdateID: {order.OrderUpdateId}) to robot {robotId}");
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warning($"SendOrderToRobotAsync: Failed to send initial order to robot {robotId}: {result.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// OrderUpdate: Send stitching node + new Base segments + new Horizon segments
|
||||
// 1. Stitching node: last node of old Base (before release)
|
||||
// 2. New Base: segments that were just released from Horizon
|
||||
// 3. New Horizon: remaining segments in Horizon
|
||||
|
||||
if (baseCountBeforeRelease < 0)
|
||||
{
|
||||
// Fallback: use current Base.Count - 1 (assume only 1 segment was released)
|
||||
// This shouldn't happen if called from TryReleaseNextHorizonSegmentAsync
|
||||
baseCountBeforeRelease = Math.Max(0, route.Base.Count - 1);
|
||||
}
|
||||
|
||||
// Get stitching node: last node of Base before release
|
||||
var oldBaseSegments = route.Base.Take(baseCountBeforeRelease).ToList();
|
||||
var lastOldBaseNode = oldBaseSegments.LastOrDefault(s => s.VdaNode != null);
|
||||
if (lastOldBaseNode?.VdaNode == null)
|
||||
{
|
||||
_logger.Warning($"SendOrderToRobotAsync: Cannot create OrderUpdate for robot {robotId}: no old base node found");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 1. Add stitching node (last node of old Base, released = true)
|
||||
var stitchingNode = CloneNode(lastOldBaseNode.VdaNode);
|
||||
stitchingNode.Released = true;
|
||||
nodes.Add(stitchingNode);
|
||||
|
||||
// 2. Add new Base segments (segments that were just released, released = true)
|
||||
var newBaseSegments = route.Base.Skip(baseCountBeforeRelease).ToList();
|
||||
foreach (var segment in newBaseSegments)
|
||||
{
|
||||
if (segment.VdaNode != null)
|
||||
{
|
||||
var node = CloneNode(segment.VdaNode);
|
||||
node.Released = true; // New Base segments are released
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
if (segment.VdaEdge != null)
|
||||
{
|
||||
var edge = CloneEdge(segment.VdaEdge);
|
||||
edge.Released = true; // New Base segments are released
|
||||
edges.Add(edge);
|
||||
}
|
||||
}
|
||||
|
||||
// 3. Add new Horizon segments (remaining segments in Horizon, released = false)
|
||||
foreach (var segment in route.Horizon)
|
||||
{
|
||||
if (segment.VdaNode != null)
|
||||
{
|
||||
var node = CloneNode(segment.VdaNode);
|
||||
node.Released = false; // Horizon segments are not released
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
if (segment.VdaEdge != null)
|
||||
{
|
||||
var edge = CloneEdge(segment.VdaEdge);
|
||||
edge.Released = false; // Horizon segments are not released
|
||||
edges.Add(edge);
|
||||
}
|
||||
}
|
||||
|
||||
// Get current order to increment OrderUpdateId
|
||||
var currentOrder = robotController.Data.Order;
|
||||
var orderUpdateId = currentOrder?.OrderUpdateId ?? 0;
|
||||
|
||||
var orderUpdate = new OrderMsg
|
||||
{
|
||||
OrderId = route.OrderId,
|
||||
OrderUpdateId = orderUpdateId + 1,
|
||||
SerialNumber = robotId,
|
||||
Timestamp = DateTime.UtcNow,
|
||||
Nodes = [.. nodes],
|
||||
Edges = [.. edges]
|
||||
};
|
||||
|
||||
var result = await robotController.SendOrderAsync(orderUpdate);
|
||||
if (result.IsSuccess)
|
||||
{
|
||||
route.OrderUpdateId = orderUpdate.OrderUpdateId;
|
||||
_logger.Info($"SendOrderToRobotAsync: Successfully sent OrderUpdate (ID: {route.OrderId}, UpdateID: {orderUpdate.OrderUpdateId}) to robot {robotId}");
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
_logger.Warning($"SendOrderToRobotAsync: Failed to send OrderUpdate to robot {robotId}: {result.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"SendOrderToRobotAsync: Error sending order to robot {robotId}: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private static Node CloneNode(Node source)
|
||||
{
|
||||
return new Node
|
||||
{
|
||||
NodeId = source.NodeId,
|
||||
SequenceId = source.SequenceId,
|
||||
Released = source.Released,
|
||||
NodeDescription = source.NodeDescription,
|
||||
NodePosition = source.NodePosition is null ? null : new NodePosition
|
||||
{
|
||||
X = source.NodePosition.X,
|
||||
Y = source.NodePosition.Y,
|
||||
Theta = source.NodePosition.Theta,
|
||||
AllowedDeviationXY = source.NodePosition.AllowedDeviationXY,
|
||||
AllowedDeviationTheta = source.NodePosition.AllowedDeviationTheta,
|
||||
MapId = source.NodePosition.MapId,
|
||||
MapDescription = source.NodePosition.MapDescription
|
||||
},
|
||||
Actions = source.Actions?.Select(a => CloneAction(a)).ToArray() ?? [] // Clone each action
|
||||
};
|
||||
}
|
||||
|
||||
private static RobotNet.VDA5050.InstantAction.Action CloneAction(RobotNet.VDA5050.InstantAction.Action source)
|
||||
{
|
||||
return new RobotNet.VDA5050.InstantAction.Action
|
||||
{
|
||||
ActionType = source.ActionType,
|
||||
ActionId = source.ActionId,
|
||||
ActionDescription = source.ActionDescription,
|
||||
BlockingType = source.BlockingType,
|
||||
ActionParameters = source.ActionParameters?.Select(p => new RobotNet.VDA5050.InstantAction.ActionParameter
|
||||
{
|
||||
Key = p.Key,
|
||||
Value = p.Value
|
||||
}).ToArray() ?? null
|
||||
};
|
||||
}
|
||||
|
||||
private static Edge CloneEdge(Edge source)
|
||||
{
|
||||
return new Edge
|
||||
{
|
||||
EdgeId = source.EdgeId,
|
||||
SequenceId = source.SequenceId,
|
||||
Released = source.Released,
|
||||
EdgeDescription = source.EdgeDescription,
|
||||
StartNodeId = source.StartNodeId,
|
||||
EndNodeId = source.EndNodeId,
|
||||
MaxSpeed = source.MaxSpeed,
|
||||
MaxHeight = source.MaxHeight,
|
||||
MinHeight = source.MinHeight,
|
||||
Orientation = source.Orientation,
|
||||
OrientationType = source.OrientationType,
|
||||
Direction = source.Direction,
|
||||
RotationAllowed = source.RotationAllowed,
|
||||
MaxRotationSpeed = source.MaxRotationSpeed,
|
||||
Length = source.Length,
|
||||
Trajectory = source.Trajectory == null ? null : new Trajectory
|
||||
{
|
||||
Degree = source.Trajectory.Degree,
|
||||
KnotVector = [.. source.Trajectory.KnotVector], // Clone array
|
||||
ControlPoints = [.. source.Trajectory.ControlPoints.Select(cp => new ControlPoint
|
||||
{
|
||||
X = cp.X,
|
||||
Y = cp.Y,
|
||||
Weight = cp.Weight
|
||||
})]
|
||||
},
|
||||
Corridor = source.Corridor == null ? null : new Corridor
|
||||
{
|
||||
LeftWidth = source.Corridor.LeftWidth,
|
||||
RightWidth = source.Corridor.RightWidth,
|
||||
CorridorRefPoint = source.Corridor.CorridorRefPoint
|
||||
},
|
||||
Actions = source.Actions?.Select(a => CloneAction(a)).ToArray() ?? [] // Clone each action
|
||||
};
|
||||
}
|
||||
|
||||
private void CheckOrderCompletion(string robotId, OrderACSState orderState, StateMsg stateMsg)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Check if NodeStates and EdgeStates are empty
|
||||
var nodeStatesEmpty = stateMsg.NodeStates == null || stateMsg.NodeStates.Length == 0;
|
||||
var edgeStatesEmpty = stateMsg.EdgeStates == null || stateMsg.EdgeStates.Length == 0;
|
||||
|
||||
if (!nodeStatesEmpty || !edgeStatesEmpty)
|
||||
{
|
||||
// Still processing, not completed yet
|
||||
return;
|
||||
}
|
||||
|
||||
// NodeStates and EdgeStates are empty - check completion conditions
|
||||
var lastNodeInRoute = orderState.Route.FullRoute.LastOrDefault()?.VdaNode?.NodeId;
|
||||
var lastNodeId = stateMsg.LastNodeId;
|
||||
|
||||
// Check if LastNodeId matches the last node in route
|
||||
bool isAtLastNode = lastNodeId == lastNodeInRoute;
|
||||
|
||||
// Check actions on last node - get last node's actions from route
|
||||
bool allActionsFinished = true;
|
||||
bool hasActionFailed = false;
|
||||
|
||||
if (stateMsg.ActionStates != null && stateMsg.ActionStates.Length > 0)
|
||||
{
|
||||
// Get actions from last node in route
|
||||
var lastNodeSegment = orderState.Route.FullRoute.LastOrDefault(s => s.VdaNode != null);
|
||||
if (lastNodeSegment?.VdaNode?.Actions != null && lastNodeSegment.VdaNode.Actions.Length > 0)
|
||||
{
|
||||
// Check if all actions from last node are finished
|
||||
var lastNodeActionIds = lastNodeSegment.VdaNode.Actions.Select(a => a.ActionId).ToHashSet();
|
||||
var lastNodeActionStates = stateMsg.ActionStates
|
||||
.Where(a => lastNodeActionIds.Contains(a.ActionId))
|
||||
.ToList();
|
||||
|
||||
if (lastNodeActionStates.Count > 0)
|
||||
{
|
||||
foreach (var actionState in lastNodeActionStates)
|
||||
{
|
||||
if (actionState.ActionStatus == RobotNet.VDA5050.Type.ActionStatus.FAILED)
|
||||
{
|
||||
hasActionFailed = true;
|
||||
allActionsFinished = false;
|
||||
break;
|
||||
}
|
||||
else if (actionState.ActionStatus != RobotNet.VDA5050.Type.ActionStatus.FINISHED)
|
||||
{
|
||||
allActionsFinished = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Determine order status
|
||||
if (isAtLastNode && allActionsFinished)
|
||||
{
|
||||
if (orderState.Status != OrderStatus.IsCompleted) _logger.Info($"CheckOrderCompletion: Order completed successfully for robot {robotId}");
|
||||
// Order completed successfully
|
||||
orderState.Status = OrderStatus.IsCompleted;
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
}
|
||||
else if (!isAtLastNode || hasActionFailed)
|
||||
{
|
||||
// Order error: not at last node or action failed
|
||||
orderState.Status = OrderStatus.IsError;
|
||||
if (!isAtLastNode)
|
||||
{
|
||||
orderState.Error = $"Robot {robotId} finished at node {lastNodeId} but expected last node {lastNodeInRoute}";
|
||||
}
|
||||
else if (hasActionFailed)
|
||||
{
|
||||
orderState.Error = $"Robot {robotId} has failed actions on last node {lastNodeId}";
|
||||
}
|
||||
orderState.LastUpdated = DateTime.UtcNow;
|
||||
_logger.Warning($"CheckOrderCompletion: Order error for robot {robotId}: {orderState.Error}");
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.Error($"CheckOrderCompletion: Error for robot {robotId}: {ex.Message}");
|
||||
orderState.Status = OrderStatus.IsError;
|
||||
orderState.Error = ex.Message;
|
||||
}
|
||||
}
|
||||
|
||||
public RobotRoute? GetRobotRoute(string robotId)
|
||||
{
|
||||
if (_orderStates.TryGetValue(robotId, out var state))
|
||||
{
|
||||
return state.Route;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user