613 lines
22 KiB
C#
613 lines
22 KiB
C#
using RobotNet.VDA5050;
|
|
using RobotNet.VDA5050.InstantAction;
|
|
using RobotNet.VDA5050.Order;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.FleetManager.Data;
|
|
using RobotNet10.FleetManager.Script;
|
|
using RobotNet10.FleetManager.Services.ConfigManager;
|
|
using RobotNet10.FleetManager.Services.RobotConnections;
|
|
using RobotNet10.FleetManager.Services.RobotManager.Models;
|
|
using RobotNet10.FleetManager.Services.TrafficControl;
|
|
using RobotNet10.FleetManager.Services.TrafficControl.Models;
|
|
using RobotNet10.MapManager.Services;
|
|
using RobotNet10.Shared;
|
|
using Action = RobotNet.VDA5050.InstantAction.Action;
|
|
|
|
namespace RobotNet10.FleetManager.Services.RobotController;
|
|
|
|
/// <summary>
|
|
/// RobotController - instance per robot
|
|
/// Mô hình hóa thông tin của 1 robot, định danh bằng RobotId (SerialNumber)
|
|
/// </summary>
|
|
public class RobotController : IRobotController
|
|
{
|
|
private readonly IRobotConnectionsService _robotConnectionsService;
|
|
private readonly IConnectionConfig _configManager;
|
|
private readonly ITrafficControlService _trafficControlService;
|
|
private readonly IServiceScopeFactory _serviceScopeFactory;
|
|
private readonly Logger<RobotController> _logger;
|
|
private readonly SemaphoreSlim _methodLock = new(1, 1);
|
|
private readonly SemaphoreSlim _publishLock = new(1, 1);
|
|
|
|
private uint _headerIdCounter = 0;
|
|
private readonly Lock _headerIdLock = new();
|
|
|
|
private readonly TimeSpan SendTimeOut = TimeSpan.FromSeconds(10);
|
|
|
|
public string RobotId { get; }
|
|
public RobotData Data { get; }
|
|
public bool IsOnline => Data.ConnectionState == ConnectionState.ONLINE;
|
|
public bool IsReady => IsOnline && !IsRobotBusy();
|
|
|
|
public RobotController(
|
|
string robotId,
|
|
IRobotConnectionsService robotConnectionsService,
|
|
IConnectionConfig configManager,
|
|
ITrafficControlService trafficControlService,
|
|
IServiceScopeFactory serviceScopeFactory,
|
|
Logger<RobotController> logger)
|
|
{
|
|
RobotId = robotId;
|
|
_robotConnectionsService = robotConnectionsService;
|
|
_configManager = configManager;
|
|
_trafficControlService = trafficControlService;
|
|
_serviceScopeFactory = serviceScopeFactory;
|
|
_logger = logger;
|
|
|
|
Data = new RobotData
|
|
{
|
|
RobotId = robotId,
|
|
ConnectionState = ConnectionState.OFFLINE,
|
|
LastUpdated = DateTime.UtcNow
|
|
};
|
|
}
|
|
|
|
public async Task<MessageResult> MoveToNodeAsync(string nodeName, double? angle, CancellationToken cancellationToken = default)
|
|
{
|
|
if (string.IsNullOrEmpty(nodeName))
|
|
{
|
|
_logger.Warning($"Cannot move to node: nodeName is null or empty for robot {RobotId}");
|
|
return new(false, $"Cannot move to node: nodeName is null or empty for robot {RobotId}");
|
|
}
|
|
|
|
await _methodLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
// 1. Check if robot is busy with an order
|
|
if (IsRobotBusy())
|
|
{
|
|
_logger.Warning($"Cannot move to node: robot {RobotId} is busy with an order {Data.State?.OrderId}");
|
|
return new(false, $"Robot {RobotId} is busy with an order");
|
|
}
|
|
|
|
// 2. Get robot's levelId (MapId)
|
|
var levelId = await GetRobotLevelIdAsync();
|
|
if (levelId == null)
|
|
{
|
|
_logger.Warning($"Cannot move to node: robot {RobotId} has no MapId assigned");
|
|
return new(false, $"Robot {RobotId} has no MapId assigned");
|
|
}
|
|
|
|
// 3. Find goal node by NodeName
|
|
var goalNode = await FindNodeByNameAsync(levelId.Value, nodeName);
|
|
if (goalNode == null)
|
|
{
|
|
_logger.Warning($"Cannot move to node: node '{nodeName}' not found in level {levelId}");
|
|
return new(false, $"Node '{nodeName}' not found");
|
|
}
|
|
|
|
// 4. Plan route and send order
|
|
return await PlanRouteAndSendOrderAsync(goalNode.Id, angle, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error in MoveToNodeAsync for robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error in MoveToNodeAsync for robot {RobotId}: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
_methodLock.Release();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Move robot to a node by NodeId (string)
|
|
/// </summary>
|
|
public async Task<MessageResult> MoveToNodeByIdAsync(string nodeId, double? angle, CancellationToken cancellationToken = default)
|
|
{
|
|
if (string.IsNullOrEmpty(nodeId))
|
|
{
|
|
_logger.Warning($"Cannot move to node: nodeId is null or empty for robot {RobotId}");
|
|
return new(false, $"Cannot move to node: nodeId is null or empty for robot {RobotId}");
|
|
}
|
|
|
|
await _methodLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
// 1. Check if robot is busy with an order
|
|
if (IsRobotBusy())
|
|
{
|
|
_logger.Warning($"Cannot move to node: robot {RobotId} is busy with an order {Data.State?.OrderId}");
|
|
return new(false, $"Robot {RobotId} is busy with an order");
|
|
}
|
|
|
|
// 2. Get robot's levelId (MapId)
|
|
var levelId = await GetRobotLevelIdAsync();
|
|
if (levelId == null)
|
|
{
|
|
_logger.Warning($"Cannot move to node: robot {RobotId} has no MapId assigned");
|
|
return new(false, $"Robot {RobotId} has no MapId assigned");
|
|
}
|
|
|
|
// 3. Find goal node by NodeId (string)
|
|
var goalNode = await FindNodeByNodeIdAsync(levelId.Value, nodeId);
|
|
if (goalNode == null)
|
|
{
|
|
_logger.Warning($"Cannot move to node: node with NodeId '{nodeId}' not found in level {levelId}");
|
|
return new(false, $"Node with NodeId '{nodeId}' not found");
|
|
}
|
|
|
|
// 4. Plan route and send order
|
|
return await PlanRouteAndSendOrderAsync(goalNode.Id, angle, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error in MoveToNodeByIdAsync for robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error in MoveToNodeByIdAsync for robot {RobotId}: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
_methodLock.Release();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Move robot to a node by NodeId (Guid)
|
|
/// </summary>
|
|
public async Task<MessageResult> MoveToNodeByGuidAsync(Guid nodeId, double? angle, CancellationToken cancellationToken = default)
|
|
{
|
|
if (nodeId == Guid.Empty)
|
|
{
|
|
_logger.Warning($"Cannot move to node: nodeId is empty for robot {RobotId}");
|
|
return new(false, $"Cannot move to node: nodeId is empty for robot {RobotId}");
|
|
}
|
|
|
|
await _methodLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
// 1. Check if robot is busy with an order
|
|
if (IsRobotBusy())
|
|
{
|
|
_logger.Warning($"Cannot move to node: robot {RobotId} is busy with an order {Data.State?.OrderId}");
|
|
return new(false, $"Robot {RobotId} is busy with an order");
|
|
}
|
|
|
|
// 2. Verify node exists
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
|
var goalNode = await nodeService.GetByIdAsync(nodeId);
|
|
if (goalNode == null)
|
|
{
|
|
_logger.Warning($"Cannot move to node: node with Id '{nodeId}' not found");
|
|
return new(false, $"Node with Id '{nodeId}' not found");
|
|
}
|
|
|
|
// 3. Plan route and send order
|
|
return await PlanRouteAndSendOrderAsync(nodeId, angle, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error in MoveToNodeByGuidAsync for robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error in MoveToNodeByGuidAsync for robot {RobotId}: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
_methodLock.Release();
|
|
}
|
|
}
|
|
|
|
public Task<MessageResult> MoveToStationAsync(string nodeName, StationAction action, CancellationToken cancellationToken = default)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
public async Task<MessageResult> SendInstantActionAsync(Action action, CancellationToken cancellationToken = default)
|
|
{
|
|
if (action == null)
|
|
{
|
|
_logger.Warning($"Cannot send instant action: action is null for robot {RobotId}");
|
|
return new(false, $"Cannot send instant action: action is null for robot {RobotId}");
|
|
}
|
|
|
|
await _methodLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
var instantActions = new InstantActionsMsg
|
|
{
|
|
Actions = [action]
|
|
};
|
|
|
|
FillVDA5050Header(instantActions);
|
|
var publish = await _robotConnectionsService.PublishInstantActionsAsync(RobotId, instantActions, cancellationToken);
|
|
return new(publish);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error sending instant action to robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error sending instant action to robot {RobotId}: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
_methodLock.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<MessageResult> SendOrderAsync(OrderMsg order, CancellationToken cancellationToken = default)
|
|
{
|
|
if (order == null)
|
|
{
|
|
_logger.Warning($"Cannot send order: order message is null for robot {RobotId}");
|
|
return new(false, $"Cannot send order: order message is null for robot {RobotId}");
|
|
}
|
|
|
|
await _publishLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
// Fill header if not already set
|
|
if (order.HeaderId == 0)
|
|
{
|
|
FillVDA5050Header(order);
|
|
}
|
|
else
|
|
{
|
|
// Ensure SerialNumber matches
|
|
order.SerialNumber = RobotId;
|
|
}
|
|
|
|
var published = await _robotConnectionsService.PublishOrderAsync(RobotId, order, cancellationToken);
|
|
|
|
// Update order in RobotData when successfully published
|
|
if (published)
|
|
{
|
|
CancellationTokenSource cancelSend = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
|
|
cancelSend.CancelAfter(SendTimeOut);
|
|
while (true)
|
|
{
|
|
if (cancelSend.IsCancellationRequested) return new(false, "Publish order failed timout");
|
|
if (Data.State is not null)
|
|
{
|
|
if (Data.State.OrderId == order.OrderId && IsRobotBusy() && Data.State.OrderUpdateId == order.OrderUpdateId)
|
|
{
|
|
Data.LastUpdated = DateTime.UtcNow;
|
|
Data.Order = order;
|
|
return new(true);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return new(false, "Publish order failed");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error sending order to robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error sending order to robot {RobotId}");
|
|
}
|
|
finally
|
|
{
|
|
_publishLock.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<MessageResult> SendInstantActionsAsync(InstantActionsMsg instantActions, CancellationToken cancellationToken = default)
|
|
{
|
|
if (instantActions == null)
|
|
{
|
|
_logger.Warning($"Cannot send instant actions: instantActions message is null for robot {RobotId}");
|
|
return new(false, $"Cannot send instant actions: instantActions message is null for robot {RobotId}");
|
|
}
|
|
|
|
if (instantActions.Actions == null || instantActions.Actions.Length == 0)
|
|
{
|
|
_logger.Warning($"Cannot send instant actions: actions array is null or empty for robot {RobotId}");
|
|
return new(false, $"Cannot send instant actions: actions array is null or empty for robot {RobotId}");
|
|
}
|
|
|
|
await _publishLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
// Fill header if not already set
|
|
if (instantActions.HeaderId == 0)
|
|
{
|
|
FillVDA5050Header(instantActions);
|
|
}
|
|
else
|
|
{
|
|
// Ensure SerialNumber matches
|
|
instantActions.SerialNumber = RobotId;
|
|
}
|
|
|
|
var publish = await _robotConnectionsService.PublishInstantActionsAsync(RobotId, instantActions, cancellationToken);
|
|
return new(publish);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error sending instant actions to robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error sending instant actions to robot {RobotId}");
|
|
}
|
|
finally
|
|
{
|
|
_publishLock.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<MessageResult> RequestFactsheetAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
await _methodLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
var action = new Action
|
|
{
|
|
ActionType = ActionType.FACTSHEET_REQUEST.ToJsonString(),
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
BlockingType = BlockingType.NONE
|
|
};
|
|
|
|
return await SendInstantActionAsync(action, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error requesting factsheet from robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error requesting factsheet from robot {RobotId}");
|
|
}
|
|
finally
|
|
{
|
|
_methodLock.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<MessageResult> RequestStateAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
await _methodLock.WaitAsync(cancellationToken);
|
|
try
|
|
{
|
|
var action = new Action
|
|
{
|
|
ActionType = ActionType.STATE_REQUEST.ToJsonString(),
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
BlockingType = BlockingType.NONE
|
|
};
|
|
|
|
return await SendInstantActionAsync(action, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error requesting state from robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error requesting state from robot {RobotId}");
|
|
}
|
|
finally
|
|
{
|
|
_methodLock.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<MessageResult> CancelOrderAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
try
|
|
{
|
|
var action = new Action
|
|
{
|
|
ActionType = ActionType.CANCEL_ORDER.ToJsonString(),
|
|
ActionId = Guid.NewGuid().ToString(),
|
|
BlockingType = BlockingType.NONE,
|
|
ActionDescription = "Cancel current order"
|
|
};
|
|
|
|
var result = await SendInstantActionAsync(action, cancellationToken);
|
|
if (result.IsSuccess && Data.State != null)
|
|
{
|
|
// Clear order state immediately so a new order can be accepted without waiting
|
|
// for the robot to report empty NodeStates/EdgeStates (avoids "robot is busy" after cancel).
|
|
Data.State.NodeStates = [];
|
|
Data.State.EdgeStates = [];
|
|
Data.State.OrderId = string.Empty;
|
|
Data.State.OrderUpdateId = 0;
|
|
Data.OrderClearedByCancelAt = DateTime.UtcNow;
|
|
_logger.Info($"Robot {RobotId}: cleared order state after cancel (ready for new order)");
|
|
}
|
|
return result;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error canceling order for robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error canceling order for robot {RobotId}");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if robot is busy with an order
|
|
/// </summary>
|
|
private bool IsRobotBusy()
|
|
{
|
|
// Robot is busy if order status is Sent or Accepted
|
|
return Data.State?.NodeStates.Length > 0 || Data.State?.EdgeStates.Length > 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get robot's levelId (MapId) from database
|
|
/// </summary>
|
|
private async Task<Guid?> GetRobotLevelIdAsync()
|
|
{
|
|
try
|
|
{
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
|
var robot = await robotService.GetByRobotIdAsync(RobotId);
|
|
return robot?.MapId; // MapId is the levelId
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error getting levelId for robot {RobotId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find node by NodeName in a level
|
|
/// </summary>
|
|
private async Task<RobotNet10.MapManager.Data.Node?> FindNodeByNameAsync(Guid levelId, string nodeName)
|
|
{
|
|
try
|
|
{
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
|
|
|
// Get all nodes in level
|
|
var nodes = await nodeService.GetNodesByLevelAsync(levelId);
|
|
|
|
// Find by NodeName (case-insensitive)
|
|
var node = nodes.FirstOrDefault(n =>
|
|
!string.IsNullOrEmpty(n.NodeName) &&
|
|
n.NodeName.Equals(nodeName, StringComparison.OrdinalIgnoreCase));
|
|
|
|
return node;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error finding node by name '{nodeName}' in level {levelId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find node by NodeId (string) in a level
|
|
/// </summary>
|
|
private async Task<RobotNet10.MapManager.Data.Node?> FindNodeByNodeIdAsync(Guid levelId, string nodeId)
|
|
{
|
|
try
|
|
{
|
|
using var scope = _serviceScopeFactory.CreateScope();
|
|
var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
|
|
|
|
// Get all nodes in level
|
|
var nodes = await nodeService.GetNodesByLevelAsync(levelId);
|
|
|
|
// Find by NodeId (case-insensitive)
|
|
var node = nodes.FirstOrDefault(n =>
|
|
n.NodeId.Equals(nodeId, StringComparison.OrdinalIgnoreCase));
|
|
|
|
return node;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error finding node by NodeId '{nodeId}' in level {levelId}: {ex.Message}");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Plan route and send order to robot
|
|
/// </summary>
|
|
private async Task<MessageResult> PlanRouteAndSendOrderAsync(Guid goalNodeId, double? angle, CancellationToken cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
// Check if robot has state (required for route planning)
|
|
if (Data.State == null || Data.State.AgvPosition == null)
|
|
{
|
|
_logger.Warning($"Cannot plan route: robot {RobotId} has no state or position");
|
|
return new(false, $"Robot {RobotId} has no state or position");
|
|
}
|
|
|
|
// Cancel current order if exists
|
|
if (IsRobotBusy())
|
|
{
|
|
return new(false, $"The robot is working on order {Data.State.OrderId}");
|
|
}
|
|
|
|
// Plan route using TrafficControlService from current position
|
|
// Get current position from State.AgvPosition
|
|
var currentX = Data.State.AgvPosition.X;
|
|
var currentY = Data.State.AgvPosition.Y;
|
|
var currentThetaRadians = Data.State.AgvPosition.Theta; // radians
|
|
var currentThetaDegrees = currentThetaRadians * 180.0 / Math.PI; // convert to degrees
|
|
|
|
// Plan route from current position to goal node
|
|
RobotRoute? route = await _trafficControlService.PlanRouteFromPositionACSTrafficAsync(
|
|
RobotId,
|
|
currentX,
|
|
currentY,
|
|
currentThetaDegrees,
|
|
goalNodeId,
|
|
angle, // goalAngle in degrees
|
|
null, // startDirection
|
|
null, // finalDirection
|
|
cancellationToken);
|
|
|
|
if (route == null)
|
|
{
|
|
_logger.Warning($"Cannot plan route to node {goalNodeId} for robot {RobotId}");
|
|
return new(false, $"Failed to plan route to node {goalNodeId}");
|
|
}
|
|
return new(true);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error planning route and sending order for robot {RobotId}: {ex.Message}");
|
|
return new(false, $"Error planning route: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private uint GetNextHeaderId()
|
|
{
|
|
lock (_headerIdLock)
|
|
{
|
|
_headerIdCounter++;
|
|
if (_headerIdCounter == 0) // Handle overflow
|
|
{
|
|
_headerIdCounter = 1;
|
|
}
|
|
return _headerIdCounter;
|
|
}
|
|
}
|
|
|
|
private void FillVDA5050Header(OrderMsg msg)
|
|
{
|
|
var config = _configManager.GetVDA5050Config();
|
|
msg.HeaderId = GetNextHeaderId();
|
|
msg.Timestamp = DateTime.UtcNow;
|
|
msg.Version = config.Version;
|
|
msg.Manufacturer = config.Manufacturer;
|
|
msg.SerialNumber = RobotId;
|
|
}
|
|
|
|
private void FillVDA5050Header(InstantActionsMsg msg)
|
|
{
|
|
var config = _configManager.GetVDA5050Config();
|
|
msg.HeaderId = 1;
|
|
msg.Timestamp = DateTime.UtcNow;
|
|
msg.Version = config.Version;
|
|
msg.Manufacturer = config.Manufacturer;
|
|
msg.SerialNumber = RobotId;
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
// Cleanup resources if needed
|
|
_methodLock?.Dispose();
|
|
// RobotData will be cleaned up by GC
|
|
_logger.Debug($"RobotController disposed for robot {RobotId}");
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|