378 lines
14 KiB
C#
378 lines
14 KiB
C#
using RobotNet10.FleetManager.Events;
|
|
using RobotNet10.FleetManager.Events.Events;
|
|
using RobotNet10.FleetManager.Services.ConfigManager;
|
|
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
|
using RobotNet10.FleetManager.Services.TrafficControl.Services;
|
|
|
|
namespace RobotNet10.FleetManager.Services.TrafficControl;
|
|
|
|
/// <summary>
|
|
/// Orchestrator service for traffic control and conflict management between robots
|
|
/// Delegates to specialized sub-services for actual implementation
|
|
/// </summary>
|
|
public class TrafficControlService : BackgroundService, ITrafficControlService
|
|
{
|
|
private readonly Logger<TrafficControlService> _logger;
|
|
private readonly ITrafficConfig _trafficConfig;
|
|
private readonly IRobotEventBus? _eventBus;
|
|
|
|
// Sub-services (injected via constructor)
|
|
private readonly IRoutePlanningService _routePlanningService;
|
|
private readonly IConflictDetectionService _conflictDetectionService;
|
|
private readonly IConflictResolutionService _conflictResolutionService;
|
|
private readonly IBaseHorizonManagementService _baseHorizonManagementService;
|
|
private readonly IEdgeReservationService _edgeReservationService;
|
|
private readonly IPriorityService _priorityService;
|
|
private readonly IRobotInfoService _robotInfoService;
|
|
private readonly IRouteStorageService _routeStorageService;
|
|
private readonly IOrderUpdateService _orderUpdateService;
|
|
|
|
public TrafficControlService(
|
|
Logger<TrafficControlService> logger,
|
|
ITrafficConfig trafficConfig,
|
|
IRobotEventBus? eventBus,
|
|
IRoutePlanningService routePlanningService,
|
|
IConflictDetectionService conflictDetectionService,
|
|
IConflictResolutionService conflictResolutionService,
|
|
IBaseHorizonManagementService baseHorizonManagementService,
|
|
IEdgeReservationService edgeReservationService,
|
|
IPriorityService priorityService,
|
|
IRobotInfoService robotInfoService,
|
|
IRouteStorageService routeStorageService,
|
|
IOrderUpdateService orderUpdateService)
|
|
{
|
|
_logger = logger;
|
|
_trafficConfig = trafficConfig ?? throw new ArgumentNullException(nameof(trafficConfig));
|
|
_eventBus = eventBus;
|
|
_routePlanningService = routePlanningService;
|
|
_conflictDetectionService = conflictDetectionService;
|
|
_conflictResolutionService = conflictResolutionService;
|
|
_baseHorizonManagementService = baseHorizonManagementService;
|
|
_edgeReservationService = edgeReservationService;
|
|
_priorityService = priorityService;
|
|
_robotInfoService = robotInfoService;
|
|
_routeStorageService = routeStorageService;
|
|
_orderUpdateService = orderUpdateService;
|
|
|
|
// Subscribe to State messages if event bus is available
|
|
if (_eventBus != null)
|
|
{
|
|
_eventBus.StateMessageReceived += OnStateMessageReceived;
|
|
_logger.Info("Subscribed to State messages for robot progress monitoring");
|
|
}
|
|
}
|
|
|
|
#region ITrafficControlService Implementation
|
|
|
|
public async Task<RobotRoute?> PlanRouteAsync(
|
|
string robotId,
|
|
Guid startNodeId,
|
|
Guid goalNodeId,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _routePlanningService.PlanRouteAsync(robotId, startNodeId, goalNodeId, cancellationToken);
|
|
}
|
|
|
|
public async Task<RobotRoute?> PlanRouteAsync(
|
|
string robotId,
|
|
Guid startNodeId,
|
|
Guid goalNodeId,
|
|
double? goalAngle,
|
|
RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
|
|
RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _routePlanningService.PlanRouteAsync(robotId, startNodeId, goalNodeId, goalAngle, startDirection, finalDirection, cancellationToken);
|
|
}
|
|
|
|
public async Task<RobotRoute?> PlanRouteFromPositionAsync(
|
|
string robotId,
|
|
double x,
|
|
double y,
|
|
double theta,
|
|
Guid goalNodeId,
|
|
double? goalAngle,
|
|
RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
|
|
RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _routePlanningService.PlanRouteFromPositionAsync(robotId, x, y, theta, goalNodeId, goalAngle, startDirection, finalDirection, cancellationToken);
|
|
}
|
|
|
|
public async Task<RobotRoute?> PlanRouteFromPositionACSTrafficAsync(
|
|
string robotId,
|
|
double x,
|
|
double y,
|
|
double theta,
|
|
Guid goalNodeId,
|
|
double? goalAngle,
|
|
RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
|
|
RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _routePlanningService.PlanRouteFromPositionACSTrafficAsync(robotId, x, y, theta, goalNodeId, goalAngle, startDirection, finalDirection, cancellationToken);
|
|
}
|
|
|
|
public async Task<List<Conflict>> DetectConflictsAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
return await _conflictDetectionService.DetectConflictsAsync(cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> ResolveConflictAsync(Conflict conflict, CancellationToken cancellationToken = default)
|
|
{
|
|
return await _conflictResolutionService.ResolveConflictAsync(conflict, cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> ReleaseHorizonSegmentAsync(
|
|
string robotId,
|
|
int segmentCount,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _baseHorizonManagementService.ReleaseHorizonSegmentAsync(robotId, segmentCount, cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> UpdateRobotRouteAsync(
|
|
string robotId,
|
|
RobotRoute newRoute,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _routeStorageService.UpdateRobotRouteAsync(robotId, newRoute);
|
|
}
|
|
|
|
public async Task<Dictionary<string, RobotRoute>> GetAllActiveRoutesAsync()
|
|
{
|
|
var routes = await _routeStorageService.GetAllActiveRoutesAsync();
|
|
return routes.ToDictionary(r => r.Key, r => r.Value);
|
|
}
|
|
|
|
public async Task<RobotRoute?> GetRobotRouteAsync(string robotId)
|
|
{
|
|
return await _routeStorageService.GetRobotRouteAsync(robotId);
|
|
}
|
|
|
|
public Task<bool> SetRobotPriorityAsync(string robotId, RobotPriority priority)
|
|
{
|
|
return _priorityService.SetRobotPriorityAsync(robotId, priority);
|
|
}
|
|
|
|
public Task<RobotPriority> GetRobotPriorityAsync(string robotId)
|
|
{
|
|
return _priorityService.GetRobotPriorityAsync(robotId);
|
|
}
|
|
|
|
public Task<bool> RemoveRobotPriorityAsync(string robotId)
|
|
{
|
|
return _priorityService.RemoveRobotPriorityAsync(robotId);
|
|
}
|
|
|
|
public async Task<List<Conflict>> EvaluateConflictsForResolutionAsync(
|
|
List<Conflict> conflicts,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _conflictResolutionService.EvaluateConflictsForResolutionAsync(conflicts, cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> SendOrderUpdateAsync(
|
|
string robotId,
|
|
List<RouteSegment> newSegments,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _orderUpdateService.SendOrderUpdateAsync(robotId, newSegments, cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> ReserveEdgesAsync(
|
|
string robotId,
|
|
string orderId,
|
|
List<RouteSegment> segments,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _edgeReservationService.ReserveEdgesAsync(robotId, orderId, segments, cancellationToken);
|
|
}
|
|
|
|
public async Task<List<EdgeReservation>> GetEdgeReservationsAsync(
|
|
Guid edgeId,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _edgeReservationService.GetEdgeReservationsAsync(edgeId, cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> IsEdgeAvailableAsync(
|
|
Guid edgeId,
|
|
DateTime fromTime,
|
|
DateTime toTime,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _edgeReservationService.IsEdgeAvailableAsync(edgeId, fromTime, toTime, cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> ReleaseReservationsAsync(
|
|
string robotId,
|
|
string orderId,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
return await _edgeReservationService.ReleaseReservationsAsync(robotId, orderId, cancellationToken);
|
|
}
|
|
|
|
public async Task CheckAndReleaseHorizonsAsync(
|
|
string? robotId = null,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
await _baseHorizonManagementService.CheckAndReleaseHorizonsAsync(robotId, cancellationToken);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region BackgroundService Implementation
|
|
|
|
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
|
{
|
|
await Task.Yield();
|
|
_logger.Info("TrafficControlService started");
|
|
|
|
var config = _trafficConfig.GetTrafficControlConfig();
|
|
var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(config.ConflictDetection.IntervalMs));
|
|
|
|
try
|
|
{
|
|
while (await timer.WaitForNextTickAsync(stoppingToken))
|
|
{
|
|
try
|
|
{
|
|
// Cleanup expired priorities periodically
|
|
_priorityService.CleanupExpiredPriorities();
|
|
|
|
// Check and release horizons for all robots
|
|
await _baseHorizonManagementService.CheckAndReleaseHorizonsAsync(cancellationToken: stoppingToken);
|
|
|
|
// Real-time conflict detection and resolution loop
|
|
await ProcessConflictsRealTimeAsync(stoppingToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error in conflict detection loop: {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
_logger.Info("TrafficControlService stopping");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Event Handlers
|
|
|
|
/// <summary>
|
|
/// Handle State message received event - check if robot is near end of Base
|
|
/// </summary>
|
|
private void OnStateMessageReceived(object? sender, StateMessageReceivedEvent e)
|
|
{
|
|
try
|
|
{
|
|
var robotId = e.RobotId;
|
|
var stateMsg = e.StateMessage;
|
|
|
|
// Get robot route
|
|
var route = _routeStorageService.GetRobotRouteAsync(robotId).GetAwaiter().GetResult();
|
|
if (route == null)
|
|
{
|
|
return; // No active route, nothing to check
|
|
}
|
|
|
|
// Check if robot is near end of Base (1-2 segments remaining)
|
|
var remainingBaseSegments = _baseHorizonManagementService.CountRemainingBaseSegments(route, stateMsg);
|
|
if (remainingBaseSegments <= 2 && route.Horizon.Count > 0)
|
|
{
|
|
// Trigger horizon release check (async, fire and forget)
|
|
_ = Task.Run(async () =>
|
|
{
|
|
try
|
|
{
|
|
await _baseHorizonManagementService.CheckAndReleaseHorizonsAsync(robotId, CancellationToken.None);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error checking horizon release for robot {robotId} after state update: {ex.Message}");
|
|
}
|
|
});
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error handling state message for robot {e.RobotId}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Private Helpers
|
|
|
|
/// <summary>
|
|
/// Process conflicts in real-time: detect, evaluate, and resolve
|
|
/// </summary>
|
|
private async Task ProcessConflictsRealTimeAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
// 1. Detect all conflicts
|
|
var conflicts = await _conflictDetectionService.DetectConflictsAsync(cancellationToken);
|
|
if (conflicts == null || conflicts.Count == 0)
|
|
{
|
|
return; // No conflicts detected
|
|
}
|
|
|
|
_logger.Debug($"Detected {conflicts.Count} conflict(s) in real-time loop");
|
|
|
|
// 2. Evaluate conflicts for resolution optimization
|
|
var evaluatedConflicts = await _conflictResolutionService.EvaluateConflictsForResolutionAsync(conflicts, cancellationToken);
|
|
if (evaluatedConflicts == null || evaluatedConflicts.Count == 0)
|
|
{
|
|
return; // No conflicts to resolve
|
|
}
|
|
|
|
// 3. Resolve conflicts in priority order
|
|
var resolvedCount = 0;
|
|
var failedCount = 0;
|
|
var skippedCount = 0;
|
|
|
|
foreach (var conflict in evaluatedConflicts)
|
|
{
|
|
try
|
|
{
|
|
var resolved = await _conflictResolutionService.ResolveConflictAsync(conflict, cancellationToken);
|
|
var robotIds = string.Join(", ", conflict.InvolvedRobots);
|
|
if (resolved)
|
|
{
|
|
resolvedCount++;
|
|
_logger.Info($"Resolved conflict between {robotIds} (Type: {conflict.Type})");
|
|
}
|
|
else
|
|
{
|
|
failedCount++;
|
|
_logger.Warning($"Failed to resolve conflict between {robotIds} (Type: {conflict.Type})");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
failedCount++;
|
|
var robotIds = string.Join(", ", conflict.InvolvedRobots);
|
|
_logger.Error($"Error resolving conflict between {robotIds}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
if (resolvedCount > 0 || failedCount > 0)
|
|
{
|
|
_logger.Info($"Conflict resolution summary: {resolvedCount} resolved, {failedCount} failed, {skippedCount} skipped");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error in ProcessConflictsRealTimeAsync: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
|
|
#endregion
|
|
}
|