Initial commit

2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotVisualization.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotVisualization.cs
@@ -0,0 +1,67 @@
+using RobotNet.VDA5050.Visualization;
+using RobotNet10.Common;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.ConfigManager;
+using RobotNet10.RobotApp.Services.Robot.Connection;
+
+namespace RobotNet10.RobotApp.Services.Robot;
+
+public class RobotVisualization(ILocalization Localization,
+                                INavigation Navigation,
+                                IConnectionConfig ConnectionConfig,
+                                IRobotConnectionsService RobotConnectionsService,
+                                ILogger<RobotVisualization> Logger)
+{
+    private uint HeaderId;
+
+    private WatchThread<RobotVisualization>? UpdateTimer;
+    private const int UpdateInterval = 100;
+    private VisualizationMsg GetVisualizationMsg()
+    {
+        var vdaConfig = ConnectionConfig.GetVDA5050Config();
+        return new VisualizationMsg()
+        {
+            HeaderId = HeaderId++,
+            Manufacturer = vdaConfig.Manufacturer,
+            Version = vdaConfig.Version,
+            SerialNumber = vdaConfig.SerialNumber,
+            AgvPosition = new AgvPosition()
+            {
+                X = Localization.X,
+                Y = Localization.Y,
+                Theta = Localization.Theta
+            },
+            Velocity = new Velocity()
+            {
+                Vx = Navigation.VelocityX,
+                Vy = Navigation.VelocityY,
+                Omega = Navigation.Omega
+            }
+        };
+    }
+
+    private void UpdateHandler()
+    {
+        try
+        {
+            if (!RobotConnectionsService.IsConnected) return;
+            var publish = RobotConnectionsService.PublishVisualizationAsync(GetVisualizationMsg());
+            publish.ConfigureAwait(false);
+        }
+        catch { }
+    }
+
+    public void Start()
+    {
+        if (UpdateTimer is not null) Stop();
+
+        UpdateTimer = new(UpdateInterval, UpdateHandler, Logger, ThreadPriority.Normal);
+        UpdateTimer.Start();
+    }
+
+    public void Stop()
+    {
+        UpdateTimer?.Dispose();
+        UpdateTimer = null;
+    }
+}