DangNV/RobotApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Đăng Nguyễn 2025-10-17 09:24:45 +07:00
parent 90dcb67b60
commit 9ac5270885
12 changed files with 177 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
RobotApp/Interfaces/IError.cs
View File

@ -4,6 +4,7 @@ namespace RobotApp.Interfaces;
public interface IError
{
event Action? OnNewFatalError;
bool HasFatalError { get; }
void AddError(Error error, TimeSpan? clearAfter = null);
void DeleteErrorType(string errorType);

8
RobotApp/Interfaces/IInfomation.cs
View File

@ -1,6 +1,10 @@
namespace RobotApp.Interfaces;
using RobotApp.VDA5050.State;
namespace RobotApp.Interfaces;
public interface IInfomation
{
void AddInfo(Information infor);
void DeleteInfoType(string infoType);
void ClearAllInfos();
}

8
RobotApp/Interfaces/ILoad.cs Normal file
View File

@ -0,0 +1,8 @@
using RobotApp.VDA5050.State;
namespace RobotApp.Interfaces;
public interface ILoad
{
Load? Load { get; }
}

14
RobotApp/Services/Robot/RobotConnection.cs
View File

@ -6,20 +6,25 @@ using System.Text.Json;
namespace RobotApp.Services.Robot;
public class RobotConnection(RobotConfiguration RobotConfiguration, Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger)
public class RobotConnection(RobotConfiguration RobotConfiguration,
Logger<RobotConnection> Logger,
Logger<MQTTClient> MQTTClientLogger)
{
private readonly VDA5050Setting VDA5050Setting = RobotConfiguration.VDA5050Setting;
private MQTTClient? MqttClient;
public bool IsConnected => MqttClient is not null && MqttClient.IsConnected;
public readonly string SerialNumber = RobotConfiguration.SerialNumber;
public event Action<OrderMsg>? OrderUpdated;
public event Action<InstantActionsMsg>? ActionUpdated;
private void OrderChanged(string data)
{
try
{
var msg = JsonSerializer.Deserialize<OrderMsg>(data);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return;
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return;
OrderUpdated?.Invoke(msg);
}
catch (Exception ex)
{
@ -32,7 +37,8 @@ public class RobotConnection(RobotConfiguration RobotConfiguration, Logger<Robot
try
{
var msg = JsonSerializer.Deserialize<InstantActionsMsg>(data);
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return;
if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return;
ActionUpdated?.Invoke(msg);
}
catch (Exception ex)
{

18
RobotApp/Services/Robot/RobotController.cs
View File

@ -1,4 +1,5 @@
using RobotApp.Interfaces;
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
using RobotApp.Services.Exceptions;
using RobotApp.Services.State;
using RobotApp.VDA5050.InstantAction;
@ -12,9 +13,7 @@ public partial class RobotController(IOrder OrderManager,
IBattery BatteryManager,
ILocalization Localization,
IPeripheral PeripheralManager,
ISafety SafetyManager,
IError ErrorManager,
IInfomation InfomationManager,
Logger<RobotController> Logger,
RobotConnection ConnectionManager,
RobotStateMachine StateManager) : BackgroundService
@ -22,15 +21,9 @@ public partial class RobotController(IOrder OrderManager,
private readonly Mutex NewOrderMutex = new();
private readonly Mutex NewInstanceMutex = new();
private WatchTimer<RobotController>? UpdateStateTimer;
private const int UpdateStateInterval = 1000;
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
await InitializationingHandler(stoppingToken);
UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
UpdateStateTimer.Start();
}
public override async Task StopAsync(CancellationToken cancellationToken)
@ -39,18 +32,13 @@ public partial class RobotController(IOrder OrderManager,
await base.StopAsync(cancellationToken);
}
private void UpdateStateHandler()
{
// xử lý cập nhật trạng thái robot và gửi thông tin qua kết nối
}
public void NewOrderUpdated(OrderMsg order)
{
if (NewOrderMutex.WaitOne(2000))
{
try
{
if (PeripheralManager.PeripheralMode != PeripheralMode.AUTO) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode));
if (StateManager.RootStateName != RootStateType.Auto.ToString()) throw new OrderException(RobotErrors.Error1013(StateManager.RootStateName));
if (!string.IsNullOrEmpty(OrderManager.OrderId))
{
if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));

3
RobotApp/Services/Robot/RobotControllerInitialize.cs
View File

@ -34,6 +34,8 @@ public partial class RobotController
}
Logger.Info("Robot đã khởi tạo xong. Đang kết nối tới Fleet Manager.");
ConnectionManager.OrderUpdated += NewOrderUpdated;
ConnectionManager.ActionUpdated += NewInstanceActionUpdated;
await ConnectionManager.StartConnection(cancellationToken);
Logger.Info("Robot đã kết nối tới Fleet Manager.");
StateManager.TransitionTo(SystemStateType.Standby);
@ -54,7 +56,6 @@ public partial class RobotController
private void StopHandler()
{
UpdateStateTimer?.Stop();
PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged;
PeripheralManager.OnButtonPressed -= OnButtonPressed;
PeripheralManager.OnStop -= OnStop;

9
RobotApp/Services/Robot/RobotErrors.cs
View File

@ -6,14 +6,17 @@ namespace RobotApp.Services.Robot;
public class RobotErrors : IError
{
private readonly List<Error> Errors = [];
public bool HasFatalError => Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL.ToString());
public event Action? OnNewFatalError;
public void AddError(Error error, TimeSpan? clearAfter = null)
{
if (Errors.Any(e => e.ErrorType == error.ErrorType && e.ErrorHint == error.ErrorHint)) return;
lock (Errors)
{
Errors.Add(error);
if(error.ErrorLevel == ErrorLevel.FATAL.ToString()) OnNewFatalError?.Invoke();
}
if (clearAfter is not null && clearAfter.HasValue)
{
@ -89,8 +92,8 @@ public class RobotErrors : IError
=> CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edgeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}");
public static Error Error1012(NavigationState state)
=> CreateError(ErrorType.INITIALIZE_ORDER, "1012", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi hệ thống điều hướng không ở trạng thái sẵn sàng. Trạng thái hiện tại: {state}");
public static Error Error1013()
=> new();
public static Error Error1013(string rootState)
=> CreateError(ErrorType.INITIALIZE_ORDER, "1013", ErrorLevel.WARNING, $"Robot chưa sẵn sàng để nhận Order. Trạng thái hiện tại {rootState}");
public static Error Error1014(string edgeId, string nodeId)
=> CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge {edgeId} chứa StartNodeId {nodeId} không tồn tại trong Nodes");
public static Error Error1015(string edgeId, string nodeId)

29
RobotApp/Services/Robot/RobotInfomations.cs
View File

@ -1,8 +1,33 @@
using RobotApp.Interfaces;
using RobotApp.VDA5050.State;
namespace RobotApp.Services.Robot
namespace RobotApp.Services.Robot;
public class RobotInfomations : IInfomation
{
public class RobotInfomations : IInfomation
private readonly List<Information> Infors = [];
public void AddInfo(Information infor)
{
if (Infors.Any(e => e.InfoType == infor.InfoType)) return;
lock (Infors)
{
Infors.Add(infor);
}
}
public void DeleteInfoType(string infoType)
{
lock (Infors)
{
Infors.RemoveAll(e => e.InfoType == infoType);
}
}
public void ClearAllInfos()
{
lock (Infors)
{
Infors.Clear();
}
}
}

30
RobotApp/Services/Robot/RobotLoads.cs
View File

@ -1,5 +1,31 @@
namespace RobotApp.Services.Robot;
using RobotApp.Interfaces;
using RobotApp.VDA5050.State;
public class RobotLoads
namespace RobotApp.Services.Robot;
public class RobotLoads(IPeripheral PeriperalManager) : ILoad
{
public Load? Load => PeriperalManager.HasLoad ? GetLoad() : null;
private static Load GetLoad()
{
return new()
{
LoadId = Guid.NewGuid().ToString(),
LoadDimensions = new VDA5050.Factsheet.LoadDimensions
{
Length = 0.5,
Width = 0.5,
Height = 0.5
},
LoadPosition = "on_top",
LoadType = "box",
BoundingBoxReference = new VDA5050.Factsheet.BoundingBoxReference
{
X = 0,
Y = 0,
Z = 0,
},
Weight = 999
};
}
}

73
RobotApp/Services/Robot/RobotStates.cs
View File

@ -1,5 +1,74 @@
namespace RobotApp.Services.Robot;
using RobotApp.Common.Shares;
using RobotApp.Interfaces;
using RobotApp.VDA5050;
using RobotApp.VDA5050.State;
using System.Text.Json;
public class RobotStates
namespace RobotApp.Services.Robot;
public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection ConnectionManager,
Logger<RobotStates> Logger,
IOrder OrderManager,
IPeripheral PeripheralManager) : BackgroundService
{
private uint HeaderId = 0;
private readonly string SerialNumber = RobotConfiguration.SerialNumber;
private WatchTimerAsync<RobotStates>? UpdateStateTimer;
private const int UpdateStateInterval = 1000;
public async Task PubState()
{
try
{
await ConnectionManager.Publish(VDA5050Topic.STATE.ToTopicString(), JsonSerializer.Serialize(GetStateMsg(), JsonOptionExtends.Write));
}
catch { }
}
private StateMsg GetStateMsg()
{
return new StateMsg
{
HeaderId = HeaderId++,
SerialNumber = SerialNumber,
Maps = [],
OrderId = OrderManager.OrderId,
OrderUpdateId = OrderManager.OrderUpdateId,
ZoneSetId = "",
LastNodeId = OrderManager.LastNodeId,
LastNodeSequenceId = OrderManager.LastNodeSequenceId,
Driving = false,
Paused = false,
NewBaseRequest = false,
DistanceSinceLastNode = 0,
OperatingMode = PeripheralManager.PeripheralMode.ToString(),
};
}
private async Task UpdateStateHandler()
{
await PubState();
}
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
await Task.Yield();
while (!stoppingToken.IsCancellationRequested)
{
if (ConnectionManager.IsConnected) break;
await Task.Delay(1000);
}
if(!stoppingToken.IsCancellationRequested)
{
UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
UpdateStateTimer.Start();
}
}
public override Task StopAsync(CancellationToken cancellationToken)
{
UpdateStateTimer?.Dispose();
return base.StopAsync(cancellationToken);
}
}

7
RobotApp/Services/Robot/RobotVisualization.cs
View File

@ -7,11 +7,11 @@ namespace RobotApp.Services.Robot;
public class RobotVisualization(ILocalization Localization, INavigation Navigation, RobotConfiguration RobotConfiguration, RobotConnection RobotConnection, Logger<RobotVisualization> Logger) : BackgroundService
{
public string SerialNumber { get; set; } = RobotConfiguration.SerialNumber;
public string SerialNumber = RobotConfiguration.SerialNumber;
private uint HeaderId;
private WatchTimerAsync<RobotVisualization>? UpdateTimer;
private const int UpdateInterval = 50;
private const int UpdateInterval = 100;
private VisualizationMsg GetVisualizationMsg()
{
return new VisualizationMsg()
@ -46,7 +46,8 @@ public class RobotVisualization(ILocalization Localization, INavigation Navigati
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
while(!stoppingToken.IsCancellationRequested)
await Task.Yield();
while (!stoppingToken.IsCancellationRequested)
{
if (RobotConnection.IsConnected) break;
await Task.Delay(1000);

20
RobotApp/Services/State/RobotStateMachine.cs
View File

@ -1,5 +1,4 @@
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
namespace RobotApp.Services.State;
@ -11,6 +10,7 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
public event Action<IRobotState?, IRobotState>? OnStateChanged;
public bool IsInitialized = false;
public string CurrentStateName => CurrentState.Name.ToString();
public string RootStateName => GetRootStateName();
private readonly Lock StateLock = new();
@ -330,22 +330,22 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
return false;
}
public void Update()
public string GetRootStateName()
{
if (!IsInitialized)
if (CurrentState == null) return "Unknown";
var current = CurrentState;
while (current.SuperState != null)
{
Logger.Warning("State Machine chưa được khởi tạo");
return;
}
lock (StateLock)
{
CurrentState?.Update();
current = current.SuperState;
}
return current.Name.ToString();
}
public string GetCurrentStatePath()
{
if (CurrentState == null) return "No State";
if (CurrentState == null) return "Unknown";
return GetStatePath(CurrentState);
}