Merge remote-tracking branch 'origin/sonlt' into dangnv

RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor

@@ -25,9 +25,9 @@
                 <span>X: @MonitorData.RobotPosition.X.ToString("F2")m | Y: @MonitorData.RobotPosition.Y.ToString("F2")m | θ: @((MonitorData.RobotPosition.Theta * 180 / Math.PI).ToString("F1"))°</span>
             </div>
         }
-        <MudChip T="string" Color="@(IsConnected? Color.Success: Color.Error)" Size="Size.Small">
+       @*  <MudChip T="string" Color="@(IsConnected? Color.Success: Color.Error)" Size="Size.Small">
             @(IsConnected ? "Connected" : "Disconnected")
-        </MudChip>
+        </MudChip> *@
     </div>
     <div @ref="SvgContainerRef" class="svg-container">
         <svg @ref="SvgRef"

RobotApp.Client/Pages/Dashboard.razor

@@ -38,15 +38,18 @@
         @if (CurrentState != null)
         {
             <MudChip T="string"
-                     Icon="@(IsConnected ? Icons.Material.Filled.CheckCircle : Icons.Material.Filled.Error)"
+                     Icon="@(IsConnected
+                                      ? Icons.Material.Filled.CheckCircle
+                                  : Icons.Material.Filled.Error)"
                  Size="Size.Large"
                  Color="@(IsConnected ? Color.Success : Color.Error)"
-                     Variant="@Variant.Filled"
+                 Variant="Variant.Filled"
                  Class="px-6 py-4 text-white"
                  Style="font-weight: bold; font-size: 1.1rem;">
             @(IsConnected ? "ONLINE" : "OFFLINE")
         </MudChip>
                 }
+
     </MudPaper>
 
     @{
@@ -133,7 +136,6 @@
             </MudCard>
         </MudItem>
 
-        <!-- BATTERY -->
         <!-- BATTERY -->
         <MudItem xs="12" md="6" lg="4">
             <MudCard Elevation="6" Class="h-100 rounded-lg">
@@ -371,21 +373,35 @@
     };
 
     private StateMsg? CurrentState;
-    private bool IsConnected => RobotStateClient.LatestStates.ContainsKey(RobotSerial);
+    private bool IsConnected;
     private readonly string RobotSerial = "T800-002";
     private List<MessageRow> MessageRows = new();
 
     protected override async Task OnInitializedAsync()
     {
         RobotStateClient.OnStateReceived += OnRobotStateReceived;
-        if (RobotStateClient.LatestStates.Count == 0)
+        RobotStateClient.OnRobotConnectionChanged += OnRobotConnectionChanged;
+
+        if (RobotStateClient.ConnectionState == RobotClientState.Disconnected)
         {
             await RobotStateClient.StartAsync();
         }
+
         await RobotStateClient.SubscribeRobotAsync(RobotSerial);
+
         CurrentState = RobotStateClient.GetLatestState(RobotSerial);
+        IsConnected = RobotStateClient.IsRobotConnected;
+
         UpdateMessageRows();
     }
+    private void OnRobotConnectionChanged(bool connected)
+    {
+        InvokeAsync(() =>
+        {
+            IsConnected = connected;
+            StateHasChanged();
+        });
+    }
 
 
     private void OnRobotStateReceived(string serialNumber, StateMsg state)
@@ -421,6 +437,7 @@
     public void Dispose()
     {
         RobotStateClient.OnStateReceived -= OnRobotStateReceived;
+        RobotStateClient.OnRobotConnectionChanged -= OnRobotConnectionChanged;
     }
 
     private record MessageRow(string Type, string Level, string Description, bool IsError);

RobotApp.Client/Pages/Order/ImportOrderDialog.razor

@@ -1,11 +1,21 @@
 @using System.Text.Json
 @using MudBlazor
+@using Microsoft.AspNetCore.Components.Forms
 
 <MudDialog>
+
+    <!-- ================= TITLE ================= -->
     <TitleContent>
-        <MudText Typo="Typo.h6">Import Order JSON</MudText>
+        <MudStack Row AlignItems="AlignItems.Center" Spacing="2">
+            <MudIcon Icon="@Icons.Material.Filled.UploadFile"
+                     Color="Color.Primary" />
+            <MudText Typo="Typo.h6">
+                Import Order JSON
+            </MudText>
+        </MudStack>
     </TitleContent>
 
