RobotNet/RobotNet.RobotManager/Services/RobotManager.cs
2025-10-15 15:15:53 +07:00

563 lines
23 KiB
C#

using MQTTnet;
using MQTTnet.Protocol;
using RobotNet.RobotManager.Data;
using RobotNet.RobotManager.Services.Robot;
using RobotNet.RobotManager.Services.Simulation;
using RobotNet.RobotManager.Services.Traffic;
using RobotNet.RobotShares.Dtos;
using RobotNet.RobotShares.OpenACS;
using RobotNet.RobotShares.VDA5050;
using RobotNet.RobotShares.VDA5050.Connection;
using RobotNet.RobotShares.VDA5050.Factsheet;
using RobotNet.RobotShares.VDA5050.FactsheetExtend;
using RobotNet.RobotShares.VDA5050.State;
using RobotNet.RobotShares.VDA5050.Visualization;
using RobotNet.Script.Expressions;
using RobotNet.Shares;
using System.Linq.Expressions;
using System.Text;
using System.Text.Json;
namespace RobotNet.RobotManager.Services;
public class RobotManager : BackgroundService
{
public IEnumerable<string> RobotSerialNumbers => RobotControllers.Keys;
private readonly VDA5050Setting VDA5050Setting = new();
private Dictionary<string, IRobotController> RobotControllers { get; } = [];
private readonly IServiceProvider ServiceProvider;
private IMqttClient? MQTTClient;
private readonly LoggerController<RobotManager> Logger;
public RobotManager(IConfiguration configuration, IServiceProvider serviceProvider, LoggerController<RobotManager> logger)
{
configuration.Bind("VDA5050Setting", VDA5050Setting);
ServiceProvider = serviceProvider;
Logger = logger;
}
public IRobotController? this[string robotid] => RobotControllers.TryGetValue(robotid, out IRobotController? value) ? value : null;
private RobotController? AddRobotController(string robotId)
{
using var scope = ServiceProvider.CreateScope();
var ApplicationDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var robotdb = ApplicationDb.Robots.FirstOrDefault(robot => robot.RobotId == robotId);
if (robotdb is null) return null;
var robotModel = ApplicationDb.RobotModels.FirstOrDefault(model => model.Id == robotdb.ModelId);
if (robotModel is null) return null;
var robotController = new RobotController(robotId, VDA5050Setting.Manufacturer, VDA5050Setting.Version, robotModel.NavigationType, ServiceProvider, PublishMQTT);
RobotControllers.Add(robotId, robotController);
return robotController;
}
public void AddRobotSimulation(IEnumerable<string> robotIds)
{
using var scope = ServiceProvider.CreateScope();
var ApplicationDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var Traffic = scope.ServiceProvider.GetRequiredService<TrafficManager>();
foreach (var robotId in robotIds)
{
if (RobotControllers.TryGetValue(robotId, out _)) continue;
var robotdb = ApplicationDb.Robots.FirstOrDefault(robot => robot.RobotId == robotId);
if (robotdb is null) return;
var robotModel = ApplicationDb.RobotModels.FirstOrDefault(model => model.Id == robotdb.ModelId);
if (robotModel is null) return;
var robotController = new RobotSimulation(robotId, robotModel, ServiceProvider);
RobotControllers.Add(robotId, robotController);
}
}
public bool DeleteRobot(string robotId)
{
try
{
if (RobotControllers.TryGetValue(robotId, out var robotcontroller))
{
RobotControllers.Remove(robotId);
}
return true;
}
catch (Exception ex)
{
Logger.Warning($"Hệ thống xảy ra lỗi khi xóa bỏ robot: {ex.Message}");
return false;
}
}
public IEnumerable<RobotInfomationDto> GetRobotInfo(Guid mapId)
{
try
{
using var scope = ServiceProvider.CreateScope();
var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
List<RobotInfomationDto> RobotInfos = [];
foreach (var robotcontroller in RobotControllers.Values.ToList())
{
var robotDb = RobotDb.Robots.FirstOrDefault(r => robotcontroller.SerialNumber == r.RobotId);
if (robotDb is null || robotDb.MapId != mapId) continue;
RobotInfos.Add(new()
{
RobotId = robotcontroller.SerialNumber,
Name = robotDb?.Name,
MapId = robotDb is null ? Guid.Empty : robotDb.MapId,
Battery = new()
{
BatteryHealth = robotcontroller.StateMsg.BatteryState.BatteryHealth,
BatteryCharge = robotcontroller.StateMsg.BatteryState.BatteryCharge,
BatteryVoltage = robotcontroller.StateMsg.BatteryState.BatteryVoltage,
Charging = robotcontroller.StateMsg.BatteryState.Charging,
},
Errors = [],
Infomations = [],
Navigation = new()
{
NavigationState = robotcontroller.State,
RobotPath = robotcontroller.FullPath,
RobotBasePath = robotcontroller.BasePath,
},
AgvPosition = new()
{
X = robotcontroller.VisualizationMsg.AgvPosition.X,
Y = robotcontroller.VisualizationMsg.AgvPosition.Y,
Theta = robotcontroller.VisualizationMsg.AgvPosition.Theta,
PositionInitialized = robotcontroller.VisualizationMsg.AgvPosition.PositionInitialized,
LocalizationScore = robotcontroller.VisualizationMsg.AgvPosition.LocalizationScore,
DeviationRange = robotcontroller.VisualizationMsg.AgvPosition.DeviationRange,
},
AgvVelocity = new()
{
Vx = robotcontroller.VisualizationMsg.Velocity.Vx,
Vy = robotcontroller.VisualizationMsg.Velocity.Vy,
Omega = robotcontroller.VisualizationMsg.Velocity.Omega,
},
Loads = robotcontroller.StateMsg.Loads,
});
}
return RobotInfos;
}
catch (Exception ex)
{
Logger.Warning($"Get Robot Info Error: - {ex}");
return [];
}
}
public RobotACSLockedDto[] GetRobotACSLocked()
{
try
{
List<RobotACSLockedDto> robotACSLockedDtos = [];
foreach(var robot in RobotControllers.Values)
{
robotACSLockedDtos.Add(new()
{
RobotId = robot.SerialNumber,
ZoneIds = robot.CurrentZones,
});
}
return [..robotACSLockedDtos];
}
catch (Exception ex)
{
Logger.Warning($"Get Robot ACS Locked Error: - {ex}");
return [];
}
}
private static Script.Expressions.RobotState ConvertIRobotControllerToRobotState(IRobotController robotController)
{
bool isReady = robotController.IsOnline && !robotController.OrderState.IsProcessing && robotController.StateMsg.Errors.Length == 0;
if (robotController.ActionStates.Length > 0)
{
isReady = isReady && robotController.ActionStates.All(a => !a.IsProcessing);
}
return new RobotState(isReady,
robotController.StateMsg.BatteryState.BatteryVoltage,
robotController.StateMsg.Loads.Length != 0,
robotController.StateMsg.BatteryState.Charging,
robotController.StateMsg.AgvPosition.X,
robotController.StateMsg.AgvPosition.Y,
robotController.StateMsg.AgvPosition.Theta);
}
/// <summary>
/// Tìm kiếm robot theo tên model, tên bản đồ
/// </summary>
/// <param name="robotModelName"></param>
/// <param name="mapName"></param>
/// <returns></returns>
public async Task<MessageResult<string[]>> SearchRobot(string? robotModelName, string? mapName, Func<Script.Expressions.RobotState, bool> funcSearch)
{
List<string> robotIds = [];
try
{
using var scope = ServiceProvider.CreateScope();
var RobotDbContext = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var MapManager = scope.ServiceProvider.GetRequiredService<MapManager>();
var robotModel = RobotDbContext.RobotModels.FirstOrDefault(m => m.ModelName == robotModelName);
if (robotModel is null) return new(false, $"Robot model name {robotModelName} không tồn tại");
var map = await MapManager.GetMapInfo(mapName ?? "");
if (!map.IsSuccess && !string.IsNullOrEmpty(mapName)) return new(false, map.Message);
if (map is null || map.Data is null) return new(false, $"Map name {mapName} không tồn tại");
foreach (var robot in RobotControllers.Values)
{
if (!robot.IsOnline) continue;
var robotDb = RobotDbContext.Robots.FirstOrDefault(r => r.RobotId == robot.SerialNumber);
if (robotDb is null) continue;
var robotDbModel = RobotDbContext.RobotModels.FirstOrDefault(m => m.Id == robotDb.ModelId);
if (robotDbModel is null) continue;
var robotDbMap = await MapManager.GetMapInfo(robotDb.MapId);
if (robotDbMap is null || !robotDbMap.IsSuccess) continue;
if (robotDbMap.Data is null) continue;
bool isRobotModelMatch = string.IsNullOrEmpty(robotModelName) || robotDbModel.Id == robotModel.Id;
bool isMapMatch = string.IsNullOrEmpty(mapName) || (map is not null && robotDbMap.Data.Id == map.Data.Id);
bool isExpressionMatch = funcSearch(ConvertIRobotControllerToRobotState(robot));
if (isRobotModelMatch && isMapMatch && isExpressionMatch) robotIds.Add(robot.SerialNumber);
}
return new(true, robotIds.Count > 0 ? "Tìm thấy robot" : "Không tìm thấy robot nào thỏa mãn điều kiện")
{
Data = [.. robotIds],
};
}
catch (Exception ex)
{
Logger.Warning($"Find Robot Error: {ex}");
return new(false, "Hệ thống có lỗi xảy ra");
}
}
public RobotVDA5050StateDto? GetRobotVDA5050State(string robotId)
{
try
{
using var scope = ServiceProvider.CreateScope();
var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
if (RobotControllers.TryGetValue(robotId, out IRobotController? robotController) && robotController is not null)
{
var robot = robotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
if (robot is null) return null;
return new()
{
RobotId = robotId,
Name = robot.Name,
MapId = robot.MapId,
Online = robotController.IsOnline,
State = robotController.StateMsg,
OrderState = robotController.OrderState,
IsWorking = robotController.IsWorking,
Visualization = robotController.VisualizationMsg,
};
}
return null;
}
catch (Exception ex)
{
Logger.Warning($"Get VDA State Error: {robotId} - {ex}");
return null;
}
}
public IEnumerable<RobotOnlineStateDto> GetRobotOnlineState()
{
try
{
using var scope = ServiceProvider.CreateScope();
var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var robots = robotDb.Robots.ToList();
return [.. robots.Select(robot =>
{
var robotOnline = RobotControllers.TryGetValue(robot.RobotId, out IRobotController? robotController);
return new RobotOnlineStateDto()
{
RobotId = robot.RobotId,
State = !robotOnline || robotController is null ? "OFFLINE" : robotController.State,
IsOnline = robotOnline && robotController is not null && robotController.IsOnline,
Battery = !robotOnline || robotController is null ? 0 : robotController.StateMsg.BatteryState.BatteryHealth,
};
})];
}
catch (Exception ex)
{
Logger.Warning($"Get Robot Online State Error: {ex}");
return [];
}
}
private bool PublishMQTT(string topic, string data)
{
var repeat = VDA5050Setting.Repeat;
while (repeat-- > 0)
{
try
{
var applicationMessage = new MqttApplicationMessageBuilder()
.WithTopic(topic)
.WithPayload(data)
.WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
.Build();
if (MQTTClient is null) return false;
var publish = MQTTClient.PublishAsync(applicationMessage, CancellationToken.None);
publish.Wait();
if (!publish.Result.IsSuccess) continue;
return publish.Result.IsSuccess;
}
catch
{
return false;
}
}
return false;
}
private void ConnectionChanged(string data)
{
try
{
var msg = JsonSerializer.Deserialize<ConnectionMsg>(data);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
IRobotController robotController;
if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
{
value.IsOnline = msg.ConnectionState == ConnectionState.ONLINE.ToString();
robotController = value;
}
else
{
var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
if (addRobtoControllerTask is null) return;
robotController = addRobtoControllerTask;
RobotControllers[msg.SerialNumber].IsOnline = msg.ConnectionState == ConnectionState.ONLINE.ToString();
}
robotController.RobotUpdated.Set();
}
catch (Exception ex)
{
Logger.Warning($"Robot Manager ConnectionChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
}
}
private void StateChanged(string data)
{
try
{
var msg = JsonSerializer.Deserialize<StateMsg>(data, JsonOptionExtends.Write);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
if (msg.AgvPosition is null) return;
msg.AgvPosition.Theta = msg.AgvPosition.Theta * 180 / Math.PI;
IRobotController robotController;
if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
{
value.StateMsg = msg;
robotController = value;
}
else
{
var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
if (addRobtoControllerTask is null) return;
robotController = addRobtoControllerTask;
RobotControllers[msg.SerialNumber].StateMsg = msg;
}
robotController.IsOnline = true;
robotController.RobotUpdated.Set();
}
catch (Exception ex)
{
Logger.Warning($"Robot Manager StateChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
}
}
private void VisualizationChanged(string data)
{
try
{
var msg = JsonSerializer.Deserialize<VisualizationMsg>(data, JsonOptionExtends.Write);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
if (msg.AgvPosition is null) return;
msg.AgvPosition.Theta = msg.AgvPosition.Theta * 180 / Math.PI;
IRobotController robotController;
if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
{
value.VisualizationMsg = msg;
robotController = value;
}
else
{
var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
if (addRobtoControllerTask is null) return;
robotController = addRobtoControllerTask;
RobotControllers[msg.SerialNumber].VisualizationMsg = msg;
}
robotController.IsOnline = true;
robotController.RobotUpdated.Set();
}
catch (Exception ex)
{
Logger.Warning($"Robot Manager VisualizationChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
}
}
private void FactsheetChanged(string data)
{
try
{
var msg = JsonSerializer.Deserialize<FactSheetMsg>(data, JsonOptionExtends.Write);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
IRobotController robotController;
if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
{
value.FactSheetMsg = msg;
robotController = value;
}
else
{
var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
if (addRobtoControllerTask is null) return;
robotController = addRobtoControllerTask;
RobotControllers[msg.SerialNumber].FactSheetMsg = msg;
}
robotController.IsOnline = true;
robotController.RobotUpdated.Set();
}
catch (Exception ex)
{
Logger.Warning($"Robot Manager FactsheetChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
}
}
private void FactsheetExtendChanged(string data)
{
try
{
var msg = JsonSerializer.Deserialize<FactsheetExtendMsg>(data, JsonOptionExtends.Write);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
IRobotController robotController;
if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
{
value.FactsheetExtendMsg = msg;
robotController = value;
}
else
{
var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
if (addRobtoControllerTask is null) return;
robotController = addRobtoControllerTask;
RobotControllers[msg.SerialNumber].FactsheetExtendMsg = msg;
}
robotController.IsOnline = true;
robotController.RobotUpdated.Set();
}
catch (Exception ex)
{
Logger.Warning($"Robot Manager FactsheetExtendChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
}
}
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
await Task.Yield();
while (!stoppingToken.IsCancellationRequested)
{
try
{
var mqttFactory = new MqttClientFactory();
MQTTClient = mqttFactory.CreateMqttClient();
var mqttClientOptions = new MqttClientOptionsBuilder()
.WithTcpServer(VDA5050Setting.HostServer, VDA5050Setting.Port)
.WithClientId("FleetManager")
.WithCredentials(VDA5050Setting.UserName, VDA5050Setting.Password)
.Build();
if ((await MQTTClient.ConnectAsync(mqttClientOptions, stoppingToken)).ResultCode != MqttClientConnectResultCode.Success)
{
await Task.Delay(1000, stoppingToken);
continue;
}
Logger.Info("Fleet Manager kết nối tới broker thành công");
var mqttSubscribeOptions = mqttFactory.CreateSubscribeOptionsBuilder()
.WithTopicFilter(f =>
{
f.WithTopic(VDA5050Topic.Connection);
})
.WithTopicFilter(f =>
{
f.WithTopic(VDA5050Topic.Visualization);
})
.WithTopicFilter(f =>
{
f.WithTopic(VDA5050Topic.State);
})
.WithTopicFilter(f =>
{
f.WithTopic(VDA5050Topic.Factsheet);
})
.WithTopicFilter(f =>
{
f.WithTopic(VDA5050Topic.FactsheetExtend);
})
.Build();
var response = await MQTTClient.SubscribeAsync(mqttSubscribeOptions, stoppingToken);
Logger.Info("Fleet Manager Subscribe thành công");
MQTTClient.ApplicationMessageReceivedAsync += delegate (MqttApplicationMessageReceivedEventArgs args)
{
var stringData = Encoding.Default.GetString(args.ApplicationMessage.Payload);
switch (args.ApplicationMessage.Topic)
{
case VDA5050Topic.Connection: ConnectionChanged(stringData); break;
case VDA5050Topic.Visualization: VisualizationChanged(stringData); break;
case VDA5050Topic.State: StateChanged(stringData); break;
case VDA5050Topic.Factsheet: FactsheetChanged(stringData); break;
case VDA5050Topic.FactsheetExtend: FactsheetExtendChanged(stringData); break;
default: break;
}
return Task.CompletedTask;
};
Logger.Info("Fleet Manager Subscribe event thành công");
break;
}
catch (Exception ex)
{
Logger.Warning($"Kết nối tới broker xảy ra lỗi: {ex.Message}");
await Task.Delay(3000, stoppingToken);
}
}
Logger.Info("Fleet Manager bắt đầu kiểm tra kết nối tới robot");
while (!stoppingToken.IsCancellationRequested)
{
try
{
foreach (var robotController in RobotControllers)
{
if (!robotController.Value.RobotUpdated.WaitOne(TimeSpan.FromSeconds(1)) && robotController.Value.IsOnline)
{
robotController.Value.Dispose();
continue;
}
robotController.Value.RobotUpdated.Reset();
}
await Task.Delay(TimeSpan.FromSeconds(VDA5050Setting.CheckingRobotMsgTimout), stoppingToken);
}
catch (Exception ex)
{
Logger.Warning($"Kiểm tra kết nối tới robot xảy ra lỗi: {ex.Message}");
await Task.Delay(3000, stoppingToken);
}
}
}
}