406 lines
16 KiB
C#
406 lines
16 KiB
C#
using RobotNet10.FleetManager.Services.RobotManager;
|
|
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
|
|
|
namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
|
|
|
|
/// <summary>
|
|
/// Service for generating and sending OrderUpdate messages
|
|
/// </summary>
|
|
public class OrderUpdateService(
|
|
Logger<OrderUpdateService> logger,
|
|
IServiceScopeFactory serviceScopeFactory) : IOrderUpdateService
|
|
{
|
|
private readonly Logger<OrderUpdateService> _logger = logger;
|
|
private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
|
|
|
|
public async Task<bool> GenerateAndSendOrderUpdateAsync(
|
|
string robotId,
|
|
RobotRoute route,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
// Resolve IRobotManagerService lazily to avoid circular dependency
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
|
var robotController = robotManager.GetRobotController(robotId);
|
|
if (robotController == null)
|
|
{
|
|
_logger.Warning($"Cannot send OrderUpdate: robot controller not found for robot {robotId}");
|
|
return false;
|
|
}
|
|
|
|
// Generate OrderUpdate from Horizon
|
|
var orderUpdate = await GenerateOrderUpdateFromHorizonAsync(route, robotId);
|
|
if (orderUpdate == null)
|
|
{
|
|
_logger.Warning($"Cannot send OrderUpdate: failed to generate order update for robot {robotId}");
|
|
return false;
|
|
}
|
|
|
|
// Send OrderUpdate
|
|
var sent = await robotController.SendOrderAsync(orderUpdate, cancellationToken);
|
|
if (sent.IsSuccess)
|
|
{
|
|
// Increment OrderUpdateId
|
|
route.OrderUpdateId++;
|
|
_logger.Info($"Sent OrderUpdate (ID: {route.OrderUpdateId}) to robot {robotId}");
|
|
}
|
|
|
|
return sent.IsSuccess;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error generating and sending OrderUpdate to robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public async Task<bool> SendOrderUpdateAsync(
|
|
string robotId,
|
|
List<RouteSegment> newSegments,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
if (string.IsNullOrWhiteSpace(robotId))
|
|
{
|
|
_logger.Warning("Cannot send OrderUpdate: robotId is null or empty");
|
|
return false;
|
|
}
|
|
|
|
if (newSegments == null || newSegments.Count == 0)
|
|
{
|
|
_logger.Warning($"Cannot send OrderUpdate: no new segments provided for robot {robotId}");
|
|
return false;
|
|
}
|
|
|
|
// Resolve IRobotManagerService lazily to avoid circular dependency
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
|
var robotController = robotManager.GetRobotController(robotId);
|
|
if (robotController == null)
|
|
{
|
|
_logger.Warning($"Cannot send OrderUpdate: robot controller not found for robot {robotId}");
|
|
return false;
|
|
}
|
|
|
|
// Get current order from RobotController
|
|
var currentOrder = robotController.Data.Order;
|
|
if (currentOrder == null)
|
|
{
|
|
_logger.Warning($"Cannot send OrderUpdate: no current order found for robot {robotId}");
|
|
return false;
|
|
}
|
|
|
|
// Create OrderUpdate (keep same orderId, increment orderUpdateId)
|
|
var orderUpdate = new RobotNet.VDA5050.Order.OrderMsg
|
|
{
|
|
OrderId = currentOrder.OrderId,
|
|
OrderUpdateId = currentOrder.OrderUpdateId + 1,
|
|
SerialNumber = robotId,
|
|
Timestamp = DateTime.UtcNow,
|
|
Version = currentOrder.Version,
|
|
Manufacturer = currentOrder.Manufacturer
|
|
};
|
|
|
|
// Copy existing nodes and edges from current order
|
|
var existingNodes = currentOrder.Nodes?.ToList() ?? [];
|
|
var existingEdges = currentOrder.Edges?.ToList() ?? [];
|
|
|
|
// Get MapId for NodePosition (once for all segments)
|
|
var mapId = await GetMapIdForRobotAsync(robotId);
|
|
if (string.IsNullOrEmpty(mapId))
|
|
{
|
|
_logger.Warning($"Cannot get MapId for robot {robotId} to create NodePosition");
|
|
return false;
|
|
}
|
|
|
|
// Add new nodes and edges from segments (using VdaNode and VdaEdge directly)
|
|
foreach (var segment in newSegments)
|
|
{
|
|
if (segment.VdaNode != null)
|
|
{
|
|
// Clone node and set released = true for new segments
|
|
var node = CloneNode(segment.VdaNode);
|
|
node.Released = true;
|
|
existingNodes.Add(node);
|
|
}
|
|
|
|
if (segment.VdaEdge != null)
|
|
{
|
|
// Clone edge and set released = true for new segments
|
|
var edge = CloneEdge(segment.VdaEdge);
|
|
edge.Released = true;
|
|
existingEdges.Add(edge);
|
|
}
|
|
}
|
|
|
|
// Set arrays
|
|
orderUpdate.Nodes = [.. existingNodes];
|
|
orderUpdate.Edges = [.. existingEdges];
|
|
|
|
// Send via RobotController
|
|
var sent = await robotController.SendOrderAsync(orderUpdate, cancellationToken);
|
|
if (sent.IsSuccess)
|
|
{
|
|
_logger.Info($"Sent OrderUpdate (ID: {orderUpdate.OrderUpdateId}) to robot {robotId} with {newSegments.Count} new segments");
|
|
}
|
|
|
|
return sent.IsSuccess;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error sending OrderUpdate to robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public async Task<bool> SendInitialOrderAsync(
|
|
string robotId,
|
|
RobotRoute route,
|
|
CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
// Resolve IRobotManagerService lazily to avoid circular dependency
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
|
|
var robotController = robotManager.GetRobotController(robotId);
|
|
if (robotController == null)
|
|
{
|
|
_logger.Warning($"Cannot send initial Order: robot controller not found for robot {robotId}");
|
|
return false;
|
|
}
|
|
|
|
// Get MapId (LevelId) for NodePosition
|
|
var mapId = await GetMapIdForRobotAsync(robotId);
|
|
if (string.IsNullOrEmpty(mapId))
|
|
{
|
|
_logger.Warning($"Cannot get MapId for robot {robotId} to create NodePosition");
|
|
return false;
|
|
}
|
|
|
|
// Build nodes and edges from Base segments
|
|
var nodes = new List<RobotNet.VDA5050.Order.Node>();
|
|
var edges = new List<RobotNet.VDA5050.Order.Edge>();
|
|
|
|
foreach (var segment in route.Base)
|
|
{
|
|
if (segment.VdaNode != null)
|
|
{
|
|
// Node segment with full information
|
|
// Base segments are always released
|
|
var node = CloneNode(segment.VdaNode);
|
|
node.Released = true; // Base segments are always released
|
|
nodes.Add(node);
|
|
}
|
|
|
|
if (segment.VdaEdge != null)
|
|
{
|
|
// Edge segment with full information
|
|
var edge = CloneEdge(segment.VdaEdge);
|
|
edge.Released = true; // Base segments are always released
|
|
edges.Add(edge);
|
|
}
|
|
}
|
|
|
|
// Create initial Order
|
|
var order = new RobotNet.VDA5050.Order.OrderMsg
|
|
{
|
|
OrderId = route.OrderId,
|
|
OrderUpdateId = 0, // Initial order has OrderUpdateId = 0
|
|
SerialNumber = robotId,
|
|
Timestamp = DateTime.UtcNow,
|
|
Nodes = [.. nodes],
|
|
Edges = [.. edges]
|
|
};
|
|
|
|
// Send via RobotController
|
|
var sent = await robotController.SendOrderAsync(order, cancellationToken);
|
|
if (sent.IsSuccess)
|
|
{
|
|
route.OrderUpdateId = 0; // Ensure OrderUpdateId is set
|
|
_logger.Info($"Sent initial Order (ID: {route.OrderId}, UpdateID: 0) to robot {robotId} with {route.Base.Count} base segments");
|
|
}
|
|
|
|
return sent.IsSuccess;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error sending initial Order to robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Generate OrderUpdate from Horizon segments
|
|
/// </summary>
|
|
private async Task<RobotNet.VDA5050.Order.OrderMsg?> GenerateOrderUpdateFromHorizonAsync(RobotRoute route, string robotId)
|
|
{
|
|
try
|
|
{
|
|
// Get MapId (LevelId) for NodePosition
|
|
var mapId = await GetMapIdForRobotAsync(robotId);
|
|
if (string.IsNullOrEmpty(mapId))
|
|
{
|
|
_logger.Warning($"Cannot get MapId for robot {robotId} to create NodePosition");
|
|
return null;
|
|
}
|
|
|
|
// Get last node of Base (decision point)
|
|
var lastBaseNode = route.Base.LastOrDefault(s => s.VdaNode != null);
|
|
if (lastBaseNode == null && route.Horizon.Count > 0)
|
|
{
|
|
// If no Base, use first node of Horizon
|
|
lastBaseNode = route.Horizon.FirstOrDefault(s => s.VdaNode != null);
|
|
}
|
|
|
|
if (lastBaseNode?.VdaNode == null)
|
|
{
|
|
_logger.Warning("Cannot generate OrderUpdate: no base node found");
|
|
return null;
|
|
}
|
|
|
|
// Build nodes and edges from Horizon
|
|
var nodes = new List<RobotNet.VDA5050.Order.Node>();
|
|
var edges = new List<RobotNet.VDA5050.Order.Edge>();
|
|
|
|
// Start with last base node (released) - Base nodes cannot have actions
|
|
var lastBaseVdaNode = CloneNode(lastBaseNode.VdaNode);
|
|
lastBaseVdaNode.Released = true;
|
|
nodes.Add(lastBaseVdaNode);
|
|
|
|
// Add Horizon segments (only Horizon nodes can have wait actions)
|
|
foreach (var segment in route.Horizon)
|
|
{
|
|
if (segment.VdaNode != null)
|
|
{
|
|
// Node segment with full information
|
|
var node = CloneNode(segment.VdaNode);
|
|
nodes.Add(node);
|
|
}
|
|
else if (segment.VdaEdge != null)
|
|
{
|
|
// Edge segment with full information
|
|
var edge = CloneEdge(segment.VdaEdge);
|
|
edges.Add(edge);
|
|
}
|
|
}
|
|
|
|
// Create OrderUpdate
|
|
var orderUpdate = new RobotNet.VDA5050.Order.OrderMsg
|
|
{
|
|
OrderId = route.OrderId,
|
|
OrderUpdateId = route.OrderUpdateId + 1, // Increment for new update
|
|
Nodes = [.. nodes],
|
|
Edges = [.. edges]
|
|
};
|
|
|
|
return orderUpdate;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error generating OrderUpdate from Horizon: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clone VDA5050 Node (create a copy to avoid modifying original)
|
|
/// </summary>
|
|
private static RobotNet.VDA5050.Order.Node CloneNode(RobotNet.VDA5050.Order.Node source)
|
|
{
|
|
return new RobotNet.VDA5050.Order.Node
|
|
{
|
|
NodeId = source.NodeId,
|
|
SequenceId = source.SequenceId,
|
|
Released = source.Released,
|
|
NodeDescription = source.NodeDescription,
|
|
NodePosition = source.NodePosition is null ? null : new RobotNet.VDA5050.Order.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] // Clone array
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clone VDA5050 Edge (create a copy to avoid modifying original)
|
|
/// </summary>
|
|
private static RobotNet.VDA5050.Order.Edge CloneEdge(RobotNet.VDA5050.Order.Edge source)
|
|
{
|
|
return new RobotNet.VDA5050.Order.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 RobotNet.VDA5050.Order.Trajectory
|
|
{
|
|
Degree = source.Trajectory.Degree,
|
|
KnotVector = [.. source.Trajectory.KnotVector], // Clone array
|
|
ControlPoints = [.. source.Trajectory.ControlPoints.Select(cp => new RobotNet.VDA5050.Order.ControlPoint
|
|
{
|
|
X = cp.X,
|
|
Y = cp.Y,
|
|
Weight = cp.Weight
|
|
})]
|
|
},
|
|
Corridor = source.Corridor == null ? null : new RobotNet.VDA5050.Order.Corridor
|
|
{
|
|
LeftWidth = source.Corridor.LeftWidth,
|
|
RightWidth = source.Corridor.RightWidth,
|
|
CorridorRefPoint = source.Corridor.CorridorRefPoint
|
|
},
|
|
Actions = [.. source.Actions] // Clone array
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get MapId (LevelId) for a robot to use in NodePosition
|
|
/// </summary>
|
|
private async Task<string> GetMapIdForRobotAsync(string robotId)
|
|
{
|
|
try
|
|
{
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
|
|
|
var robot = await robotService.GetByRobotIdAsync(robotId);
|
|
if (robot == null || robot.MapId == null)
|
|
{
|
|
_logger.Warning($"Robot {robotId} not found or has no MapId assigned");
|
|
return string.Empty;
|
|
}
|
|
|
|
// Robot.MapId is the LevelId, convert to string for MapId in NodePosition
|
|
return robot.MapId.ToString() ?? "";
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error getting MapId for robot {robotId}: {ex.Message}");
|
|
return string.Empty;
|
|
}
|
|
}
|
|
|
|
}
|
|
|