+    <!-- ================= CONTENT ================= -->
     <DialogContent>
 
         @if (ShowWarning)
@@ -18,6 +28,21 @@
             </MudAlert>
         }
 
+        <!-- ===== FILE INPUT (PURE BLAZOR) ===== -->
+        <div class="mb-4">
+            <label class="mud-button-root mud-button mud-button-outlined mud-button-outlined-primary"
+                   style="cursor:pointer;">
+                <MudIcon Icon="@Icons.Material.Filled.AttachFile" Class="mr-2" />
+                Choose JSON file
+                <InputFile OnChange="OnFileSelected"
+                           accept=".json"
+                           style="display:none" />
+            </label>
+        </div>
+
+        <MudDivider Class="my-3" />
+
+        <!-- ===== PASTE JSON ===== -->
         <MudTextField @bind-Value="JsonText"
                       Label="Paste Order JSON"
                       Lines="20"
@@ -34,29 +59,77 @@
 
     </DialogContent>
 
+    <!-- ================= ACTIONS ================= -->
     <DialogActions>
-        <MudButton OnClick="Cancel">Cancel</MudButton>
+        <MudButton OnClick="Cancel">
+            Cancel
+        </MudButton>
+
         <MudButton Variant="Variant.Filled"
                    Color="Color.Primary"
                    OnClick="ValidateAndImport">
             Import
         </MudButton>
     </DialogActions>
+
 </MudDialog>
