Initial commit

2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotStates.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotStates.cs
@@ -0,0 +1,142 @@
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.State;
+using RobotNet.VDA5050.Type;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Motion;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Navigation;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+using RobotNet10.RobotApp.Services.State;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotStates(IConnectionConfig ConnectionConfig,
+                         IRobotConnectionsService RobotConnectionsService,
+                         RobotStateMachine StateManager,
+                         ILogger<RobotStates> Logger,
+                         IOrder OrderManager,
+                         IAction ActionManager,
+                         IPlcController PeripheralManager,
+                         IInfomation InfoManager,
+                         IError ErrorManager, 
+                         ILocalization LocalizationManager,
+                         IDeviceProvider DeviceProvider,
+                         ILoad LoadManager,
+                         INavigation NavigationManager,
+                         IVelocityController VelocityController) : IState
+{
+    private uint HeaderId = 0;
+
+    private WatchTimerAsync<RobotStates>? UpdateStateTimer;
+    private const int UpdateStateInterval = 1000;
+
+    public async Task PubState()
+    {
+        try
+        {
+            if (!RobotConnectionsService.IsConnected) return;
+            await RobotConnectionsService.PublishStateAsync(GetStateMsg());
+        }
+        catch { }
+    }
+
+    private StateMsg GetStateMsg()
+    {
+        var vdaConfig = ConnectionConfig.GetVDA5050Config();
+        var batteryDevice = DeviceProvider.GetDevice("battery-varta-001");
+        RobotNet10.Shared.Sensor.BatteryState batteryState = new();
+        if (batteryDevice is IBattery battery && battery.CurrentBatteryState.HasValue && battery.CurrentBatteryState is RobotNet10.Shared.Sensor.BatteryState state)
+        {
+            batteryState = state;
+        }
+        return new StateMsg
+        {
+            HeaderId = HeaderId++,
+            Manufacturer = vdaConfig.Manufacturer,
+            Version = vdaConfig.Version,
+            SerialNumber = vdaConfig.SerialNumber,
+            Maps = [],
+            OrderId = OrderManager.OrderId,
+            OrderUpdateId = OrderManager.OrderUpdateId,
+            ZoneSetId = LocalizationManager.CurrentActiveMap,
+            LastNodeId = OrderManager.LastNodeId,
+            LastNodeSequenceId = OrderManager.LastNodeSequenceId,
+            Driving = NavigationManager.Driving,
+            Paused = OrderManager.IsPaused,
+            NewBaseRequest = OrderManager.NewBaseRequest,
+            DistanceSinceLastNode = OrderManager.DistanceSinceLastNode,
+            OperatingMode = PeripheralManager.PeripheralMode.ToString(),
+            NodeStates = OrderManager.NodeStates,
+            EdgeStates = OrderManager.EdgeStates,
+            ActionStates = ActionManager.ActionStates,
+            Information = [General, .. InfoManager.InformationState],
+            Errors = ErrorManager.ErrorsState,
+            AgvPosition = new()
+            {
+                X = LocalizationManager.X,
+                Y = LocalizationManager.Y,
+                Theta = LocalizationManager.Theta,
+                LocalizationScore = LocalizationManager.LocalizationScore,
+                MapId = LocalizationManager.CurrentActiveMap,
+                DeviationRange = LocalizationManager.DeviationRange,
+                PositionInitialized = LocalizationManager.PositionInitialized,
+            },
+            BatteryState = new()
+            {
+                Charging = batteryState.Current > 0,
+                BatteryHealth = 100,
+                Reach = 0,
+                BatteryVoltage = batteryState.Voltage is double.NaN ? 0 : batteryState.Voltage,
+                BatteryCharge = batteryState.Percentage is double.NaN ? 0 : batteryState.Percentage,
+            },
+            Loads = LoadManager.Load,
+            Velocity = new()
+            {
+                Vx = VelocityController.ActualVelocity.Linear,
+                Vy = 0,
+                Omega = VelocityController.ActualVelocity.Angular,
+            },
+            SafetyState = new()
+            {
+                FieldViolation = !PeripheralManager.LidarBackProtectField || !PeripheralManager.LidarFrontProtectField || PeripheralManager.LidarFrontTimProtectField,
+                EStop = PeripheralManager.Emergency || PeripheralManager.Bumper ? EStop.AUTOACK : EStop.NONE,
+            }
+        };
+    }
+
+    private Information General => new()
+    {
+        InfoType = InformationType.GENERAL.ToJsonString(),
+        InfoDescription = "Thông tin chung của robot",
+        InfoLevel = InfoLevel.INFO,
+        InfoReferences =
+        [
+            new InfomationReference
+            {
+                ReferenceKey = InformationReferencesKey.STATE.ToJsonString(),
+                ReferenceValue = StateManager.CurrentState.ToString(),
+            },
+        ],
+    };
+
+    private async Task UpdateStateHandler()
+    {
+        await PubState();
+    }
+
+    public void Start()
+    {
+        if (UpdateStateTimer is not null) Stop();
+
+        UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
+        UpdateStateTimer.Start();
+    }
+
+    public void Stop()
+    {
+        UpdateStateTimer?.Dispose();
+        UpdateStateTimer = null;
+    }   
+}