431 lines
17 KiB
C#
431 lines
17 KiB
C#
using RobotNet.VDA5050.State;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.FleetManager.Controllers;
|
|
using RobotNet10.FleetManager.Events;
|
|
using RobotNet10.FleetManager.Events.Events;
|
|
using RobotNet10.FleetManager.Script;
|
|
using RobotNet10.FleetManager.Services.ConfigManager;
|
|
using RobotNet10.FleetManager.Services.RobotConnections;
|
|
using RobotNet10.FleetManager.Services.RobotController;
|
|
using RobotNet10.FleetManager.Services.RobotManager.Models;
|
|
using RobotNet10.FleetManager.Services.TrafficControl;
|
|
using RobotNet10.MapManager.Services;
|
|
using System.Collections.Concurrent;
|
|
using System.Data;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace RobotNet10.FleetManager.Services.RobotManager;
|
|
|
|
/// <summary>
|
|
/// Service implementation for managing RobotController instances and routing events
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// This service:
|
|
/// - Manages RobotController instances per robot (instance per robot)
|
|
/// - Subscribes to Event Bus events and routes to RobotController
|
|
/// - Auto-creates RobotController when receiving first Connection/State message
|
|
/// - Timeout monitoring: 30s no State/Visualization → OFFLINE
|
|
/// </remarks>
|
|
public class RobotManagerService : BackgroundService, IRobotManagerService
|
|
{
|
|
private readonly IRobotEventBus _eventBus;
|
|
private readonly IRobotConnectionsService _robotConnectionsService;
|
|
private readonly IConnectionConfig _configManager;
|
|
private readonly ITrafficControlService _trafficControlService;
|
|
private readonly IServiceScopeFactory _serviceScopeFactory;
|
|
private readonly ILoggerFactory _loggerFactory;
|
|
private readonly Logger<RobotManagerService> _logger;
|
|
private readonly Logger<RobotController.RobotController> _loggerRobotController;
|
|
|
|
// RobotController instances - thread-safe dictionary
|
|
private readonly ConcurrentDictionary<string, IRobotController> _robotControllers = new();
|
|
|
|
// Timeout tracking - last update time for State and Visualization per robot
|
|
private readonly ConcurrentDictionary<string, (DateTime? lastStateUpdate, DateTime? lastVisualizationUpdate)> _lastUpdateTimes = new();
|
|
|
|
// Timeout monitoring timer
|
|
private WatchTimerAsync<RobotManagerService>? _timeoutTimer;
|
|
private const int TimeoutCheckIntervalMs = 15000; // 15 seconds
|
|
private const int TimeoutThresholdSeconds = 30; // 30 seconds
|
|
|
|
public RobotManagerService(
|
|
IRobotEventBus eventBus,
|
|
IRobotConnectionsService robotConnectionsService,
|
|
IConnectionConfig configManager,
|
|
ITrafficControlService trafficControlService,
|
|
IServiceScopeFactory serviceScopeFactory,
|
|
ILoggerFactory loggerFactory,
|
|
Logger<RobotManagerService> logger,
|
|
Logger<RobotController.RobotController> loggerRobotController)
|
|
{
|
|
_eventBus = eventBus;
|
|
_robotConnectionsService = robotConnectionsService;
|
|
_configManager = configManager;
|
|
_trafficControlService = trafficControlService;
|
|
_serviceScopeFactory = serviceScopeFactory;
|
|
_loggerFactory = loggerFactory;
|
|
_logger = logger;
|
|
_loggerRobotController = loggerRobotController;
|
|
|
|
// Subscribe to Event Bus events
|
|
_eventBus.StateMessageReceived += OnStateMessageReceived;
|
|
_eventBus.ConnectionStateChanged += OnConnectionStateChanged;
|
|
_eventBus.VisualizationMessageReceived += OnVisualizationMessageReceived;
|
|
_eventBus.FactsheetMessageReceived += OnFactsheetMessageReceived;
|
|
}
|
|
|
|
public IRobotController? GetRobotController(string robotId)
|
|
{
|
|
_robotControllers.TryGetValue(robotId, out var controller);
|
|
return controller;
|
|
}
|
|
|
|
public IReadOnlyDictionary<string, IRobotController> GetAllRobotControllers()
|
|
{
|
|
return _robotControllers;
|
|
}
|
|
|
|
public void RemoveRobotController(string robotId)
|
|
{
|
|
try
|
|
{
|
|
if (_robotControllers.TryRemove(robotId, out var controller))
|
|
{
|
|
controller.Dispose();
|
|
_lastUpdateTimes.TryRemove(robotId, out _);
|
|
_logger.Info($"Removed RobotController for robot {robotId}");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error removing RobotController for robot {robotId}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
public RobotData? GetRobotData(string robotId)
|
|
{
|
|
var controller = GetRobotController(robotId);
|
|
return controller?.Data;
|
|
}
|
|
|
|
public IReadOnlyDictionary<string, RobotData> GetAllRobotData()
|
|
{
|
|
return _robotControllers.ToDictionary(
|
|
kvp => kvp.Key,
|
|
kvp => kvp.Value.Data
|
|
);
|
|
}
|
|
|
|
public IReadOnlyList<string> GetAvailableRobots()
|
|
{
|
|
return [.. _robotControllers
|
|
.Where(kvp => kvp.Value.IsOnline)
|
|
.Select(kvp => kvp.Key)];
|
|
}
|
|
|
|
private static readonly TimeSpan OrderClearedByCancelWindow = TimeSpan.FromSeconds(15);
|
|
|
|
private void OnStateMessageReceived(object? sender, StateMessageReceivedEvent e)
|
|
{
|
|
try
|
|
{
|
|
var robotId = e.RobotId;
|
|
var stateMsg = e.StateMessage;
|
|
|
|
// Get or create RobotController
|
|
var controller = GetOrCreateRobotController(robotId);
|
|
if (controller == null) return;
|
|
|
|
// Update State in RobotData
|
|
controller.Data.State = stateMsg;
|
|
controller.Data.LastUpdated = DateTime.UtcNow;
|
|
|
|
var hasOrderState = (stateMsg.NodeStates?.Length ?? 0) > 0 || (stateMsg.EdgeStates?.Length ?? 0) > 0;
|
|
var cancelOrderTimedOut = stateMsg.ActionStates?.Any(a =>
|
|
(string.Equals(a.ActionType, "CANCEL_ORDER", StringComparison.OrdinalIgnoreCase) || a.ActionType?.Contains("cancelOrder", StringComparison.OrdinalIgnoreCase) == true) &&
|
|
a.ActionStatus == ActionStatus.FAILED &&
|
|
(a.ResultDescription?.Contains("Timeout", StringComparison.OrdinalIgnoreCase) ?? false)) == true;
|
|
|
|
// Force-clear order state when cancel was requested but timed out on robot (so FleetManager still shows robot as not busy)
|
|
if (hasOrderState && cancelOrderTimedOut)
|
|
{
|
|
controller.Data.State.NodeStates = [];
|
|
controller.Data.State.EdgeStates = [];
|
|
controller.Data.State.OrderId = string.Empty;
|
|
controller.Data.State.OrderUpdateId = 0;
|
|
}
|
|
// After cancel we clear order state locally; don't let stale robot state overwrite it until robot reports empty or window expires
|
|
else
|
|
{
|
|
var clearedAt = controller.Data.OrderClearedByCancelAt;
|
|
if (clearedAt.HasValue)
|
|
{
|
|
var elapsed = DateTime.UtcNow - clearedAt.Value;
|
|
if (elapsed >= OrderClearedByCancelWindow)
|
|
{
|
|
controller.Data.OrderClearedByCancelAt = null;
|
|
}
|
|
else if (hasOrderState)
|
|
{
|
|
// Robot hasn't reported empty yet; keep order state cleared so new order can be accepted
|
|
controller.Data.State.NodeStates = [];
|
|
controller.Data.State.EdgeStates = [];
|
|
controller.Data.State.OrderId = string.Empty;
|
|
controller.Data.State.OrderUpdateId = 0;
|
|
}
|
|
else
|
|
{
|
|
controller.Data.OrderClearedByCancelAt = null;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Update last State update time
|
|
if(controller.Data.ConnectionState != RobotNet.VDA5050.Type.ConnectionState.ONLINE) controller.Data.ConnectionState = RobotNet.VDA5050.Type.ConnectionState.ONLINE;
|
|
UpdateLastStateTime(robotId);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error processing state message for robot {e.RobotId}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void OnConnectionStateChanged(object? sender, ConnectionStateChangedEvent e)
|
|
{
|
|
try
|
|
{
|
|
var robotId = e.RobotId;
|
|
var connectionState = e.ConnectionState;
|
|
|
|
// Get or create RobotController
|
|
var controller = GetOrCreateRobotController(robotId);
|
|
if (controller == null) return;
|
|
|
|
// Update Connection State in RobotData
|
|
controller.Data.ConnectionState = connectionState;
|
|
controller.Data.LastUpdated = DateTime.UtcNow;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error processing connection state change for robot {e.RobotId}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void OnVisualizationMessageReceived(object? sender, VisualizationMessageReceivedEvent e)
|
|
{
|
|
try
|
|
{
|
|
var robotId = e.RobotId;
|
|
var visualizationMsg = e.VisualizationMessage;
|
|
|
|
// Get or create RobotController
|
|
var controller = GetOrCreateRobotController(robotId);
|
|
if (controller == null) return;
|
|
|
|
// Update Visualization in RobotData
|
|
controller.Data.Visualization = visualizationMsg;
|
|
controller.Data.LastUpdated = DateTime.UtcNow;
|
|
|
|
// Update last Visualization update time
|
|
UpdateLastVisualizationTime(robotId);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error processing visualization message for robot {e.RobotId}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void OnFactsheetMessageReceived(object? sender, FactsheetMessageReceivedEvent e)
|
|
{
|
|
try
|
|
{
|
|
var robotId = e.RobotId;
|
|
var factsheetMsg = e.FactsheetMessage;
|
|
|
|
// Get or create RobotController
|
|
var controller = GetOrCreateRobotController(robotId);
|
|
if (controller == null) return;
|
|
|
|
// Update Factsheet in RobotData
|
|
controller.Data.Factsheet = factsheetMsg;
|
|
controller.Data.LastUpdated = DateTime.UtcNow;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error processing factsheet message for robot {e.RobotId}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private IRobotController? GetOrCreateRobotController(string robotId)
|
|
{
|
|
// Check if already exists
|
|
if (_robotControllers.TryGetValue(robotId, out var existingController))
|
|
{
|
|
return existingController;
|
|
}
|
|
|
|
// Validate robot exists in database
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var _robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
|
if (!_robotService.ExistsAsync(robotId).Result)
|
|
{
|
|
_logger.Warning($"Robot {robotId} not found in database, skipping RobotController creation");
|
|
return null;
|
|
}
|
|
|
|
// Create new RobotController instance
|
|
try
|
|
{
|
|
var controller = new Services.RobotController.RobotController(
|
|
robotId,
|
|
_robotConnectionsService,
|
|
_configManager,
|
|
_trafficControlService,
|
|
_serviceScopeFactory,
|
|
_loggerRobotController
|
|
);
|
|
|
|
if (_robotControllers.TryAdd(robotId, controller))
|
|
{
|
|
_lastUpdateTimes.TryAdd(robotId, (null, null));
|
|
_logger.Info($"Created RobotController for robot {robotId}");
|
|
return controller;
|
|
}
|
|
else
|
|
{
|
|
// Another thread created it, dispose this one and get the existing
|
|
((IDisposable)controller).Dispose();
|
|
_robotControllers.TryGetValue(robotId, out var createdController);
|
|
return createdController;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error creating RobotController for robot {robotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
public async Task<IReadOnlyList<string>> GetAvailableRobots(string layout, string version, string level, string model, Func<RobotState, bool> func)
|
|
{
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var _robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
|
var _layoutService = scope.ServiceProvider.GetRequiredService<ILayoutService>();
|
|
|
|
var layoutDb = await _layoutService.GetLayoutByNameAsync(layout);
|
|
if (layoutDb is null) return [];
|
|
var levelDb = layoutDb.Versions.FirstOrDefault(v => v.Version == version)?.Levels.FirstOrDefault(l => l.LayoutLevelId == level);
|
|
if (levelDb is null) return [];
|
|
|
|
var robotDbs = await _robotService.GetByModelNameAsync(model);
|
|
if (robotDbs is null) return [];
|
|
|
|
var robotDbinMap = robotDbs.Where(r => r.MapId == levelDb.Id);
|
|
List<IRobotController> robotControllers = [.. _robotControllers.Where(kvp => kvp.Value.IsOnline && robotDbinMap.Any(r => r.RobotId == kvp.Key)).Select(kvp => kvp.Value)];
|
|
return [..robotControllers.Where(r => r.IsOnline && r.Data is not null && r.Data.State is not null && r.Data.Visualization is not null && func(ToRobotState(r))).Select(r => r.RobotId)];
|
|
}
|
|
|
|
private static RobotState ToRobotState(IRobotController robotController)
|
|
{
|
|
return new RobotState(robotController.IsReady,
|
|
robotController.Data.State?.BatteryState.BatteryVoltage ?? 0,
|
|
robotController.Data.State?.Loads ?? [],
|
|
robotController.Data.State?.BatteryState.Charging ?? false,
|
|
robotController.Data.Visualization?.AgvPosition.X ?? 0,
|
|
robotController.Data.Visualization?.AgvPosition.Y ?? 0,
|
|
robotController.Data.Visualization?.AgvPosition.Theta ?? 0);
|
|
}
|
|
|
|
private void UpdateLastStateTime(string robotId)
|
|
{
|
|
_lastUpdateTimes.AddOrUpdate(robotId,
|
|
(DateTime.UtcNow, null),
|
|
(key, oldValue) => (DateTime.UtcNow, oldValue.lastVisualizationUpdate));
|
|
}
|
|
|
|
private void UpdateLastVisualizationTime(string robotId)
|
|
{
|
|
_lastUpdateTimes.AddOrUpdate(robotId,
|
|
(null, DateTime.UtcNow),
|
|
(key, oldValue) => (oldValue.lastStateUpdate, DateTime.UtcNow));
|
|
}
|
|
|
|
private async Task CheckTimeouts()
|
|
{
|
|
try
|
|
{
|
|
var now = DateTime.UtcNow;
|
|
var robotsToCheck = _robotControllers.Keys.ToList();
|
|
|
|
foreach (var robotId in robotsToCheck)
|
|
{
|
|
if (!_lastUpdateTimes.TryGetValue(robotId, out var updateTimes))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var stateTimeout = updateTimes.lastStateUpdate.HasValue &&
|
|
(now - updateTimes.lastStateUpdate.Value).TotalSeconds > TimeoutThresholdSeconds;
|
|
|
|
var visualizationTimeout = updateTimes.lastVisualizationUpdate.HasValue &&
|
|
(now - updateTimes.lastVisualizationUpdate.Value).TotalSeconds > TimeoutThresholdSeconds;
|
|
|
|
// If both State and Visualization timeout, set OFFLINE
|
|
if (stateTimeout && visualizationTimeout)
|
|
{
|
|
if (_robotControllers.TryGetValue(robotId, out var controller))
|
|
{
|
|
if (controller.Data.ConnectionState != RobotNet.VDA5050.Type.ConnectionState.OFFLINE)
|
|
{
|
|
controller.Data.ConnectionState = RobotNet.VDA5050.Type.ConnectionState.OFFLINE;
|
|
controller.Data.LastUpdated = now;
|
|
_logger.Warning($"Robot {robotId} timed out (30s no State/Visualization), set to OFFLINE");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error in timeout check: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
|
{
|
|
await Task.Yield();
|
|
await _robotConnectionsService.StartAsync(stoppingToken);
|
|
|
|
_timeoutTimer = new WatchTimerAsync<RobotManagerService>(
|
|
TimeoutCheckIntervalMs,
|
|
CheckTimeouts,
|
|
_loggerFactory.CreateLogger<RobotManagerService>()
|
|
);
|
|
_timeoutTimer.Start();
|
|
_logger.Info("Started timeout monitoring (15s interval, 30s threshold)");
|
|
}
|
|
|
|
public override Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
_timeoutTimer?.Dispose();
|
|
|
|
// Dispose all RobotController instances
|
|
foreach (var controller in _robotControllers.Values)
|
|
{
|
|
try
|
|
{
|
|
controller.Dispose();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error disposing RobotController: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
_robotControllers.Clear();
|
|
_lastUpdateTimes.Clear();
|
|
return base.StopAsync(cancellationToken);
|
|
}
|
|
}
|