+
 @code {
-    [CascadingParameter] public IMudDialogInstance MudDialog { get; set; } = default!;
+    [CascadingParameter]
+    public IMudDialogInstance MudDialog { get; set; } = default!;
 
     public string JsonText { get; set; } = "";
     public string? ErrorMessage;
-    public bool ShowWarning { get; set; } = false;
+    public bool ShowWarning { get; set; }
 
     private void Cancel() => MudDialog.Cancel();
 
+    // ================= FILE HANDLER =================
+    private async Task OnFileSelected(InputFileChangeEventArgs e)
+    {
+        ErrorMessage = null;
+        ShowWarning = false;
+
+        var file = e.File;
+        if (file == null)
+            return;
+
+        if (!file.Name.EndsWith(".json", StringComparison.OrdinalIgnoreCase) &&
+            !file.Name.EndsWith(".txt", StringComparison.OrdinalIgnoreCase))
+        {
+            ShowWarning = true;
+            ErrorMessage = "Only .json or .txt files are supported.";
+            return;
+        }
+
+        try
+        {
+            using var stream = file.OpenReadStream(maxAllowedSize: 1_048_576);
+            using var reader = new StreamReader(stream);
+
+            JsonText = await reader.ReadToEndAsync();
+            StateHasChanged();
+        }
+        catch (Exception ex)
+        {
+            ShowWarning = true;
+            ErrorMessage = $"Failed to read file: {ex.Message}";
+        }
+    }
+
+    // ================= VALIDATE & IMPORT =================
     private void ValidateAndImport()
     {
         ErrorMessage = null;
         ShowWarning = false;
 
+        if (string.IsNullOrWhiteSpace(JsonText))
+        {
+            ShowWarning = true;
+            ErrorMessage = "JSON content is empty.";
+            return;
+        }
+
         try
         {
             using var doc = JsonDocument.Parse(JsonText);
@@ -65,10 +138,11 @@
             // ===== BASIC STRUCTURE CHECK =====
             if (!root.TryGetProperty("nodes", out _) ||
                 !root.TryGetProperty("edges", out _))
+            {
                 throw new Exception("Missing 'nodes' or 'edges' field.");
+            }
 
             var order = OrderMessage.FromSchemaObject(root);
-
             ValidateOrder(order);
 
             MudDialog.Close(DialogResult.Ok(order));
@@ -85,19 +159,40 @@
         }
     }
 
+    // ================= DOMAIN VALIDATION =================
     private void ValidateOrder(OrderMessage order)
     {
         if (order.Nodes.Count == 0)
             throw new Exception("Order must contain at least one node.");
 
-        var nodeIds = order.Nodes.Select(n => n.NodeId).ToHashSet();
+        if (order.Nodes.Count != order.Edges.Count + 1)
+            throw new Exception(
+                $"Invalid path structure: Nodes count ({order.Nodes.Count}) " +
+                $"must equal Edges count + 1 ({order.Edges.Count + 1})."
+            );
+
+        var nodeIds = order.Nodes
+            .Select(n => n.NodeId)
+            .ToHashSet(StringComparer.Ordinal);
 
         foreach (var e in order.Edges)
         {
-            if (!nodeIds.Contains(e.StartNodeId) ||
+            if (string.IsNullOrWhiteSpace(e.StartNodeId) ||
-                !nodeIds.Contains(e.EndNodeId))
+                string.IsNullOrWhiteSpace(e.EndNodeId))
+            {
                 throw new Exception(
-                    $"Edge '{e.EdgeId}' references unknown node."
+                    $"Edge '{e.EdgeId}' must define both StartNodeId and EndNodeId."
+                );
+            }
+
+            if (!nodeIds.Contains(e.StartNodeId))
+                throw new Exception(
+                    $"Edge '{e.EdgeId}' references unknown StartNodeId '{e.StartNodeId}'."
+                );
+
+            if (!nodeIds.Contains(e.EndNodeId))
+                throw new Exception(
+                    $"Edge '{e.EdgeId}' references unknown EndNodeId '{e.EndNodeId}'."
                 );
         }
     }

RobotApp.Client/Pages/Order/JsonOutputPanel.razor

@@ -1,4 +1,4 @@
-<MudPaper Class="pa-4 h-100 d-flex flex-column" Elevation="2">
+<MudPaper Class="pa-4 h-100 d-flex flex-column overflow-hidden" Elevation="2">
     <MudStack Row AlignItems="AlignItems.Center" Justify="Justify.SpaceBetween"
               Class="mb-4 flex-shrink-0">
         <MudText Typo="Typo.h6">📄 JSON Output (/order)</MudText>
@@ -27,7 +27,6 @@
             <MudTooltip Text="@(Copied ? "Copied!" : "Copy to clipboard")">
                 <MudButton Variant="Variant.Filled"
                            Color="@(Copied ? Color.Success : Color.Primary)"
-                           Size="Size.Small"
                            StartIcon="@(Copied ? Icons.Material.Filled.Check : Icons.Material.Filled.ContentCopy)"
                            OnClick="OnCopy">
                     @(Copied ? "Copied!" : "Copy")
@@ -36,12 +35,11 @@
         </div>
     </MudStack>
 
-    <div class="flex-grow-1" style="overflow:auto;">
+    <div class="flex-grow-1">
         <MudTextField Value="@OrderJson"
                       ReadOnly
                       Variant="Variant.Filled"
-                      Lines="70"
+                      Lines="50"
-                      Class="h-100"
                       Style="font-family: 'Roboto Mono', Consolas, monospace;
                              font-size: 0.85rem;
                              background:#1e1e1e;

RobotApp.Client/Pages/Order/NodesPanel.razor

@@ -44,21 +44,22 @@
                             </MudItem>
 
                             <!-- Sequence -->
-                            <MudItem xs="12">
+                            @* <MudItem xs="12">
                                 <MudNumericField T="int"
                                                  Value="@node.SequenceId"
                                                  ValueChanged="@((int v) => SetValue(() => node.SequenceId = v))"
                                                  Immediate="true"
                                                  Label="Sequence ID" />
-                            </MudItem>
+                            </MudItem> *@
 
                             <!-- Released -->
-                            <MudItem xs="12">
+                            @* <MudItem xs="12">
                                 <MudSwitch T="bool"
                                            Checked="@node.Released"
                                            CheckedChanged="@((bool v) => SetValue(() => node.Released = v))"
                                            Label="Released" />
-                            </MudItem>
+
+                            </MudItem> *@
 
                             <!-- Position -->
                             <MudItem xs="6">

RobotApp.Client/Services/RobotStateClient.cs

@@ -7,7 +7,7 @@ using System.Text.Json;
 
 namespace RobotApp.Client.Services;
 
-// ================= CONNECTION STATE =================
+// ================= SIGNALR CONNECTION STATE =================
 public enum RobotClientState
 {
     Disconnected,
@@ -16,7 +16,7 @@ public enum RobotClientState
     Reconnecting
 }
 
-// ================= CLIENT =================
+// ================= ROBOT STATE CLIENT =================
 public sealed class RobotStateClient : IAsyncDisposable
 {
     private readonly NavigationManager _nav;
@@ -25,11 +25,17 @@ public sealed class RobotStateClient : IAsyncDisposable
     private readonly object _lock = new();
     private bool _started;
 
+    // ================= STATE CACHE =================
     public ConcurrentDictionary<string, StateMsg> LatestStates { get; } = new();
 
+    // ================= ROBOT CONNECTION =================
+    private bool _isRobotConnected;
+    public bool IsRobotConnected => _isRobotConnected;
+
     // ================= EVENTS =================
     public event Action<string, StateMsg>? OnStateReceived;
     public event Action<StateMsg>? OnStateReceivedAny;
+    public event Action<bool>? OnRobotConnectionChanged;
     public event Action<RobotClientState>? OnConnectionStateChanged;
 
     public RobotClientState ConnectionState { get; private set; } = RobotClientState.Disconnected;
@@ -43,7 +49,8 @@ public sealed class RobotStateClient : IAsyncDisposable
     // ================= STATE HELPER =================
     private void SetState(RobotClientState state)
     {
-        if (ConnectionState == state) return;
+        if (ConnectionState == state)
+            return;
 
         ConnectionState = state;
         OnConnectionStateChanged?.Invoke(state);
@@ -82,9 +89,10 @@ public sealed class RobotStateClient : IAsyncDisposable
             return Task.CompletedTask;
         };
 
-        _connection.Closed += async error =>
+        _connection.Closed += async _ =>
         {
             _started = false;
+            SetState(RobotClientState.Disconnected);
 
             if (_connection != null)
             {
@@ -96,8 +104,14 @@ public sealed class RobotStateClient : IAsyncDisposable
             }
         };
 
+        // ================= SIGNALR HANDLERS =================
+
+        // VDA5050 State
         _connection.On<string>("ReceiveState", HandleState);
 
+        // Robot connection (bool only)
+        _connection.On<bool>("ReceiveRobotConnection", HandleRobotConnection);
+
         try
         {
             await _connection.StartAsync();
@@ -136,6 +150,13 @@ public sealed class RobotStateClient : IAsyncDisposable
         OnStateReceivedAny?.Invoke(state);
     }
 
+    // ================= HANDLE ROBOT CONNECTION =================
+    private void HandleRobotConnection(bool isConnected)
+    {
+        _isRobotConnected = isConnected;
+        OnRobotConnectionChanged?.Invoke(isConnected);
+    }
+
     // ================= SUBSCRIBE =================
     public async Task SubscribeRobotAsync(string serialNumber)
     {
@@ -164,6 +185,7 @@ public sealed class RobotStateClient : IAsyncDisposable
         }
 
         LatestStates.TryRemove(serialNumber, out _);
+        _isRobotConnected = false;
     }
 
     // ================= GET CACHE =================
@@ -177,6 +199,7 @@ public sealed class RobotStateClient : IAsyncDisposable
     public async ValueTask DisposeAsync()
     {
         _started = false;
+        _isRobotConnected = false;
         SetState(RobotClientState.Disconnected);
 
         if (_connection != null)

RobotApp.Client/Services/UiEdge.cs

@@ -270,31 +270,17 @@ public class OrderMessage
     }
     public object ToSchemaObject()
     {
-        int seq = 0;
+        // ================= SORT NODES BY UI SEQUENCE =================
+        var orderedNodes = Nodes
+            .OrderBy(n => n.SequenceId)
+            .ToList();
 
-        return new
+        // ================= BUILD NODE OBJECTS =================
-        {
+        var nodeObjects = orderedNodes
-            headerId = HeaderId++,
+            .Select((n, index) => new
-
-            timestamp = string.IsNullOrWhiteSpace(Timestamp)
-                ? DateTime.UtcNow.ToString("O")
-                : Timestamp,
-
-            version = Version,
-            manufacturer = Manufacturer,
-            serialNumber = SerialNumber,
-            orderId = OrderId,
-            orderUpdateId = OrderUpdateId,
-
-            zoneSetId = string.IsNullOrWhiteSpace(ZoneSetId)
-                ? null
-                : ZoneSetId,
-
-            // ================= NODES =================
-            nodes = Nodes.Select(n => new
             {
                 nodeId = n.NodeId,
-                sequenceId = seq++,
+                sequenceId = index * 2,               // ✅ NODE = EVEN
                 released = n.Released,
 
                 nodePosition = new
@@ -311,36 +297,55 @@ public class OrderMessage
                         : n.NodePosition.MapId
                 },
 
-                actions = n.Actions.Select(a => new
+                actions = n.Actions?
+                    .Select(a => new
                     {
                         actionId = a.ActionId,
                         actionType = a.ActionType,
                         blockingType = a.BlockingType,
 
-                    actionParameters = a.ActionParameters != null
+                        actionParameters = a.ActionParameters?
-                        ? a.ActionParameters
+                            .Select(p => new
-                            .Select(p => new { key = p.Key, value = p.Value })
-                            .ToArray()
-                        : Array.Empty<object>()
-                }).ToArray()
-            }).ToArray(),
-
-            // ================= EDGES =================
-            edges = Edges.Select<UiEdge, object>(e =>
                             {
+                                key = p.Key,
+                                value = p.Value
+                            })
+                            .ToArray()
+                            ?? Array.Empty<object>()
+                    })
+                    .ToArray()
+                    ?? Array.Empty<object>()
+            })
+            .ToArray();
+
+        // ================= BUILD EDGE OBJECTS =================
+        var edgeObjects = Edges
+            .Select<UiEdge, object>((e, index) =>
+            {
+                int sequenceId = index * 2 + 1;        // ✅ EDGE = ODD
+
+                // ---------- BASE ----------
+                var baseEdge = new
+                {
+                    edgeId = e.EdgeId,
+                    sequenceId,
+                    released = true,
+                    startNodeId = e.StartNodeId,
+                    endNodeId = e.EndNodeId
+                };
+
                 // =================================================
-                // 1️⃣ IMPORTED TRAJECTORY (ƯU TIÊN CAO NHẤT)
+                // 1️⃣ IMPORTED TRAJECTORY
                 // =================================================
                 if (e.HasTrajectory && e.Trajectory != null)
                 {
                     return new
                     {
-                        edgeId = e.EdgeId,
+                        baseEdge.edgeId,
-                        sequenceId = seq++,
+                        baseEdge.sequenceId,
-                        released = true,
+                        baseEdge.released,
-
+                        baseEdge.startNodeId,
-                        startNodeId = e.StartNodeId,
+                        baseEdge.endNodeId,
-                        endNodeId = e.EndNodeId,
 
                         trajectory = new
                         {
@@ -356,27 +361,25 @@ public class OrderMessage
                 }
 
                 // =================================================
-                // 2️⃣ STRAIGHT EDGE (KHÔNG CÓ CURVE)
+                // 2️⃣ STRAIGHT EDGE
                 // =================================================
                 if (e.Radius <= 0)
                 {
                     return new
                     {
-                        edgeId = e.EdgeId,
+                        baseEdge.edgeId,
-                        sequenceId = seq++,
+                        baseEdge.sequenceId,
-                        released = true,
+                        baseEdge.released,
-
+                        baseEdge.startNodeId,
-                        startNodeId = e.StartNodeId,
+                        baseEdge.endNodeId,
-                        endNodeId = e.EndNodeId,
-
                         actions = Array.Empty<object>()
                     };
                 }
 
                 // =================================================
-                // 3️⃣ EDITOR GENERATED CURVE (RADIUS + QUADRANT)
+                // 3️⃣ GENERATED CURVE (EDITOR)
                 // =================================================
-                var startNode = Nodes.First(n => n.NodeId == e.StartNodeId);
+                var startNode = orderedNodes.First(n => n.NodeId == e.StartNodeId);
 
                 var A = new Point(
                     startNode.NodePosition.X,
@@ -391,17 +394,40 @@ public class OrderMessage
 
                 return new
                 {
-                    edgeId = e.EdgeId,
+                    baseEdge.edgeId,
-                    sequenceId = seq++,
+                    baseEdge.sequenceId,
-                    released = true,
+                    baseEdge.released,
-
+                    baseEdge.startNodeId,
-                    startNodeId = e.StartNodeId,
+                    baseEdge.endNodeId,
-                    endNodeId = e.EndNodeId,
 
                     trajectory = result.Trajectory,
                     actions = Array.Empty<object>()
                 };
-            }).ToArray()
+            })
+            .ToArray();
+
+        // ================= FINAL SCHEMA OBJECT =================
+        return new
+        {
+            headerId = HeaderId++,
+
+            timestamp = string.IsNullOrWhiteSpace(Timestamp)
+                ? DateTime.UtcNow.ToString("O")
+                : Timestamp,
+
+            version = Version,
+            manufacturer = Manufacturer,
+            serialNumber = SerialNumber,
+
+            orderId = OrderId,
+            orderUpdateId = OrderUpdateId,
+
+            zoneSetId = string.IsNullOrWhiteSpace(ZoneSetId)
+                ? null
+                : ZoneSetId,
+
+            nodes = nodeObjects,
+            edges = edgeObjects
         };
     }
 }

