332 lines
12 KiB
C#
332 lines
12 KiB
C#
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
|
using RobotNet10.MapManager.Services;
|
|
|
|
namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
|
|
|
|
/// <summary>
|
|
/// Service for managing edge reservations
|
|
/// </summary>
|
|
public class EdgeReservationService(
|
|
Logger<EdgeReservationService> logger,
|
|
IServiceScopeFactory serviceScopeFactory,
|
|
IRobotInfoService robotInfoService) : IEdgeReservationService
|
|
{
|
|
private readonly Logger<EdgeReservationService> _logger = logger;
|
|
private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
|
|
private readonly IRobotInfoService _robotInfoService = robotInfoService;
|
|
|
|
// Edge reservations: EdgeId -> List of reservations
|
|
private readonly Dictionary<Guid, List<EdgeReservation>> _edgeReservations = [];
|
|
private readonly Lock _reservationsLock = new();
|
|
|
|
/// <summary>
|
|
/// Calculate reservation duration based on edge length and robot speed
|
|
/// </summary>
|
|
private static TimeSpan CalculateReservationDuration(
|
|
double edgeLength,
|
|
double? maxSpeed = null,
|
|
double defaultSpeed = 1.0) // Default speed: 1.0 m/s
|
|
{
|
|
var speed = maxSpeed ?? defaultSpeed;
|
|
if (speed <= 0)
|
|
{
|
|
speed = defaultSpeed;
|
|
}
|
|
|
|
// Add buffer time (10% of travel time) for safety
|
|
var travelTime = edgeLength / speed;
|
|
var bufferTime = travelTime * 0.1;
|
|
var totalTime = travelTime + bufferTime;
|
|
|
|
return TimeSpan.FromSeconds(Math.Max(totalTime, 0.5)); // Minimum 0.5 seconds
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate edge length from start and end nodes (Euclidean distance)
|
|
/// If trajectory exists, use more accurate calculation (future enhancement)
|
|
/// </summary>
|
|
private static double CalculateEdgeLength(
|
|
RobotNet10.MapManager.Data.Node startNode,
|
|
RobotNet10.MapManager.Data.Node endNode)
|
|
{
|
|
var dx = endNode.X - startNode.X;
|
|
var dy = endNode.Y - startNode.Y;
|
|
return Math.Sqrt(dx * dx + dy * dy);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get levelId for a robot from Robot.MapId in database
|
|
/// </summary>
|
|
private async Task<Guid?> GetLevelIdForRobotAsync(string robotId)
|
|
{
|
|
try
|
|
{
|
|
// Use IServiceScopeFactory to create a scope for Scoped services
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
|
|
|
var robot = await robotService.GetByRobotIdAsync(robotId);
|
|
if (robot == null)
|
|
{
|
|
_logger.Warning($"Robot {robotId} not found in database");
|
|
return null;
|
|
}
|
|
|
|
if (robot.MapId == null)
|
|
{
|
|
_logger.Warning($"Robot {robotId} has no MapId assigned");
|
|
return null;
|
|
}
|
|
|
|
// Robot.MapId is the levelId (LayoutLevel.Id)
|
|
return robot.MapId;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error getting levelId for robot {robotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
public async Task<bool> ReserveEdgesAsync(
|
|
string robotId,
|
|
string orderId,
|
|
List<RouteSegment> segments,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (segments == null || segments.Count == 0)
|
|
{
|
|
return true; // No segments to reserve
|
|
}
|
|
|
|
var reservations = new List<EdgeReservation>();
|
|
var now = DateTime.UtcNow;
|
|
|
|
// Get robot info for speed calculation
|
|
var robotInfo = await _robotInfoService.GetRobotInfoAsync(robotId, cancellationToken);
|
|
var defaultSpeed = 1.0; // Default 1.0 m/s if robot info not available
|
|
|
|
// Get levelId to fetch edge details
|
|
var levelId = await GetLevelIdForRobotAsync(robotId);
|
|
if (levelId == null)
|
|
{
|
|
_logger.Warning($"Cannot get levelId for robot {robotId} to reserve edges");
|
|
return false;
|
|
}
|
|
|
|
// Get edges with nodes for length calculation
|
|
|
|
using var scope = _serviceScopeFactory.CreateAsyncScope();
|
|
var _nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
|
var _edgeService = scope.ServiceProvider.GetRequiredService<IEdgeService>();
|
|
var edges = await _edgeService.GetEdgesByLevelAsync(levelId.Value, includeNodes: true, includeVehicleProperties: true);
|
|
var nodes = await _nodeService.GetNodesByLevelAsync(levelId.Value, includeVehicleProperties: true);
|
|
|
|
// Create lookup dictionaries
|
|
var edgeDict = edges.ToDictionary(e => e.Id);
|
|
var nodeDict = nodes.ToDictionary(n => n.Id);
|
|
|
|
// Reserve edges from segments
|
|
foreach (var segment in segments)
|
|
{
|
|
if (segment.VdaEdge == null) continue; // Skip node segments
|
|
|
|
// Skip virtual edges (created when robot is on edge, not in database)
|
|
if (segment.VdaEdge.EdgeId.StartsWith("VIRTUAL_", StringComparison.OrdinalIgnoreCase))
|
|
{
|
|
_logger.Debug($"Skipping virtual edge {segment.EdgeId} for reservation (robot is on edge)");
|
|
continue;
|
|
}
|
|
|
|
if(segment.EdgeId is null) continue;
|
|
|
|
var edgeId = segment.EdgeId.Value;
|
|
if (!edgeDict.TryGetValue(edgeId, out var edge))
|
|
{
|
|
_logger.Warning($"Edge {edgeId} not found for reservation");
|
|
continue;
|
|
}
|
|
|
|
// Get start and end nodes
|
|
if (!nodeDict.TryGetValue(edge.StartNodeId, out var startNode) ||
|
|
!nodeDict.TryGetValue(edge.EndNodeId, out var endNode))
|
|
{
|
|
_logger.Warning($"Start or end node not found for edge {edgeId}");
|
|
continue;
|
|
}
|
|
|
|
// Calculate edge length
|
|
var edgeLength = CalculateEdgeLength(startNode, endNode);
|
|
|
|
// Get max speed from EdgeVehicleProperty (if available)
|
|
// TODO: Match with robot's vehicle type
|
|
double? maxSpeed = null;
|
|
if (edge.VehicleProperties != null && edge.VehicleProperties.Count != 0)
|
|
{
|
|
// Use first vehicle property's max speed (in future, match with robot's vehicle type)
|
|
maxSpeed = edge.VehicleProperties.FirstOrDefault()?.MaxSpeed;
|
|
}
|
|
|
|
// Calculate reservation duration
|
|
var duration = CalculateReservationDuration(edgeLength, maxSpeed, defaultSpeed);
|
|
|
|
var reservation = new EdgeReservation
|
|
{
|
|
EdgeId = edgeId,
|
|
EdgeIdString = edge.EdgeId,
|
|
StartNodeId = edge.StartNodeId,
|
|
EndNodeId = edge.EndNodeId,
|
|
RobotId = robotId,
|
|
OrderId = orderId,
|
|
ReservedAt = now,
|
|
ReservedUntil = now.Add(duration),
|
|
Status = ReservationStatus.Reserved
|
|
};
|
|
|
|
reservations.Add(reservation);
|
|
}
|
|
|
|
// Add reservations to dictionary (thread-safe)
|
|
lock (_reservationsLock)
|
|
{
|
|
foreach (var reservation in reservations)
|
|
{
|
|
if (!_edgeReservations.TryGetValue(reservation.EdgeId, out List<EdgeReservation>? value))
|
|
{
|
|
value = [];
|
|
_edgeReservations[reservation.EdgeId] = value;
|
|
}
|
|
|
|
value.Add(reservation);
|
|
}
|
|
}
|
|
|
|
_logger.Debug($"Reserved {reservations.Count} edges for robot {robotId}, order {orderId}");
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error reserving edges for robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public async Task<bool> ReleaseReservationsAsync(
|
|
string robotId,
|
|
string? orderId = null,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
var releasedCount = 0;
|
|
|
|
lock (_reservationsLock)
|
|
{
|
|
foreach (var edgeReservations in _edgeReservations.Values)
|
|
{
|
|
for (int i = edgeReservations.Count - 1; i >= 0; i--)
|
|
{
|
|
var reservation = edgeReservations[i];
|
|
if (reservation.RobotId == robotId &&
|
|
(orderId == null || reservation.OrderId == orderId))
|
|
{
|
|
reservation.Status = ReservationStatus.Released;
|
|
edgeReservations.RemoveAt(i);
|
|
releasedCount++;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Clean up empty lists
|
|
var emptyEdges = _edgeReservations
|
|
.Where(kvp => kvp.Value.Count == 0)
|
|
.Select(kvp => kvp.Key)
|
|
.ToList();
|
|
|
|
foreach (var edgeId in emptyEdges)
|
|
{
|
|
_edgeReservations.Remove(edgeId);
|
|
}
|
|
}
|
|
|
|
_logger.Debug($"Released {releasedCount} reservations for robot {robotId}" +
|
|
(orderId != null ? $", order {orderId}" : ""));
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error releasing reservations for robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public async Task<List<EdgeReservation>> GetEdgeReservationsAsync(
|
|
Guid edgeId,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
lock (_reservationsLock)
|
|
{
|
|
if (_edgeReservations.TryGetValue(edgeId, out var reservations))
|
|
{
|
|
// Filter out expired reservations
|
|
var now = DateTime.UtcNow;
|
|
var activeReservations = reservations
|
|
.Where(r => r.ReservedUntil > now && r.Status == ReservationStatus.Reserved)
|
|
.ToList();
|
|
|
|
// Remove expired reservations
|
|
var expiredCount = reservations.Count - activeReservations.Count;
|
|
if (expiredCount > 0)
|
|
{
|
|
_edgeReservations[edgeId] = activeReservations;
|
|
_logger.Debug($"Cleaned up {expiredCount} expired reservations for edge {edgeId}");
|
|
}
|
|
|
|
return [.. activeReservations];
|
|
}
|
|
|
|
return [];
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error getting reservations for edge {edgeId}: {ex.Message}");
|
|
return [];
|
|
}
|
|
}
|
|
|
|
public async Task<bool> IsEdgeAvailableAsync(
|
|
Guid edgeId,
|
|
DateTime fromTime,
|
|
DateTime toTime,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
var reservations = await GetEdgeReservationsAsync(edgeId, cancellationToken);
|
|
|
|
// Check if any reservation overlaps with the requested time range
|
|
foreach (var reservation in reservations)
|
|
{
|
|
// Check for overlap: reservation.ReservedAt < toTime && reservation.ReservedUntil > fromTime
|
|
if (reservation.ReservedAt < toTime && reservation.ReservedUntil > fromTime)
|
|
{
|
|
return false; // Edge is reserved during this time
|
|
}
|
|
}
|
|
|
|
return true; // Edge is available
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error checking edge availability for edge {edgeId}: {ex.Message}");
|
|
return false; // Assume not available on error
|
|
}
|
|
}
|
|
}
|
|
|