330 lines
12 KiB
C#
330 lines
12 KiB
C#
using MQTTnet;
|
|
using MQTTnet.Packets;
|
|
using RobotNet.VDA5050;
|
|
using RobotNet.VDA5050.Connection;
|
|
using RobotNet.VDA5050.Factsheet;
|
|
using RobotNet.VDA5050.InstantAction;
|
|
using RobotNet.VDA5050.Order;
|
|
using RobotNet.VDA5050.State;
|
|
using RobotNet.VDA5050.Visualization;
|
|
using RobotNet10.FleetManager.Events;
|
|
using RobotNet10.FleetManager.Services.ConfigManager;
|
|
using RobotNet10.MqttConnection;
|
|
using System.Text;
|
|
using System.Text.Json;
|
|
|
|
namespace RobotNet10.FleetManager.Services.RobotConnections;
|
|
|
|
/// <summary>
|
|
/// Service implementation for managing MQTT connections to robots via VDA5050 protocol
|
|
/// </summary>
|
|
public class RobotConnectionsService(
|
|
IConnectionConfig configManager,
|
|
IRobotEventBus eventBus,
|
|
IServiceProvider serviceProvider,
|
|
Logger<RobotConnectionsService> logger,
|
|
ILogger<MQTTClient> mqttLogger) : IRobotConnectionsService
|
|
{
|
|
private readonly IConnectionConfig _configManager = configManager;
|
|
private readonly IRobotEventBus _eventBus = eventBus;
|
|
private readonly IServiceProvider _serviceProvider = serviceProvider;
|
|
private readonly Logger<RobotConnectionsService> _logger = logger;
|
|
private readonly ILogger<MQTTClient> _mqttLogger = mqttLogger;
|
|
|
|
private MQTTClient? _mqttClient;
|
|
private readonly SemaphoreSlim _connectionSemaphore = new(1, 1);
|
|
|
|
public bool IsConnected => _mqttClient is not null && _mqttClient.IsConnected;
|
|
|
|
public async Task StartAsync(CancellationToken? cancellationToken)
|
|
{
|
|
try
|
|
{
|
|
await StopAsync();
|
|
|
|
if (!_connectionSemaphore.Wait(1000)) return;
|
|
var mqttConfig = _configManager.GetMqttConfig();
|
|
var vdaConfig = _configManager.GetVDA5050Config();
|
|
|
|
MqttTopicFilter[] topics = [
|
|
new MqttTopicFilterBuilder().WithTopic($"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/+/{VDA5050Topic.STATE.ToJsonString()}")
|
|
.WithAtMostOnceQoS()
|
|
.Build(),
|
|
new MqttTopicFilterBuilder().WithTopic($"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/+/{VDA5050Topic.VISUALIZATION.ToJsonString()}")
|
|
.WithAtMostOnceQoS()
|
|
.Build(),
|
|
new MqttTopicFilterBuilder().WithTopic($"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/+/{VDA5050Topic.FACTSHEET.ToJsonString()}")
|
|
.WithAtMostOnceQoS()
|
|
.Build(),
|
|
new MqttTopicFilterBuilder().WithTopic($"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/+/{VDA5050Topic.CONNECTION.ToJsonString()}")
|
|
.WithAtMostOnceQoS()
|
|
.Build()
|
|
];
|
|
|
|
_mqttClient = new MQTTClient(mqttConfig, topics, _mqttLogger);
|
|
_mqttClient.MessageUpdated += MessageUpdated;
|
|
if (_mqttClient is not null) await _mqttClient.ConnectAsync(cancellationToken);
|
|
if (_mqttClient is not null) await _mqttClient.SubscribeAsync(cancellationToken);
|
|
|
|
_logger.Info("RobotConnectionsService started successfully");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Warning($"Connection broker is failed: {ex.Message}");
|
|
}
|
|
finally
|
|
{
|
|
_connectionSemaphore.Release();
|
|
}
|
|
}
|
|
|
|
public async Task StopAsync()
|
|
{
|
|
if (_mqttClient is not null)
|
|
{
|
|
await _mqttClient.DisposeAsync();
|
|
_mqttClient = null;
|
|
}
|
|
_logger.Info("RobotConnectionsService stopped");
|
|
}
|
|
|
|
private async Task MessageUpdated(MqttApplicationMessageReceivedEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
var topic = e.ApplicationMessage.Topic;
|
|
var payload = Encoding.UTF8.GetString(e.ApplicationMessage.Payload);
|
|
var (robotId, messageType) = ParseVDA5050Topic(topic);
|
|
if (!string.IsNullOrEmpty(robotId) && !string.IsNullOrEmpty(messageType))
|
|
{
|
|
using var scope = _serviceProvider.CreateAsyncScope();
|
|
var _robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
|
|
if (await _robotService.ExistsAsync(robotId))
|
|
{
|
|
if (messageType == VDA5050Topic.STATE.ToJsonString())
|
|
{
|
|
HandleStateMessageAsync(robotId, payload);
|
|
}
|
|
else if (messageType == VDA5050Topic.VISUALIZATION.ToJsonString())
|
|
{
|
|
HandleVisualizationMessageAsync(robotId, payload);
|
|
}
|
|
else if (messageType == VDA5050Topic.FACTSHEET.ToJsonString())
|
|
{
|
|
HandleFactsheetMessageAsync(robotId, payload);
|
|
}
|
|
else if (messageType == VDA5050Topic.CONNECTION.ToJsonString())
|
|
{
|
|
HandleConnectionMessageAsync(robotId, payload);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger.Warning("Failed to parse topic");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Warning($"Error processing message: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private (string? robotId, string? messageType) ParseVDA5050Topic(string topic)
|
|
{
|
|
try
|
|
{
|
|
if (string.IsNullOrEmpty(topic)) return (null, null);
|
|
|
|
var vdaConfig = _configManager.GetVDA5050Config();
|
|
ReadOnlySpan<char> topicSpan = topic.AsSpan();
|
|
var manufacturerSpan = $"/{vdaConfig.Manufacturer}/".AsSpan();
|
|
int manufacturerIndex = topicSpan.IndexOf(manufacturerSpan);
|
|
|
|
if (manufacturerIndex == -1) return (null, null);
|
|
|
|
var remaining = topicSpan[(manufacturerIndex + manufacturerSpan.Length)..];
|
|
int firstSlash = remaining.IndexOf('/');
|
|
if (firstSlash == -1) return (null, null);
|
|
|
|
var robotId = remaining[..firstSlash].ToString();
|
|
var messageType = remaining[(firstSlash + 1)..].ToString();
|
|
|
|
return (robotId, messageType);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Warning($"Parse VDA5050 Topic failed: {ex}");
|
|
return (null, null);
|
|
}
|
|
}
|
|
|
|
public async Task<bool> PublishOrderAsync(string robotId, OrderMsg order, CancellationToken cancellationToken = default)
|
|
{
|
|
if (_mqttClient is null)
|
|
{
|
|
_logger.Warning("Mqtt Client not initialized");
|
|
return false;
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(robotId))
|
|
{
|
|
_logger.Warning("Cannot publish order: robotId is null or empty");
|
|
return false;
|
|
}
|
|
|
|
if (order == null)
|
|
{
|
|
_logger.Warning("Cannot publish order: order message is null");
|
|
return false;
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(order.OrderId))
|
|
{
|
|
_logger.Warning("Cannot publish order: orderId is null or empty");
|
|
return false;
|
|
}
|
|
|
|
if (!IsConnected)
|
|
{
|
|
_logger.Warning($"Cannot publish order to robot {robotId}: MQTT client is not connected");
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
var topic = BuildPublishTopic(robotId, VDA5050Topic.ORDER);
|
|
var data = JsonSerializer.Serialize(order, JsonOptionExtends.Write);
|
|
await _mqttClient.PublishAsync(topic, data);
|
|
return true;
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
_logger.Warning($"Publish order to robot {robotId} was cancelled");
|
|
return false;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error publishing order to robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public async Task<bool> PublishInstantActionsAsync(string robotId, InstantActionsMsg instantActions, CancellationToken cancellationToken = default)
|
|
{
|
|
if (_mqttClient is null)
|
|
{
|
|
_logger.Warning("Mqtt Client not initialized");
|
|
return false;
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(robotId))
|
|
{
|
|
_logger.Warning("Cannot publish instantActions: robotId is null or empty");
|
|
return false;
|
|
}
|
|
|
|
if (instantActions == null)
|
|
{
|
|
_logger.Warning("Cannot publish instantActions: instantActions message is null");
|
|
return false;
|
|
}
|
|
|
|
if (instantActions.Actions == null || instantActions.Actions.Length == 0)
|
|
{
|
|
_logger.Warning("Cannot publish instantActions: actions array is null or empty");
|
|
return false;
|
|
}
|
|
|
|
if (!IsConnected)
|
|
{
|
|
_logger.Warning($"Cannot publish instantActions to robot {robotId}: MQTT client is not connected");
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
var topic = BuildPublishTopic(robotId, VDA5050Topic.INSTANTACTIONS);
|
|
var data = JsonSerializer.Serialize(instantActions, JsonOptionExtends.Write);
|
|
await _mqttClient.PublishAsync(topic, data);
|
|
return true;
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
_logger.Warning($"Publish instantActions to robot {robotId} was cancelled");
|
|
return false;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error publishing instantActions to robot {robotId}: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private void HandleStateMessageAsync(string serialNumber, string payload)
|
|
{
|
|
try
|
|
{
|
|
var stateMsg = JsonSerializer.Deserialize<StateMsg>(payload, JsonOptionExtends.Read);
|
|
if (stateMsg == null || stateMsg.SerialNumber != serialNumber) return;
|
|
|
|
_eventBus.PublishStateMessageReceived(serialNumber, stateMsg);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error handling state message from robot {serialNumber}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void HandleConnectionMessageAsync(string serialNumber, string payload)
|
|
{
|
|
try
|
|
{
|
|
var connectionMsg = JsonSerializer.Deserialize<ConnectionMsg>(payload, JsonOptionExtends.Read);
|
|
if (connectionMsg == null || connectionMsg.SerialNumber != serialNumber) return;
|
|
|
|
_eventBus.PublishConnectionStateChanged(serialNumber, connectionMsg.ConnectionState);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error handling connection message from robot {serialNumber}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void HandleVisualizationMessageAsync(string serialNumber, string payload)
|
|
{
|
|
try
|
|
{
|
|
var visualizationMsg = JsonSerializer.Deserialize<VisualizationMsg>(payload, JsonOptionExtends.Read);
|
|
if (visualizationMsg == null || visualizationMsg.SerialNumber != serialNumber) return;
|
|
|
|
_eventBus.PublishVisualizationMessageReceived(serialNumber, visualizationMsg);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error handling visualization message from robot {serialNumber}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void HandleFactsheetMessageAsync(string serialNumber, string payload)
|
|
{
|
|
try
|
|
{
|
|
var factsheetMsg = JsonSerializer.Deserialize<FactSheetMsg>(payload, JsonOptionExtends.Read);
|
|
if (factsheetMsg == null || factsheetMsg.SerialNumber != serialNumber) return;
|
|
|
|
_eventBus.PublishFactsheetMessageReceived(serialNumber, factsheetMsg);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.Error($"Error handling factsheet message from robot {serialNumber}: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private string BuildPublishTopic(string robotId, VDA5050Topic topic)
|
|
{
|
|
var vdaConfig = _configManager.GetVDA5050Config();
|
|
return $"{vdaConfig.TopicPrefix}/{vdaConfig.Manufacturer}/{robotId}/{topic.ToJsonString()}";
|
|
}
|
|
}
|