RobotApp/Services/Robot/RobotStatePublisher.cs

@@ -26,6 +26,8 @@ public class RobotStatePublisher : BackgroundService
     private readonly ILoad _loadManager;
     private readonly INavigation _navigationManager;
     private readonly RobotStateMachine _stateManager;
+    private readonly RobotConnection _robotConnection;
+    private bool? _lastRobotConnectionState;
 
     private uint _headerId = 0;
     private readonly PeriodicTimer _timer = new(TimeSpan.FromMilliseconds(1000)); // 1 giây/lần
@@ -42,7 +44,8 @@ public class RobotStatePublisher : BackgroundService
         IBattery batteryManager,
         ILoad loadManager,
         INavigation navigationManager,
-        RobotStateMachine stateManager)
+        RobotStateMachine stateManager,
+        RobotConnection robotConnection)
     {
         _hubContext = hubContext;
         _robotConfig = robotConfig;
@@ -56,6 +59,7 @@ public class RobotStatePublisher : BackgroundService
         _loadManager = loadManager;
         _navigationManager = navigationManager;
         _stateManager = stateManager;
+        _robotConnection = robotConnection;
     }
 
     private StateMsg GetStateMsg()
@@ -137,35 +141,47 @@ public class RobotStatePublisher : BackgroundService
 
     protected override async Task ExecuteAsync(CancellationToken stoppingToken)
     {
-        Console.WriteLine("[RobotStatePublisher] Started - Publishing state every 1 second via SignalR");
+        while (await _timer.WaitForNextTickAsync(stoppingToken))
-
-        while (await _timer.WaitForNextTickAsync(stoppingToken) && !stoppingToken.IsCancellationRequested)
         {
             try
             {
-                var state = GetStateMsg();
                 var serialNumber = _robotConfig.SerialNumber;
 
+                // ===== SEND STATE =====
+                var state = GetStateMsg();
                 var json = JsonSerializer.Serialize(state, JsonOptionExtends.Write);
 
-                // Push đến tất cả client đang theo dõi robot này
                 await _hubContext.Clients
                     .Group(serialNumber)
                     .SendAsync("ReceiveState", json, stoppingToken);
 
-                //Console.WriteLine($"[RobotStatePublisher] Published state for {serialNumber} | " +
+                // ===== SEND ROBOT CONNECTION (ONLY WHEN CHANGED) =====
-                //                  $"HeaderId: {state.HeaderId} | " +
+                var isConnected = _robotConnection.IsConnected;
-                //                  $"Pos: ({state.AgvPosition.X:F2}, {state.AgvPosition.Y:F2}) | " +
+
-                //                  $"Battery: {state.BatteryState.BatteryCharge:F1}%");
+                if (_lastRobotConnectionState != isConnected)
+                {
+                    _lastRobotConnectionState = isConnected;
+
+                    await _hubContext.Clients
+                        .Group(serialNumber)   // routing only
+                        .SendAsync(
+                            "ReceiveRobotConnection",
+                            isConnected,       // payload only bool
+                            stoppingToken
+                        );
+
+                    Console.WriteLine(
+                        $"[RobotStatePublisher] Robot connection changed → {(isConnected ? "ONLINE" : "OFFLINE")}"
+                    );
+                }
             }
             catch (Exception ex)
             {
-                Console.WriteLine($"[RobotStatePublisher] Error publishing state: {ex.Message}");
+                Console.WriteLine(ex);
+            }
         }
     }
 
-        Console.WriteLine("[RobotStatePublisher] Stopped.");
-    }
 
     public override void Dispose()
     {