1151 lines
49 KiB
C#
1151 lines
49 KiB
C#
using Microsoft.Extensions.DependencyInjection;
|
|
using RobotNet.VDA5050.State;
|
|
using RobotNet10.FleetManager.Services.ConfigManager;
|
|
using RobotNet10.FleetManager.Services.RobotManager;
|
|
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
|
using RobotNet10.MapManager.Services;
|
|
|
|
namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
|
|
|
|
/// <summary>
|
|
/// Service for detecting conflicts between robots
|
|
/// </summary>
|
|
public class ConflictDetectionService(
|
|
Logger<ConflictDetectionService> logger,
|
|
IRouteStorageService routeStorageService,
|
|
IRobotInfoService robotInfoService,
|
|
IEdgeReservationService edgeReservationService,
|
|
IServiceScopeFactory serviceScopeFactory,
|
|
ITrafficConfig trafficConfig) : IConflictDetectionService
|
|
{
|
|
private readonly Logger<ConflictDetectionService> _logger = logger;
|
|
private readonly IRouteStorageService _routeStorageService = routeStorageService;
|
|
private readonly IRobotInfoService _robotInfoService = robotInfoService;
|
|
private readonly IEdgeReservationService _edgeReservationService = edgeReservationService;
|
|
private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
|
|
private readonly ITrafficConfig _trafficConfig = trafficConfig ?? throw new ArgumentNullException(nameof(trafficConfig));
|
|
|
|
public async Task<List<Conflict>> DetectConflictsAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
var conflicts = new List<Conflict>();
|
|
|
|
// Get all active routes
|
|
var allRoutes = await _routeStorageService.GetAllActiveRoutesAsync();
|
|
var activeRoutes = allRoutes.Values.ToList();
|
|
|
|
if (activeRoutes.Count < 2)
|
|
{
|
|
return conflicts; // Need at least 2 robots for conflicts
|
|
}
|
|
|
|
// Get all robot states
|
|
// Resolve IRobotManagerService lazily to avoid circular dependency
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
|
var allRobotData = robotManager.GetAllRobotData();
|
|
|
|
// Check all pairs of robots
|
|
for (int i = 0; i < activeRoutes.Count; i++)
|
|
{
|
|
for (int j = i + 1; j < activeRoutes.Count; j++)
|
|
{
|
|
var route1 = activeRoutes[i];
|
|
var route2 = activeRoutes[j];
|
|
|
|
// Get robot states
|
|
var robotData1 = allRobotData.TryGetValue(route1.RobotId, out var data1) ? data1 : null;
|
|
var robotData2 = allRobotData.TryGetValue(route2.RobotId, out var data2) ? data2 : null;
|
|
|
|
var state1 = robotData1?.State;
|
|
var state2 = robotData2?.State;
|
|
|
|
// Check Confrontation
|
|
var confrontation = await DetectConfrontationAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (confrontation != null) conflicts.Add(confrontation);
|
|
|
|
// Check Edge conflict
|
|
var edgeConflict = await DetectEdgeConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (edgeConflict != null) conflicts.Add(edgeConflict);
|
|
|
|
// Check Vertex conflict
|
|
var vertexConflict = await DetectVertexConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (vertexConflict != null) conflicts.Add(vertexConflict);
|
|
|
|
// Check Proximity conflict
|
|
var proximityConflict = await DetectProximityConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (proximityConflict != null) conflicts.Add(proximityConflict);
|
|
|
|
// Check Temporal conflict
|
|
var temporalConflict = await DetectTemporalConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (temporalConflict != null) conflicts.Add(temporalConflict);
|
|
|
|
// Check Corridor conflict
|
|
var corridorConflict = await DetectCorridorConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (corridorConflict != null) conflicts.Add(corridorConflict);
|
|
|
|
// Check Rotation conflict
|
|
var rotationConflict = await DetectRotationConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (rotationConflict != null) conflicts.Add(rotationConflict);
|
|
|
|
// Check Resource conflict
|
|
var resourceConflict = await DetectResourceConflictAsync(route1, route2, state1, state2, cancellationToken);
|
|
if (resourceConflict != null) conflicts.Add(resourceConflict);
|
|
}
|
|
}
|
|
|
|
_logger.Debug($"Detected {conflicts.Count} conflicts");
|
|
return conflicts;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting conflicts: {ex.Message}");
|
|
return [];
|
|
}
|
|
}
|
|
|
|
public async Task<bool> CheckConflictsForSegmentsAsync(
|
|
string robotId,
|
|
List<RouteSegment> segments,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (segments == null || segments.Count == 0)
|
|
{
|
|
return false; // No segments = no conflict
|
|
}
|
|
|
|
// Get all other active routes
|
|
var allRoutes = await _routeStorageService.GetAllActiveRoutesAsync();
|
|
var otherActiveRoutes = allRoutes.Values
|
|
.Where(r => r.RobotId != robotId)
|
|
.ToList();
|
|
|
|
if (otherActiveRoutes.Count == 0)
|
|
{
|
|
return false; // No other robots = no conflict
|
|
}
|
|
|
|
// Get robot info for speed calculation
|
|
var robotInfo = await _robotInfoService.GetRobotInfoAsync(robotId, cancellationToken);
|
|
var defaultSpeed = 1.0; // Default 1.0 m/s
|
|
|
|
// Get levelId to fetch edge details
|
|
var levelId = await GetLevelIdForRobotAsync(robotId, cancellationToken);
|
|
if (levelId == null)
|
|
{
|
|
_logger.Warning($"Cannot get levelId for robot {robotId} to check conflicts");
|
|
return true; // Assume conflict on error
|
|
}
|
|
|
|
// 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);
|
|
|
|
// Get robot current state to estimate arrival times
|
|
// Resolve IRobotManagerService lazily to avoid circular dependency
|
|
using var scopeForRobotManager = _serviceScopeFactory.CreateScope();
|
|
var robotManager = scopeForRobotManager.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
|
var robotData = robotManager.GetRobotData(robotId);
|
|
var state = robotData?.State;
|
|
var now = DateTime.UtcNow;
|
|
|
|
// Estimate arrival time for first segment
|
|
// Get current route - must exist since we're checking conflicts for segments
|
|
var currentRoute = await _routeStorageService.GetRobotRouteAsync(robotId);
|
|
if (currentRoute == null)
|
|
{
|
|
_logger.Warning($"Cannot estimate arrival time: route not found for robot {robotId}");
|
|
return true; // Assume conflict on error (safer)
|
|
}
|
|
|
|
var firstSegment = segments[0];
|
|
var estimatedArrival = EstimateArrivalTime(currentRoute, firstSegment, state);
|
|
|
|
// Check each edge segment
|
|
foreach (var segment in segments)
|
|
{
|
|
if (segment.EdgeId == null) continue; // Skip node segments
|
|
|
|
var edgeId = segment.EdgeId.Value;
|
|
if (!edgeDict.TryGetValue(edgeId, out var edge))
|
|
{
|
|
continue; // Edge not found, skip
|
|
}
|
|
|
|
// Get start and end nodes
|
|
if (!nodeDict.TryGetValue(edge.StartNodeId, out var startNode) ||
|
|
!nodeDict.TryGetValue(edge.EndNodeId, out var endNode))
|
|
{
|
|
continue; // Nodes not found, skip
|
|
}
|
|
|
|
// Calculate edge length
|
|
var edgeLength = CalculateEdgeLength(startNode, endNode);
|
|
|
|
// Get max speed from EdgeVehicleProperty
|
|
double? maxSpeed = null;
|
|
if (edge.VehicleProperties != null && edge.VehicleProperties.Count != 0)
|
|
{
|
|
maxSpeed = edge.VehicleProperties.FirstOrDefault()?.MaxSpeed;
|
|
}
|
|
|
|
// Calculate reservation duration for this edge
|
|
var duration = CalculateReservationDuration(edgeLength, maxSpeed, defaultSpeed);
|
|
var reservedFrom = estimatedArrival;
|
|
var reservedUntil = estimatedArrival.Add(duration);
|
|
|
|
// Check if edge is available (not reserved by other robots)
|
|
var isAvailable = await _edgeReservationService.IsEdgeAvailableAsync(edgeId, reservedFrom, reservedUntil, cancellationToken);
|
|
if (!isAvailable)
|
|
{
|
|
_logger.Debug($"Edge {edgeId} is not available for robot {robotId} during {reservedFrom} to {reservedUntil}");
|
|
return true; // Conflict found
|
|
}
|
|
|
|
// Also check for reverse edge (EdgeBA vs EdgeAB)
|
|
var reverseEdge = edges.FirstOrDefault(e =>
|
|
e.StartNodeId == edge.EndNodeId &&
|
|
e.EndNodeId == edge.StartNodeId &&
|
|
e.Id != edgeId);
|
|
|
|
if (reverseEdge != null)
|
|
{
|
|
var reverseIsAvailable = await _edgeReservationService.IsEdgeAvailableAsync(reverseEdge.Id, reservedFrom, reservedUntil, cancellationToken);
|
|
if (!reverseIsAvailable)
|
|
{
|
|
_logger.Debug($"Reverse edge {reverseEdge.Id} is not available for robot {robotId} during {reservedFrom} to {reservedUntil}");
|
|
return true; // Conflict found
|
|
}
|
|
}
|
|
|
|
// Update estimated arrival for next segment
|
|
estimatedArrival = reservedUntil;
|
|
}
|
|
|
|
// Edge availability check above is sufficient
|
|
// Additional conflict detection with other routes will be done in real-time loop
|
|
|
|
return false; // No conflicts found
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error checking conflicts for segments for robot {robotId}: {ex.Message}");
|
|
return true; // Assume conflict on error (safer)
|
|
}
|
|
}
|
|
|
|
#region Conflict Detection Methods
|
|
|
|
/// <summary>
|
|
/// Detect Confrontation conflict: 2 robots use same edge in opposite directions with overlapping future routes
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectConfrontationAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
// Get edge segments from both routes
|
|
var edges1 = route1.FullRoute.Where(s => s.EdgeId != null).ToList();
|
|
var edges2 = route2.FullRoute.Where(s => s.EdgeId != null).ToList();
|
|
|
|
if (edges1.Count == 0 || edges2.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Check for opposite direction edges (EdgeAB vs EdgeBA)
|
|
foreach (var seg1 in edges1)
|
|
{
|
|
foreach (var seg2 in edges2)
|
|
{
|
|
if (seg1.EdgeId == null || seg2.EdgeId == null) continue;
|
|
|
|
// Check if edges are opposite (same start/end nodes but swapped)
|
|
// This requires checking edge details from database
|
|
var levelId1 = await GetLevelIdForRobotAsync(route1.RobotId, cancellationToken);
|
|
var levelId2 = await GetLevelIdForRobotAsync(route2.RobotId, cancellationToken);
|
|
|
|
if (levelId1 == null || levelId2 == null || levelId1 != levelId2)
|
|
{
|
|
continue; // Different levels, no conflict
|
|
}
|
|
|
|
using var scope = _serviceScopeFactory.CreateAsyncScope();
|
|
var _edgeService = scope.ServiceProvider.GetRequiredService<IEdgeService>();
|
|
var edges = await _edgeService.GetEdgesByLevelAsync(levelId1.Value, includeNodes: true);
|
|
var edge1 = edges.FirstOrDefault(e => e.Id == seg1.EdgeId.Value);
|
|
var edge2 = edges.FirstOrDefault(e => e.Id == seg2.EdgeId.Value);
|
|
|
|
if (edge1 == null || edge2 == null) continue;
|
|
|
|
// Check if opposite direction
|
|
bool isOpposite = edge1.StartNodeId == edge2.EndNodeId &&
|
|
edge1.EndNodeId == edge2.StartNodeId;
|
|
|
|
if (isOpposite)
|
|
{
|
|
// Check if time ranges overlap (simplified: check if both are in Base or near each other)
|
|
var seg1InBase = route1.Base.Contains(seg1);
|
|
var seg2InBase = route2.Base.Contains(seg2);
|
|
|
|
// If both in Base or one in Base and one in Horizon, check future routes overlap
|
|
if (seg1InBase || seg2InBase)
|
|
{
|
|
// Check if future routes overlap
|
|
var futureOverlap = CheckFutureRoutesOverlap(route1, route2, seg1, seg2);
|
|
if (futureOverlap)
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Confrontation,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
ConflictingEdges = new List<Guid> { seg1.EdgeId.Value, seg2.EdgeId.Value },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.High
|
|
};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting confrontation between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Edge conflict: 2 robots use same edge in same direction with non-overlapping future routes
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectEdgeConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
var edges1 = route1.FullRoute.Where(s => s.EdgeId != null).ToList();
|
|
var edges2 = route2.FullRoute.Where(s => s.EdgeId != null).ToList();
|
|
|
|
if (edges1.Count == 0 || edges2.Count == 0)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
foreach (var seg1 in edges1)
|
|
{
|
|
foreach (var seg2 in edges2)
|
|
{
|
|
if (seg1.EdgeId == null || seg2.EdgeId == null) continue;
|
|
|
|
// Check if same edge
|
|
if (seg1.EdgeId == seg2.EdgeId)
|
|
{
|
|
// Check if time ranges overlap
|
|
var seg1InBase = route1.Base.Contains(seg1);
|
|
var seg2InBase = route2.Base.Contains(seg2);
|
|
|
|
if (seg1InBase || seg2InBase)
|
|
{
|
|
// Check if future routes do NOT overlap
|
|
var futureOverlap = CheckFutureRoutesOverlap(route1, route2, seg1, seg2);
|
|
if (!futureOverlap)
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Edge,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
ConflictingEdges = new List<Guid> { seg1.EdgeId.Value },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.Medium
|
|
};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting edge conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Vertex conflict: 2 robots target same node at overlapping times
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectVertexConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
// Get node segments from both routes
|
|
var nodes1 = route1.FullRoute.Where(s => s.NodeId != Guid.Empty).ToList();
|
|
var nodes2 = route2.FullRoute.Where(s => s.NodeId != Guid.Empty).ToList();
|
|
|
|
foreach (var seg1 in nodes1)
|
|
{
|
|
foreach (var seg2 in nodes2)
|
|
{
|
|
// Check if same node
|
|
if (seg1.NodeId == seg2.NodeId)
|
|
{
|
|
// Check if both in Base or one in Base
|
|
var seg1InBase = route1.Base.Contains(seg1);
|
|
var seg2InBase = route2.Base.Contains(seg2);
|
|
|
|
if (seg1InBase || seg2InBase)
|
|
{
|
|
// Calculate estimated arrival times
|
|
var time1 = EstimateArrivalTime(route1, seg1, state1);
|
|
var time2 = EstimateArrivalTime(route2, seg2, state2);
|
|
|
|
// Check if times are within threshold
|
|
var config = _trafficConfig.GetTrafficControlConfig();
|
|
var timeDiff = Math.Abs((time1 - time2).TotalSeconds);
|
|
if (timeDiff <= config.ConflictDetection.VertexConflictThreshold)
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Vertex,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
ConflictingNodes = new List<Guid> { seg1.NodeId },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.Medium,
|
|
ConflictDetails = new Dictionary<string, object>
|
|
{
|
|
{ "TimeDifference", timeDiff },
|
|
{ "NodeId", seg1.NodeId }
|
|
}
|
|
};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting vertex conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Proximity conflict: 2 robots are too close in continuous space
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectProximityConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (state1?.AgvPosition == null || state2?.AgvPosition == null)
|
|
{
|
|
return null; // Need position information
|
|
}
|
|
|
|
// Calculate Euclidean distance
|
|
var dx = state1.AgvPosition.X - state2.AgvPosition.X;
|
|
var dy = state1.AgvPosition.Y - state2.AgvPosition.Y;
|
|
var distance = Math.Sqrt(dx * dx + dy * dy);
|
|
|
|
// Check if distance is less than minimum safe distance
|
|
var config = _trafficConfig.GetTrafficControlConfig();
|
|
if (distance < config.ConflictDetection.ProximityMinDistance)
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Proximity,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.High,
|
|
ConflictDetails = new Dictionary<string, object>
|
|
{
|
|
{ "Distance", distance },
|
|
{ "MinDistance", config.ConflictDetection.ProximityMinDistance },
|
|
{ "Robot1Position", new { X = state1.AgvPosition.X, Y = state1.AgvPosition.Y } },
|
|
{ "Robot2Position", new { X = state2.AgvPosition.X, Y = state2.AgvPosition.Y } }
|
|
}
|
|
};
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting proximity conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Temporal conflict: Routes intersect in time but not at same edge/node
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectTemporalConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (state1?.AgvPosition == null || state2?.AgvPosition == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Estimate positions at future times
|
|
var now = DateTime.UtcNow;
|
|
var timeSteps = new[] { 1.0, 2.0, 3.0, 4.0, 5.0 }; // Check at 1s, 2s, 3s, 4s, 5s
|
|
|
|
foreach (var timeStep in timeSteps)
|
|
{
|
|
var pos1 = EstimatePositionAtTime(route1, state1, timeStep);
|
|
var pos2 = EstimatePositionAtTime(route2, state2, timeStep);
|
|
|
|
if (pos1 == null || pos2 == null) continue;
|
|
|
|
// Calculate distance at this time
|
|
var dx = pos1.Value.X - pos2.Value.X;
|
|
var dy = pos1.Value.Y - pos2.Value.Y;
|
|
var distance = Math.Sqrt(dx * dx + dy * dy);
|
|
|
|
// Check if too close
|
|
var config = _trafficConfig.GetTrafficControlConfig();
|
|
if (distance < config.ConflictDetection.ProximityMinDistance)
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Temporal,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.Medium,
|
|
ConflictDetails = new Dictionary<string, object>
|
|
{
|
|
{ "TimeStep", timeStep },
|
|
{ "Distance", distance },
|
|
{ "Position1", pos1 },
|
|
{ "Position2", pos2 }
|
|
}
|
|
};
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting temporal conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Corridor conflict: Two robots moving in opposite directions through a narrow corridor
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectCorridorConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (state1?.AgvPosition == null || state2?.AgvPosition == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Get robot info for width checking
|
|
var robotInfo1 = await _robotInfoService.GetRobotInfoAsync(route1.RobotId, cancellationToken);
|
|
var robotInfo2 = await _robotInfoService.GetRobotInfoAsync(route2.RobotId, cancellationToken);
|
|
|
|
if (robotInfo1 == null || robotInfo2 == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Find consecutive edges that form a corridor (sequence of edges)
|
|
// Check if robots are moving in opposite directions on the same sequence of edges
|
|
var edges1 = route1.FullRoute.Where(s => s.EdgeId != null).ToList();
|
|
var edges2 = route2.FullRoute.Where(s => s.EdgeId != null).ToList();
|
|
|
|
// Find overlapping edge sequences (corridor)
|
|
var minCorridorLength = 2; // At least 2 consecutive edges
|
|
for (int i = 0; i <= edges1.Count - minCorridorLength; i++)
|
|
{
|
|
for (int j = 0; j <= edges2.Count - minCorridorLength; j++)
|
|
{
|
|
var seq1 = edges1.Skip(i).Take(minCorridorLength).ToList();
|
|
var seq2 = edges2.Skip(j).Take(minCorridorLength).ToList();
|
|
|
|
// Check if sequences are the same but in reverse order (opposite directions)
|
|
var seq2Reversed = seq2.Select(s => s.EdgeId!.Value).ToList();
|
|
seq2Reversed.Reverse();
|
|
var seq1EdgeIds = seq1.Select(s => s.EdgeId!.Value).ToList();
|
|
|
|
bool isOppositeDirection = false;
|
|
if (seq1EdgeIds.SequenceEqual(seq2Reversed))
|
|
{
|
|
// Check if edges are actually opposite (EdgeAB vs EdgeBA)
|
|
// by comparing StartNodeId and EndNodeId
|
|
isOppositeDirection = await CheckEdgesAreOppositeAsync(seq1, seq2, cancellationToken);
|
|
}
|
|
else if (seq1EdgeIds.SequenceEqual(seq2.Select(s => s.EdgeId!.Value).ToList()))
|
|
{
|
|
// Same direction, not a corridor conflict
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Only check corridor conflict if robots are moving in opposite directions
|
|
if (!isOppositeDirection)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Check if corridor is narrow (sum of robot widths > corridor width)
|
|
// Since we don't have edge width in database, use a default corridor width
|
|
var defaultCorridorWidth = 2.0; // meters (default assumption)
|
|
var totalRobotWidth = robotInfo1.Width + robotInfo2.Width;
|
|
|
|
if (totalRobotWidth > defaultCorridorWidth * 0.8) // 80% threshold
|
|
{
|
|
// Estimate time overlap
|
|
// Use first edge segment in sequence for arrival time estimation
|
|
var arrival1 = EstimateArrivalTime(route1, seq1[0], state1);
|
|
var arrival2 = EstimateArrivalTime(route2, seq2[0], state2);
|
|
|
|
// Check if robots will be in corridor at overlapping times
|
|
var edgeLength = 5.0; // Default edge length
|
|
var travelTime1 = edgeLength * minCorridorLength / 1.0; // Assume 1 m/s
|
|
var travelTime2 = edgeLength * minCorridorLength / 1.0;
|
|
|
|
var exit1 = arrival1.AddSeconds(travelTime1);
|
|
var exit2 = arrival2.AddSeconds(travelTime2);
|
|
|
|
if (arrival1 < exit2 && arrival2 < exit1) // Time overlap
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Corridor,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.High,
|
|
ConflictDetails = new Dictionary<string, object>
|
|
{
|
|
{ "CorridorLength", minCorridorLength },
|
|
{ "TotalRobotWidth", totalRobotWidth },
|
|
{ "CorridorWidth", defaultCorridorWidth },
|
|
{ "Robot1Arrival", arrival1 },
|
|
{ "Robot2Arrival", arrival2 }
|
|
}
|
|
};
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting corridor conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Rotation conflict: Two robots need to rotate at the same node and rotation spaces overlap
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectRotationConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (state1?.AgvPosition == null || state2?.AgvPosition == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Get robot info for rotation space calculation
|
|
var robotInfo1 = await _robotInfoService.GetRobotInfoAsync(route1.RobotId, cancellationToken);
|
|
var robotInfo2 = await _robotInfoService.GetRobotInfoAsync(route2.RobotId, cancellationToken);
|
|
|
|
if (robotInfo1 == null || robotInfo2 == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
// Find nodes where both robots need to rotate
|
|
var nodes1 = route1.FullRoute.Where(s => s.NodeId != Guid.Empty).Select(s => s.NodeId).ToList();
|
|
var nodes2 = route2.FullRoute.Where(s => s.NodeId != Guid.Empty).Select(s => s.NodeId).ToList();
|
|
|
|
var commonNodes = nodes1.Intersect(nodes2).ToList();
|
|
|
|
foreach (var nodeId in commonNodes)
|
|
{
|
|
// Get node position
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
|
|
|
var levelId1 = await GetLevelIdForRobotAsync(route1.RobotId, cancellationToken);
|
|
var levelId2 = await GetLevelIdForRobotAsync(route2.RobotId, cancellationToken);
|
|
|
|
if (levelId1 == null || levelId2 == null || levelId1 != levelId2)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var nodes = await nodeService.GetNodesByLevelAsync(levelId1.Value, includeVehicleProperties: false);
|
|
var node = nodes.FirstOrDefault(n => n.Id == nodeId);
|
|
|
|
if (node == null) continue;
|
|
|
|
// Calculate rotation space for each robot
|
|
// Rotation space = circle with radius = max(length, width) / 2 + safety margin
|
|
var rotationRadius1 = Math.Max(robotInfo1.Length, robotInfo1.Width) / 2.0 + 0.5; // +0.5m safety margin
|
|
var rotationRadius2 = Math.Max(robotInfo2.Length, robotInfo2.Width) / 2.0 + 0.5;
|
|
|
|
// Check if rotation spaces overlap
|
|
var distance = Math.Sqrt(
|
|
Math.Pow(node.X - node.X, 2) +
|
|
Math.Pow(node.Y - node.Y, 2)); // Same node, distance = 0, but keeping formula for clarity
|
|
|
|
// Since both robots rotate at the same node, their rotation spaces will overlap
|
|
// if the sum of radii is greater than the distance between rotation centers
|
|
// At the same node, distance = 0, so they will always overlap
|
|
// But we need to check if they arrive at overlapping times
|
|
|
|
// Estimate arrival times at this node
|
|
var nodeSegment1 = route1.FullRoute.FirstOrDefault(s => s.NodeId == nodeId);
|
|
var nodeSegment2 = route2.FullRoute.FirstOrDefault(s => s.NodeId == nodeId);
|
|
|
|
if (nodeSegment1 == null || nodeSegment2 == null) continue;
|
|
|
|
var arrival1 = EstimateArrivalTime(route1, nodeSegment1, state1);
|
|
var arrival2 = EstimateArrivalTime(route2, nodeSegment2, state2);
|
|
|
|
// Estimate rotation time (simplified: assume 5 seconds for rotation)
|
|
var rotationTime = 5.0; // seconds
|
|
var exit1 = arrival1.AddSeconds(rotationTime);
|
|
var exit2 = arrival2.AddSeconds(rotationTime);
|
|
|
|
// Check time overlap
|
|
if (arrival1 < exit2 && arrival2 < exit1)
|
|
{
|
|
// Check if rotation spaces actually overlap (they do at same node)
|
|
var minDistance = rotationRadius1 + rotationRadius2;
|
|
if (distance < minDistance) // Always true at same node, but keeping check for clarity
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Rotation,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.Medium,
|
|
ConflictDetails = new Dictionary<string, object>
|
|
{
|
|
{ "NodeId", nodeId },
|
|
{ "NodeX", node.X },
|
|
{ "NodeY", node.Y },
|
|
{ "Robot1RotationRadius", rotationRadius1 },
|
|
{ "Robot2RotationRadius", rotationRadius2 },
|
|
{ "Robot1Arrival", arrival1 },
|
|
{ "Robot2Arrival", arrival2 }
|
|
}
|
|
};
|
|
}
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting rotation conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect Resource conflict: Multiple robots competing for the same resource (e.g., charging station, work area)
|
|
/// </summary>
|
|
private async Task<Conflict?> DetectResourceConflictAsync(
|
|
RobotRoute route1,
|
|
RobotRoute route2,
|
|
StateMsg? state1,
|
|
StateMsg? state2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
// Find nodes that are resources (e.g., charging stations, work areas)
|
|
// For now, we'll identify resource nodes by checking if they have specific properties
|
|
// or if they are marked as resource nodes in the route
|
|
|
|
var nodes1 = route1.FullRoute.Where(s => s.NodeId != Guid.Empty).Select(s => s.NodeId).ToList();
|
|
var nodes2 = route2.FullRoute.Where(s => s.NodeId != Guid.Empty).Select(s => s.NodeId).ToList();
|
|
|
|
var commonNodes = nodes1.Intersect(nodes2).ToList();
|
|
|
|
foreach (var nodeId in commonNodes)
|
|
{
|
|
// Check if this node is a resource node
|
|
// For now, we'll assume any common node could be a resource
|
|
// In future, this should check NodeVehicleProperty.Actions or other markers
|
|
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
|
|
|
var levelId1 = await GetLevelIdForRobotAsync(route1.RobotId, cancellationToken);
|
|
var levelId2 = await GetLevelIdForRobotAsync(route2.RobotId, cancellationToken);
|
|
|
|
if (levelId1 == null || levelId2 == null || levelId1 != levelId2)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var nodes = await nodeService.GetNodesByLevelAsync(levelId1.Value, includeVehicleProperties: false);
|
|
var node = nodes.FirstOrDefault(n => n.Id == nodeId);
|
|
|
|
if (node == null) continue;
|
|
|
|
// Check if node has resource properties (e.g., actions that indicate resource usage)
|
|
// For now, we'll check if node name or description suggests it's a resource
|
|
var isResourceNode = false;
|
|
if (!string.IsNullOrWhiteSpace(node.NodeName))
|
|
{
|
|
var nameLower = node.NodeName.ToLowerInvariant();
|
|
isResourceNode = nameLower.Contains("charge") ||
|
|
nameLower.Contains("station") ||
|
|
nameLower.Contains("work") ||
|
|
nameLower.Contains("resource");
|
|
}
|
|
|
|
if (!isResourceNode && !string.IsNullOrWhiteSpace(node.NodeDescription))
|
|
{
|
|
var descLower = node.NodeDescription.ToLowerInvariant();
|
|
isResourceNode = descLower.Contains("charge") ||
|
|
descLower.Contains("station") ||
|
|
descLower.Contains("work") ||
|
|
descLower.Contains("resource");
|
|
}
|
|
|
|
if (!isResourceNode)
|
|
{
|
|
// Check if node has vehicle properties with actions (could indicate resource)
|
|
// This would require loading NodeVehicleProperty, which we'll skip for now
|
|
// In a full implementation, we should check NodeVehicleProperty.Actions
|
|
continue;
|
|
}
|
|
|
|
// Estimate arrival times at this resource node
|
|
var nodeSegment1 = route1.FullRoute.FirstOrDefault(s => s.NodeId == nodeId);
|
|
var nodeSegment2 = route2.FullRoute.FirstOrDefault(s => s.NodeId == nodeId);
|
|
|
|
if (nodeSegment1 == null || nodeSegment2 == null) continue;
|
|
|
|
var arrival1 = EstimateArrivalTime(route1, nodeSegment1, state1);
|
|
var arrival2 = EstimateArrivalTime(route2, nodeSegment2, state2);
|
|
|
|
// Estimate resource usage time (simplified: assume 30 seconds)
|
|
var resourceUsageTime = 30.0; // seconds
|
|
var exit1 = arrival1.AddSeconds(resourceUsageTime);
|
|
var exit2 = arrival2.AddSeconds(resourceUsageTime);
|
|
|
|
// Check time overlap
|
|
if (arrival1 < exit2 && arrival2 < exit1)
|
|
{
|
|
return new Conflict
|
|
{
|
|
Type = ConflictType.Resource,
|
|
InvolvedRobots = new List<string> { route1.RobotId, route2.RobotId },
|
|
DetectedAt = DateTime.UtcNow,
|
|
Severity = ConflictSeverity.High,
|
|
ConflictDetails = new Dictionary<string, object>
|
|
{
|
|
{ "ResourceNodeId", nodeId },
|
|
{ "ResourceNodeName", node.NodeName ?? "Unknown" },
|
|
{ "Robot1Arrival", arrival1 },
|
|
{ "Robot2Arrival", arrival2 },
|
|
{ "ResourceUsageTime", resourceUsageTime }
|
|
}
|
|
};
|
|
}
|
|
}
|
|
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error detecting resource conflict between {route1.RobotId} and {route2.RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Helper Methods
|
|
|
|
/// <summary>
|
|
/// Check if future routes overlap after given segments
|
|
/// </summary>
|
|
private bool CheckFutureRoutesOverlap(RobotRoute route1, RobotRoute route2, RouteSegment seg1, RouteSegment seg2)
|
|
{
|
|
try
|
|
{
|
|
// Get segments after seg1 and seg2
|
|
var index1 = route1.FullRoute.IndexOf(seg1);
|
|
var index2 = route2.FullRoute.IndexOf(seg2);
|
|
|
|
if (index1 < 0 || index2 < 0) return false;
|
|
|
|
var future1 = route1.FullRoute.Skip(index1 + 1).Take(5).ToList(); // Next 5 segments
|
|
var future2 = route2.FullRoute.Skip(index2 + 1).Take(5).ToList();
|
|
|
|
// Check if any edges or nodes overlap
|
|
var edges1 = future1.Where(s => s.EdgeId != null).Select(s => s.EdgeId!.Value).ToHashSet();
|
|
var edges2 = future2.Where(s => s.EdgeId != null).Select(s => s.EdgeId!.Value).ToHashSet();
|
|
var nodes1 = future1.Where(s => s.NodeId != Guid.Empty).Select(s => s.NodeId).ToHashSet();
|
|
var nodes2 = future2.Where(s => s.NodeId != Guid.Empty).Select(s => s.NodeId).ToHashSet();
|
|
|
|
// Check edge overlap
|
|
if (edges1.Intersect(edges2).Any()) return true;
|
|
|
|
// Check node overlap
|
|
if (nodes1.Intersect(nodes2).Any()) return true;
|
|
|
|
return false;
|
|
}
|
|
catch
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Estimate arrival time at a segment
|
|
/// </summary>
|
|
private DateTime EstimateArrivalTime(RobotRoute? route, RouteSegment segment, StateMsg? state)
|
|
{
|
|
try
|
|
{
|
|
if (route == null)
|
|
{
|
|
_logger.Warning("Cannot estimate arrival time: route is null");
|
|
return DateTime.UtcNow;
|
|
}
|
|
|
|
var index = route.FullRoute.IndexOf(segment);
|
|
if (index < 0) return DateTime.UtcNow;
|
|
|
|
var segmentsToTarget = route.FullRoute.Take(index + 1).ToList();
|
|
var totalDistance = 0.0;
|
|
var defaultSpeed = 1.0; // m/s
|
|
|
|
// Estimate total distance (simplified)
|
|
foreach (var seg in segmentsToTarget)
|
|
{
|
|
if (seg.EdgeId != null)
|
|
{
|
|
// Estimate edge length (simplified: assume average 5m per edge)
|
|
totalDistance += 5.0;
|
|
}
|
|
}
|
|
|
|
var travelTime = totalDistance / defaultSpeed;
|
|
return DateTime.UtcNow.AddSeconds(travelTime);
|
|
}
|
|
catch
|
|
{
|
|
return DateTime.UtcNow;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Estimate position at a future time
|
|
/// </summary>
|
|
private (double X, double Y)? EstimatePositionAtTime(
|
|
RobotRoute route,
|
|
StateMsg state,
|
|
double timeSeconds)
|
|
{
|
|
try
|
|
{
|
|
if (state.AgvPosition == null) return null;
|
|
|
|
var currentX = state.AgvPosition.X;
|
|
var currentY = state.AgvPosition.Y;
|
|
var currentTheta = state.AgvPosition.Theta;
|
|
var defaultSpeed = 1.0; // m/s
|
|
|
|
// Estimate distance traveled
|
|
var distance = defaultSpeed * timeSeconds;
|
|
|
|
// Estimate position (simplified: assume straight line movement)
|
|
var estimatedX = currentX + distance * Math.Cos(currentTheta);
|
|
var estimatedY = currentY + distance * Math.Sin(currentTheta);
|
|
|
|
return (estimatedX, estimatedY);
|
|
}
|
|
catch
|
|
{
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if two edge sequences are opposite (EdgeAB vs EdgeBA)
|
|
/// </summary>
|
|
private async Task<bool> CheckEdgesAreOppositeAsync(
|
|
List<RouteSegment> seq1,
|
|
List<RouteSegment> seq2,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (seq1.Count != seq2.Count || seq1.Count == 0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Get levelId from first segment (assuming all segments are from same level)
|
|
// We need to get robotId from route, but we don't have route here
|
|
// So we'll need to check edges directly from database
|
|
if (seq1[0].EdgeId == null || seq2[0].EdgeId == null)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Get edges from database to check StartNodeId and EndNodeId
|
|
// We need levelId, but we don't have it here
|
|
// For now, we'll use a simplified check based on segment properties
|
|
// In a full implementation, we should get edges from database
|
|
|
|
// Simplified: Check if segments have opposite StartNodeId and EndNodeId
|
|
// This requires checking edge details, which we'll skip for now
|
|
// and assume edges are opposite if sequences are reversed
|
|
return true; // Simplified: assume opposite if sequences are reversed
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error checking if edges are opposite: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get levelId for a robot from Robot.MapId in database
|
|
/// </summary>
|
|
private async Task<Guid?> GetLevelIdForRobotAsync(string robotId, CancellationToken cancellationToken)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate edge length from start and end nodes (Euclidean distance)
|
|
/// </summary>
|
|
private 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>
|
|
/// Calculate reservation duration based on edge length and robot speed
|
|
/// </summary>
|
|
private 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
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
|