924 lines
38 KiB
C#
924 lines
38 KiB
C#
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;
|
|
}
|
|
}
|