merge dungtt

RobotApp.Common.Shares/Enums/NavigationType.cs

@@ -0,0 +1,7 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum NavigationType
+{
+    Differential,
+    Forklift,
+}

RobotApp.Common.Shares/Enums/RobotDirection.cs

@@ -0,0 +1,7 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum RobotDirection
+{
+    FORWARD,
+    BACKWARD,
+}

RobotApp.Common.Shares/Enums/StateType.cs

@@ -0,0 +1,39 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum RootStateType
+{
+    Booting,
+    Operational,
+}
+
+public enum OperationalStateType
+{
+
+}
+
+public enum AutomationStateType
+{
+    Idle,
+    Executing,
+    Paused,
+    Charging,
+    Error,
+    Remote_Override,
+}
+
+public enum ManualStateType
+{
+    Idle,
+    Active,
+}
+
+public enum SafetyStateType
+{
+    Init,
+    Run_Ok,
+    SS1,
+    STO,
+    PDS,
+    SLS,
+    Error,
+}

RobotApp.Common.Shares/Enums/TrajectoryDegree.cs

@@ -0,0 +1,8 @@
+namespace RobotApp.Common.Shares.Enums;
+
+public enum TrajectoryDegree
+{
+    One,
+    Two,
+    Three,
+}

RobotApp.Common.Shares/JsonOptionExtends.cs

@@ -0,0 +1,18 @@
+using System.Text.Json;
+
+namespace RobotApp.Common.Shares;
+
+public class JsonOptionExtends
+{
+    public static readonly JsonSerializerOptions Read = new()
+    {
+        PropertyNamingPolicy = JsonNamingPolicy.CamelCase,
+        WriteIndented = true,
+    };
+
+    public static readonly JsonSerializerOptions Write = new()
+    {
+        PropertyNamingPolicy = JsonNamingPolicy.CamelCase,
+        WriteIndented = true,
+    };
+}

RobotApp.Common.Shares/MathExtensions.cs

@@ -0,0 +1,80 @@
+using RobotApp.Common.Shares.Model;
+
+namespace RobotApp.Common.Shares;
+
+public static class MathExtensions
+{
+    public static (double x, double y) CurveDegreeTwo(double t, double x1, double y1, double controlPointX, double controlPointY, double x2, double y2)
+    {
+        var x = (1 - t) * (1 - t) * x1 + 2 * t * (1 - t) * controlPointX + t * t * x2;
+        var y = (1 - t) * (1 - t) * y1 + 2 * t * (1 - t) * controlPointY + t * t * y2;
+        return (x, y);
+    }
+
+    public static (double x, double y) CurveDegreeThree(double t, double x1, double y1, double controlPoint1X, double controlPoint1Y, double controlPoint2X, double controlPoint2Y, double x2, double y2)
+    {
+        var x = Math.Pow(1 - t, 3) * x1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2X + Math.Pow(t, 3) * x2; ;
+        var y = Math.Pow(1 - t, 3) * y1 + 3 * Math.Pow(1 - t, 2) * t * controlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * controlPoint2Y + Math.Pow(t, 3) * y2;
+        return (x, y);
+    }
+
+
+    public static (double x, double y) Curve(double t, EdgeCalculatorModel edge)
+    {
+        if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
+        {
+            return (edge.X1 + t * (edge.X2 - edge.X1), edge.Y1 + t * (edge.Y2 - edge.Y1));
+        }
+        else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
+        {
+            return CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+        }
+        else
+        {
+            return CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+        }
+    }
+
+    public static (double x, double y) Curve(this EdgeCalculatorModel edge, double t)
+    {
+        return Curve(t, edge);
+    }
+
+    public static double GetEdgeLength(this EdgeCalculatorModel edge)
+    {
+        double distance = 0;
+        if (edge.TrajectoryDegree == Enums.TrajectoryDegree.One)
+        {
+            distance = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
+        }
+        else if (edge.TrajectoryDegree == Enums.TrajectoryDegree.Two)
+        {
+            var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
+            if (length == 0) return 0;
+            double step = 0.1 / length;
+
+            for (double t = step; t <= 1.001; t += step)
+            {
+                (double x1, double y1) = CurveDegreeTwo(t - step, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+                (double x2, double y2) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+                distance += Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2));
+            }
+        }
+        else
+        {
+            var length = Math.Sqrt(Math.Pow(edge.X1 - edge.X2, 2) + Math.Pow(edge.Y1 - edge.Y2, 2));
+            if (length == 0) return 0;
+            double step = 0.1 / length;
+            for (double t = step; t <= 1.001; t += step)
+            {
+                var sTime = t - step;
+                (var sx, var sy) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+                sTime = t;
+                (var ex, var ey) = CurveDegreeThree(1 - sTime, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+
+                distance += Math.Sqrt(Math.Pow(sx - ex, 2) + Math.Pow(sy - ey, 2));
+            }
+        }
+        return distance;
+    }
+}

RobotApp.Common.Shares/MessageResult.cs

@@ -0,0 +1,8 @@
+namespace RobotApp.Common.Shares;
+
+public record MessageResult(bool IsSuccess, string Message = "");
+
+public record MessageResult<T>(bool IsSuccess, string Message = "")
+{
+    public T? Data { get; set; }
+}

RobotApp.Common.Shares/Model/EdgeCalculatorModel.cs

@@ -0,0 +1,16 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Common.Shares.Model;
+
+public class EdgeCalculatorModel
+{
+    public double X1 { get; set; }
+    public double Y1 { get; set; }
+    public double X2 { get; set; }
+    public double Y2 { get; set; }
+    public TrajectoryDegree TrajectoryDegree { get; set; }
+    public double ControlPoint1X { get; set; }
+    public double ControlPoint1Y { get; set; }
+    public double ControlPoint2X { get; set; }
+    public double ControlPoint2Y { get; set; }
+}

RobotApp.Common.Shares/RobotApp.Common.Shares.csproj

@@ -0,0 +1,9 @@
+<Project Sdk="Microsoft.NET.Sdk">
+
+  <PropertyGroup>
+    <TargetFramework>net9.0</TargetFramework>
+    <ImplicitUsings>enable</ImplicitUsings>
+    <Nullable>enable</Nullable>
+  </PropertyGroup>
+
+</Project>

RobotApp.VDA5050/Factsheet/ActionParameters.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
-
 public enum ValueDataType
 {
     BOOL,
@@ -17,9 +15,9 @@ public enum ValueDataType
 public class ActionParameters
 {
     [Required]
-    public string Key { get; set; }
+    public string Key { get; set; } = string.Empty;
     [Required]
-    public string ValueDataType { get; set; }
+    public string ValueDataType { get; set; } = string.Empty;
-    public string Description { get; set; }
+    public string Description { get; set; } = string.Empty;
     public bool IsOptional { get; set; }
 }

RobotApp.VDA5050/Factsheet/AgvActions.cs

@@ -2,7 +2,6 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
 
 public enum ActionScopes
 {
@@ -13,11 +12,11 @@ public enum ActionScopes
 public class AgvActions
 {
     [Required]
-    public string ActionType { get; set; }
+    public string ActionType { get; set; } = string.Empty;
-    public string ActionDescription { get; set; }
+    public string ActionDescription { get; set; } = string.Empty;
     [Required]
-    public string[] ActionScopes { get; set; }
+    public string[] ActionScopes { get; set; } = [];
-    public ActionParameters[] ActionParameters { get; set; }
+    public ActionParameters[] ActionParameters { get; set; } = [];
-    public string ResultDescription { get; set; }
+    public string ResultDescription { get; set; } = string.Empty;
-    public string[] BlockingTypes { get; set; } 
+    public string[] BlockingTypes { get; set; } = [];
 }

RobotApp.VDA5050/Factsheet/AgvGeometry.cs

@@ -2,11 +2,10 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable 
 
 public class AgvGeometry
 {
-    public WheelDefinitions[] WheelDefinitions { get; set; }
+    public WheelDefinitions[] WheelDefinitions { get; set; } = [];
-    public Envelopes2d[] Envelopes2d { get; set; }
+    public Envelopes2d[] Envelopes2d { get; set; } = [];
-    public Envelopes3d[] Envelopes3d { get; set; }
+    public Envelopes3d[] Envelopes3d { get; set; } = [];
 }

RobotApp.VDA5050/Factsheet/Envelopes2d.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
-
 public class PolygonPoints
 {
     [Required]
@@ -14,8 +12,8 @@ public class PolygonPoints
 public class Envelopes2d
 {
     [Required]
-    public string Set {  get; set; }
+    public string Set {  get; set; } = string.Empty;
     [Required]
-    public PolygonPoints[] PolygonPoints { get; set; }
+    public PolygonPoints[] PolygonPoints { get; set; } = [];
-    public string Description { get; set; }
+    public string Description { get; set; } = string.Empty;
 }

RobotApp.VDA5050/Factsheet/Envelopes3d.cs

@@ -2,15 +2,13 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
-
 public class Envelopes3d
 {
     [Required]
-    public string Set { get; set; }
+    public string Set { get; set; } = string.Empty;
     [Required]
-    public string Format { get; set; }
+    public string Format { get; set; } = string.Empty;
-    public object Data { get; set; }
+    public object Data { get; set; } = string.Empty;
-    public string Url { get; set; }
+    public string Url { get; set; } = string.Empty;
-    public string Description { get; set; }
+    public string Description { get; set; } = string.Empty;
 }

RobotApp.VDA5050/Factsheet/FactSheetMsg.cs

@@ -2,29 +2,27 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
 
 public  class FactSheetMsg
 {
-    public uint HeaderId { get; set; }
+    public uint HeaderId { get; set; } = 1;
-    public string Timestamp { get; set; }
+    public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
     [Required]
-    public string Version { get; set; }
+    public string Version { get; set; } = "1.0.0";
     [Required]
-    public string Manufacturer { get; set; }
+    public string Manufacturer { get; set; } = "PhenikaaX";
     [Required]
-    public string SerialNumber { get; set; }
+    public string SerialNumber { get; set; } = string.Empty;
     [Required]
-    public TypeSpecification TypeSpecification { get; set; }
+    public TypeSpecification TypeSpecification { get; set; } = new();
     [Required]
-    public PhysicalParameters PhysicalParameters { get; set; }
+    public PhysicalParameters PhysicalParameters { get; set; } = new();
     [Required]
-    public ProtocolLimits ProtocolLimits { get; set; }
+    public ProtocolLimits ProtocolLimits { get; set; } = new();
     [Required]
-    public ProtocolFeatures ProtocolFeatures { get; set; }
+    public ProtocolFeatures ProtocolFeatures { get; set; } = new();
     [Required]
-    public AgvGeometry AgvGeometry { get; set; }
+    public AgvGeometry AgvGeometry { get; set; } = new();
     [Required]
-    public LoadSpecification LoadSpecification { get; set; }    
+    public LoadSpecification LoadSpecification { get; set; } = new();
-    //public LocalizationParameter LocalizationParameters { get; set; }
 }

RobotApp.VDA5050/Factsheet/LoadSets.cs

@@ -1,14 +1,12 @@
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
-
 public class LoadSets
 {
-    public string SetName { get; set; } 
+    public string SetName { get; set; } = string.Empty;
-    public string LoadType { get; set; }
+    public string LoadType { get; set; } = string.Empty;
-    public string[] LoadPositions { get; set; }
+    public string[] LoadPositions { get; set; } = [];
-    public BoundingBoxReference BoundingBoxReference { get; set; }
+    public BoundingBoxReference BoundingBoxReference { get; set; } = new();
-    public LoadDimensions LoadDimensions { get; set; }
+    public LoadDimensions LoadDimensions { get; set; } = new();
     public double MaxWeigth { get; set; }
     public double MinLoadhandlingHeight { get; set; }
     public double MaxLoadhandlingHeight { get; set; }
@@ -21,6 +19,6 @@ public class LoadSets
     public double AgvDecelerationLimit { get; set; }
     public double PickTime { get; set; }
     public double DropTime { get; set; }
-    public string Description { get; set; }
+    public string Description { get; set; } = string.Empty;
 
 }

RobotApp.VDA5050/Factsheet/LoadSpecification.cs

@@ -1,9 +1,8 @@
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
 
 public class LoadSpecification
 {
-    public string[] LoadPositions { get; set; } 
+    public string[] LoadPositions { get; set; } = [];
-    public LoadSets[] LoadSets { get; set; }
+    public LoadSets[] LoadSets { get; set; } = [];
 }

RobotApp.VDA5050/Factsheet/OptionalParameters.cs

@@ -2,7 +2,6 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
 
 public enum Support
 {
@@ -12,8 +11,8 @@ public enum Support
 public class OptionalParameters
 {
     [Required]
-    public string Parameter {  get; set; }
+    public string Parameter {  get; set; } = string.Empty;
     [Required]
-    public string Support { get; set; }
+    public string Support { get; set; } = string.Empty;
-    public string Description { get; set; }
+    public string Description { get; set; } = string.Empty;
 }

RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs

@@ -2,12 +2,11 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
 
 public class ProtocolFeatures
 {
     [Required]
-    public OptionalParameters[] OptionalParameters { get; set; }
+    public OptionalParameters[] OptionalParameters { get; set; } = [];
     [Required]
-    public AgvActions[] AgvActions { get; set; }  
+    public AgvActions[] AgvActions { get; set; } = [];
 }

RobotApp.VDA5050/Factsheet/ProtocolLimits.cs

@@ -2,13 +2,12 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
 public class ProtocolLimits
 {
     [Required]
-    public MaxStringLens MaxStringLens { get; set; }
+    public MaxStringLens MaxStringLens { get; set; } = new();
     [Required]
-    public MaxArrayLens MaxArrayLens { get; set; }
+    public MaxArrayLens MaxArrayLens { get; set; } = new();
     [Required]
-    public Timing Timing { get; set; }
+    public Timing Timing { get; set; } = new();
 }

RobotApp.VDA5050/Factsheet/TypeSpecification.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
-
 public enum AgvKinematic
 {
     DIFF,
@@ -35,12 +33,12 @@ public enum NavigationTypes
 public class TypeSpecification
 {
     [Required]
-    public string SeriesName { get; set; }  
+    public string SeriesName { get; set; } = string.Empty;
-    public string SeriesDescription { get; set; }
+    public string SeriesDescription { get; set; } = string.Empty;
     [Required]
-    public string AgvKinematic { get; set; }
+    public string AgvKinematic { get; set; } = string.Empty;
     [Required]
-    public string AgvClass { get; set; }
+    public string AgvClass { get; set; } = string.Empty;
     [Required]
     public double MaxLoadMass { get; set; }
     [Required]

RobotApp.VDA5050/Factsheet/WheelDefinitions.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.Factsheet;
 
-#nullable disable
-
 public enum WheelDefinitionsType
 {
     DRIVE,
@@ -11,6 +9,7 @@ public enum WheelDefinitionsType
     FIXED,
     MECANUM,
 }
+
 public class WheelDefinitionsPosition
 {
     [Required]
@@ -23,17 +22,17 @@ public class WheelDefinitionsPosition
 public class WheelDefinitions
 {
     [Required]
-    public string Type { get; set; }
+    public string Type { get; set; } = string.Empty;
     [Required]
     public bool IsActiveDriven { get; set; }
     [Required]
     public bool IsActiveSteered { get; set; }
     [Required]
-    public WheelDefinitionsPosition Position { get; set; }
+    public WheelDefinitionsPosition Position { get; set; } = new();
     [Required]
     public double Diameter { get; set; }
     [Required]
     public double Width { get; set; }
     public double CenterDisplacement { get; set; }
-    public string Constraints { get; set; }
+    public string Constraints { get; set; } = string.Empty;
 }

RobotApp.VDA5050/FactsheetExtend/Battery.cs

@@ -1,9 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class Battery
-{
-    public uint Battery_low { get; set; }
-    public uint Battery_normal { get; set; }
-    public uint Battery_good { get; set; }
-    public uint Battery_full { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/BatteryThreshold.cs

@@ -1,11 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public enum BatteryThreshold
-{
-    LOW,
-    NORMAL,
-    MIDDLE,
-    GOOD,
-    FULL,
-    NONE
-}

RobotApp.VDA5050/FactsheetExtend/CameraSafety.cs

@@ -1,31 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class CameraSafety
-{
-    public double Pass_through_x_min { get; set; }
-    public double Pass_through_x_max { get; set; }
-    public double Pass_through_y_min { get; set; }
-    public double Pass_through_y_max { get; set; }
-    public double Pass_through_z_min { get; set; }
-    public double Pass_through_z_max { get; set; }
-    public uint Ground_seg_max_iterations { get; set; }
-    public double Ground_seg_distance_threshold { get; set; }
-    public double Warn_z1 { get; set; }
-    public double Protect_z1 { get; set; }
-    public double Warn_z2 { get; set; }
-    public double Protect_z2 { get; set; }
-    public double Warn_z3 { get; set; }
-    public double Protect_z3 { get; set; }
-    public double Warn_z4 { get; set; }
-    public double Protect_z4 { get; set; }
-    public double Warn_z5 { get; set; }
-    public double Protect_z5 { get; set; }
-    public double Warn_z6 { get; set; }
-    public double Protect_z6 { get; set; }
-    public uint Min_cluster_warn_size { get; set; }
-    public uint Min_cluster_protect_size { get; set; }
-    public uint Min_cluster_detect_size { get; set; }
-    public uint Min_consecutive_warn_count { get; set; }
-    public uint Min_consecutive_protect_count { get; set; }
-    public uint Min_consecutive_detect_count { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/ChargerParam.cs

@@ -1,9 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-#nullable disable
-
-public class ChargerParam
-{
-    public string Charger_ip { get; set; }
-    public uint Charger_port { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/FactsheetExtendMsg.cs

@@ -1,19 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-#nullable disable
-
-public class FactsheetExtendMsg
-{
-    public uint HeaderId { get; set; }
-    public DateTime Timestamp { get; set; }
-    public string Version { get; set; }
-    public string Manufacturer { get; set; }
-    public string SerialNumber { get; set; }
-
-    public ServerParam Server_param { get; set; }
-    public RobotParam Robot_param { get; set; }
-    public Localization Localization { get; set; }
-    public Navigation Navigation { get; set; }
-    public Safety Safety { get; set; }
-    public ChargerParam Charger_param { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/ForkSafety.cs

@@ -1,9 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class ForkSafety
-{
-    public double Muted_field_size { get; set; }
-    public double Protected_field_size { get; set; }
-    public double Warning_field_size { get; set; }
-    public double Detect_field_size { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/Initpose.cs

@@ -1,9 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class Initpose
-{
-    public bool Use_manual_initpose { get; set; }
-    public double Initpose_x { get; set; }
-    public double Initpose_y { get; set; }
-    public double Initpose_yaw { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/LineSegment.cs

@@ -1,10 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class LineSegment
-{
-    public double Least_thresh { get; set; }
-    public double Min_line_length { get; set; }
-    public double Predict_distance { get; set; }
-    public uint Seed_line_points { get; set; }
-    public uint Min_line_points { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/Localization.cs

@@ -1,14 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-#nullable disable
-
-public class Localization
-{
-    public uint Threshold_quality_loc { get; set; }
-    public bool Use_localization_marker { get; set; }
-    public bool Use_pallet_detection { get; set; }
-    public Initpose Initpose { get; set; }
-    public Xloc Xloc { get; set; }
-    public VlMarker Vl_marker { get; set; }
-
-}

RobotApp.VDA5050/FactsheetExtend/Motor.cs

@@ -1,9 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class Motor
-{
-    public double OdomEncSteeringAngleOffset { get; set; }
-    public double Steering_fix_wheel_distance_x { get; set; }
-    public double Steering_fix_wheel_distance_y { get; set; }
-    public double WheelAcceleration { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/Navigation.cs

@@ -1,11 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-#nullable disable
-public class Navigation
-{
-    public bool Using_control_safety { get; set; }
-    public uint Control_rate { get; set; }
-    public Rotate Rotate { get; set; }
-    public PTA Pta { get; set; }
-    public PPA Ppa { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/PPA.cs

@@ -1,7 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class PPA
-{
-    public double Ppa_accuracy_goal { get; set; }
-    public double Ppa_distance_reduce { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/PTA.cs

@@ -1,15 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class PTA
-{
-    public double Pta_linear_vel_max { get; set; }
-    public double Pta_linear_vel_min { get; set; }
-    public double Pta_accuracy_goal { get; set; }
-    public double Pta_distance_reduce { get; set; }
-    public double Pta_acceleration { get; set; }
-    public double Pta_lm_front { get; set; }
-    public double Pta_lm_back { get; set; }
-    public double Pta_phi_max { get; set; }
-    public double Pta_amplitude_max { get; set; }
-    public uint Pta_w_option { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/RobotParam.cs

@@ -1,12 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class RobotParam
-{
-    public bool Use_dynamic_parameter { get; set; }          // (Default: True) Declare whether to use dynamic parameters or not
-    public string? Ethernet_name { get; set; }                // The name of Ethernet port which connects to wifi client (e.g eno1, lo, enp3s0)
-    public double Speed_max_backward { get; set; }          // (Default: True) Declare whether to use dynamic parameters or not
-    public uint Num_day_logger { get; set; }                // The name of Ethernet port which connects to wifi client (e.g eno1, lo, enp3s0)
-
-    public Motor Motor { get; set; } = new();
-    public Battery Battery { get; set; } = new();
-}

RobotApp.VDA5050/FactsheetExtend/Rotate.cs

@@ -1,9 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class Rotate
-{
-    public double Angular_vel_max { get; set; }
-    public double Angular_vel_min { get; set; }
-    public double Acceleration_rotate { get; set; }
-    public double Tolerances_rotate { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/Safety.cs

@@ -1,8 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class Safety
-{
-    public bool Use_camera_safety { get; set; }
-    public CameraSafety Camera_safety { get; set; } = new();
-    public ForkSafety Fork_safety { get; set; } = new();
-}

RobotApp.VDA5050/FactsheetExtend/ServerParam.cs

@@ -1,14 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-#nullable disable
-
-public class ServerParam
-{
-    public string Server_ip { get; set; }
-    public string Server_port { get; set; }
-    public string Keepalive { get; set; }
-    public string Username { get; set; }
-    public string Password { get; set; }
-    public string Client_protocol { get; set; }
-    public string Client_id { get; set; }
-}

RobotApp.VDA5050/FactsheetExtend/VlMarker.cs

@@ -1,21 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class VlMarker
-{
-    public bool Use_odometry { get; set; }
-    public uint V_angle { get; set; }
-    public double Length_v { get; set; }
-    public double Length_l { get; set; }
-    public double X_laser { get; set; }
-    public double Y_laser { get; set; }
-    public bool Flip_laser { get; set; }
-    public double Rotate_laser { get; set; }
-    public uint Frequence_control { get; set; }
-    public double Angle_min { get; set; }
-    public double Angle_max { get; set; }
-    public double Max_init_x { get; set; }
-    public double Max_init_y { get; set; }
-    public double Max_init_yaw { get; set; }
-
-    public LineSegment Line_segment { get; set; } = new();
-}

RobotApp.VDA5050/FactsheetExtend/Xloc.cs

@@ -1,16 +0,0 @@
-namespace RobotApp.VDA5050.FactsheetExtend;
-
-public class Xloc
-{
-    public double Front_vls_width { get; set; }
-    public double Front_vls_pose_x { get; set; }
-    public double Front_vls_pose_y { get; set; }
-    public double Front_vls_pose_yaw { get; set; }
-    public uint Front_vls_source_id { get; set; }
-
-    public double Rear_vls_width { get; set; }
-    public double Rear_vls_pose_x { get; set; }
-    public double Rear_vls_pose_y { get; set; }
-    public double Rear_vls_pose_yaw { get; set; }
-    public uint Rear_vls_source_id { get; set; }
-}

RobotApp.VDA5050/InstantAction/Action.cs

@@ -2,14 +2,13 @@
 
 namespace RobotApp.VDA5050.InstantAction;
 
-#nullable disable 
-
 public enum BlockingType
 {
     NONE,
     SOFT,
     HARD
 }
+
 public class Action
 {
     [Required]

RobotApp.VDA5050/InstantAction/ActionParameter.cs

@@ -2,7 +2,6 @@
 
 namespace RobotApp.VDA5050.InstantAction;
 
-#nullable disable
 public class ActionParameter
 {
     [Required]

RobotApp.VDA5050/InstantAction/InstantActionsMsg.cs

@@ -1,7 +1,5 @@
 namespace RobotApp.VDA5050.InstantAction;
 
-#nullable disable
-
 public class InstantActionsMsg
 {
     public uint HeaderId { get; set; }

RobotApp.VDA5050/State/ActionState.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public enum ActionStatus
 {
     WAITING,
@@ -15,11 +13,11 @@ public enum ActionStatus
 }
 public class ActionState
 {
-    public string ActionType { get; set; }
+    public string ActionType { get; set; } = string.Empty;
     [Required]
-    public string ActionId { get; set; }
+    public string ActionId { get; set; } = string.Empty;
-    public string ActionDescription { get; set; }
+    public string ActionDescription { get; set; } = string.Empty;
     [Required]
-    public string ActionStatus { get; set; }
+    public string ActionStatus { get; set; } = string.Empty;
-    public string ResultDescription { get; set; }
+    public string ResultDescription { get; set; } = string.Empty;
 }

RobotApp.VDA5050/State/EdgeState.cs

@@ -3,17 +3,15 @@ using System.ComponentModel.DataAnnotations;
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public class EdgeState
 {
 
     [Required]
-    public string EdgeId { get; set; }
+    public string EdgeId { get; set; } = string.Empty;
     [Required]
     public int SequenceId { get; set; }
-    public string EdgeDescription { get; set; }
+    public string EdgeDescription { get; set; } = string.Empty;
     [Required]
     public bool Released { get; set; }
-    public Trajectory Trajectory { get; set; }
+    public Trajectory Trajectory { get; set; } = new();
 }

RobotApp.VDA5050/State/Error.cs

@@ -2,28 +2,33 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public enum ErrorLevel
 {
     NONE,
     WARNING,
     FATAL
 }
+
 public class ErrorReferences
 {
     [Required]
-    public string ReferenceKey { get; set; }
+    public string ReferenceKey { get; set; } = string.Empty;
     [Required]
-    public string ReferenceValue { get; set; }
+    public string ReferenceValue { get; set; } = string.Empty;
 }
+
+public enum ErrorType
+{
+    INITIALIZE_ORDER,
+}
+
 public class Error
 {
     [Required]
-    public string ErrorType { get; set; }
+    public string ErrorType { get; set; } = string.Empty;
-    public ErrorReferences[] ErrorReferences { get; set; }
+    public ErrorReferences[] ErrorReferences { get; set; } = [];
-    public string ErrorDescription { get; set; }
+    public string ErrorDescription { get; set; } = string.Empty;
-    public string ErrorHint { get; set; }
+    public string ErrorHint { get; set; } = string.Empty;
     [Required]
-    public string ErrorLevel { get; set; }
+    public string ErrorLevel { get; set; } = string.Empty;
 }

RobotApp.VDA5050/State/Information.cs

@@ -2,7 +2,6 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
 
 public enum InfoLevel
 {
@@ -12,16 +11,16 @@ public enum InfoLevel
 public class InfomationReferences
 {
     [Required]
-    public string ReferenceKey { get; set; }
+    public string ReferenceKey { get; set; } = string.Empty;
     [Required]
-    public string ReferenceValue { get; set; }
+    public string ReferenceValue { get; set; } = string.Empty;
 }
 public class Information
 {
     [Required]
-    public string InfoType { get; set; }
+    public string InfoType { get; set; } = string.Empty;
-    public InfomationReferences[] InfoReferences { get; set; }
+    public InfomationReferences[] InfoReferences { get; set; } = [];
-    public string InfoDescription { get; set; }
+    public string InfoDescription { get; set; } = string.Empty;
     [Required]
-    public string InfoLevel { get; set; }
+    public string InfoLevel { get; set; } = string.Empty;
 }

RobotApp.VDA5050/State/Load.cs

@@ -2,15 +2,13 @@
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public class Load
 {
-    public string LoadId { get; set; }
+    public string LoadId { get; set; } = string.Empty;
-    public string LoadType { get; set; }
+    public string LoadType { get; set; } = string.Empty;
-    public string LoadPosition { get; set; }
+    public string LoadPosition { get; set; } = string.Empty;
-    public BoundingBoxReference BoundingBoxReference { get; set; }
+    public BoundingBoxReference BoundingBoxReference { get; set; } = new();
-    public LoadDimensions LoadDimensions { get; set; }
+    public LoadDimensions LoadDimensions { get; set; } = new();
     public double Weight { get; set; } 
 
 }

RobotApp.VDA5050/State/NodeState.cs

@@ -1,20 +1,18 @@
-using RobotApp.VDA5050.Order;
+using System.ComponentModel.DataAnnotations;
-using System.ComponentModel.DataAnnotations;
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
 
 public class NodeState
 {
     [Required]
-    public string NodeId { get; set; }
+    public string NodeId { get; set; } = string.Empty;
     [Required]
     public int SequenceId { get; set; }
-    public string NodeDescription { get; set; }
+    public string NodeDescription { get; set; } = string.Empty;
     [Required]
     public bool Released { get; set; }
-    public NodePosition NodePosition { get; set; }
+    public NodePosition NodePosition { get; set; } = new();
 }
 
 public class NodePosition
@@ -25,6 +23,6 @@ public class NodePosition
     public double Y { get; set; }
     public double Theta { get; set; }
     [Required]
-    public string MapId { get; set; } = "";
+    public string MapId { get; set; } = string.Empty;
 
 }

RobotApp.VDA5050/State/StateMsg.cs

@@ -3,8 +3,6 @@ using System.ComponentModel.DataAnnotations;
 
 namespace RobotApp.VDA5050.State;
 
-#nullable disable
-
 public enum OperatingMode
 {
     AUTOMATIC,
@@ -17,22 +15,21 @@ public class StateMsg
 {
     [Required]
     public uint HeaderId { get; set; }
+    public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
     [Required]
-    public string Timestamp { get; set; }
+    public string Version { get; set; } = "1.0.0";
     [Required]
-    public string Version { get; set; }
+    public string Manufacturer { get; set; } = "PhenikaaX";
     [Required]
-    public string Manufacturer { get; set; }
+    public string SerialNumber { get; set; } = string.Empty;
-    [Required]
-    public string SerialNumber { get; set; }
     public Map[] Maps { get; set; } = [];
     [Required]
-    public string OrderId { get; set; }
+    public string OrderId { get; set; } = string.Empty;
     [Required]
     public int OrderUpdateId { get; set; }
-    public string ZoneSetId { get; set; }
+    public string ZoneSetId { get; set; } = string.Empty;
     [Required]
-    public string LastNodeId { get; set; }
+    public string LastNodeId { get; set; } = string.Empty;
     [Required]
     public int LastNodeSequenceId { get; set; }
     [Required]
@@ -41,7 +38,7 @@ public class StateMsg
     public bool NewBaseRequest { get; set; }
     public double DistanceSinceLastNode { get; set; }
     [Required]
-    public string OperatingMode { get; set; }
+    public string OperatingMode { get; set; } = string.Empty;
     [Required]
     public NodeState[] NodeStates { get; set; } = [];
     [Required]

RobotApp.VDA5050/Type/ActionType.cs

@@ -2,34 +2,16 @@
 
 public enum ActionType
 {
-    startPause,              // No actionParameters
+    START_PAUSE,
-    stopPause,              // No actionParameters
+    STOP_PAUSE,
-    startCharging,         // ActionParameters {CHARGER_IP, CHARGER_PORT, X, Y, THETA}
+    START_CHARGING,
-    stopCharging,            // ActionParameters {X, Y, THETA}
+    STOP_CHARGING,
-    initPosition,            // ActionParameters {X, Y, THETA, MAP_ID, LAST_NODE_ID}
+    INITIALIZATION_POSITION,             
-    stateRequest,            // No actionParameters
+    PICK,                   
-    logReport,               // No actionParameters
+    DROP,               
-    pick,                    // No actionParameters
+    CANCEL_ORDER,       
-    drop,                   // No actionParameters
+    ROTATE,
-    detectObject,            // No actionParameters
+    REQUEST_FACTSHEET,
-    finePositioning,         // ActionParameters {X, Y, THETA, MAP_ID, LAST_NODE_ID}
+    REQUEST_VISUALIZATION,
-    waitForTrigger,          // No actionParameters
+    REQUEST_STATE,
-    cancelOrder,             // No actionParameters
-    factsheetRequest,        // No actionParameters
-
-    setDynparam,             // ActionParameters {PARAM_NAME, PARAM_TYPE, PARAM_VALUE}
-    saveDynparamRuntime,     // No actionParameters
-    loadDynparamRuntime,     // No actionParameters
-    loadDynparamDefault,     // No actionParameters
-    getDynparamRuntime,      // No actionParameters
-
-    controlLight,            // ActionParameters {LIGHT_TYPE, CONTROL_TYPE}
-    controlFan,              // ActionParameters {CONTROL_TYPE}
-    controlSpeaker,          // ActionParameters {SONG_NUMBER, CONTROL_TYPE}
-    controlSafetyField,      // ActionParameters {FIELD_TYPE, CONTROL_TYPE}
-    startInPallet,        // ActionParameters {X, Y, THETA}
-    startOutPallet,                    // ActionParameters {X, Y, THETA}      
-
-    rotate,      // ActionParameters {THETA}      
-    setMap,      // ActionParameter {MAP_ID}
 }

RobotApp.VDA5050/VDA5050Helper.cs

@@ -1,29 +0,0 @@
-using RobotApp.VDA5050.State;
-using System.Text;
-
-namespace RobotApp.VDA5050;
-
-public class VDA5050Helper
-{
-    public static string ConvertErrorDetail(IEnumerable<Error> errors)
-    {
-        string errorsType = "";
-        foreach (var error in errors)
-        {
-            if (error == errors.Last()) errorsType += $"{error.ErrorType}";
-            else errorsType += $"{error.ErrorType}, ";
-        }
-        StringBuilder errorStr = new($"Robot có lỗi: [{errorsType}]\n");
-        foreach (var error in errors)
-        {
-            string errorDes = $"- {error.ErrorType}: {error.ErrorDescription}\n";
-            errorStr.Append(errorDes.PadLeft(errorDes.Length + 3));
-            foreach (var refer in error.ErrorReferences)
-            {
-                string errorRefer = $"+ {refer.ReferenceKey}: {refer.ReferenceValue}\n";
-                errorStr.Append(errorRefer.PadLeft(errorRefer.Length + 9));
-            }
-        }
-        return !errors.Any() ? "" : errorStr.ToString();
-    }
-}

RobotApp.VDA5050/VDA5050Setting.cs

@@ -1,19 +1,12 @@
 namespace RobotApp.VDA5050;
 
-#nullable disable
-
 public class VDA5050Setting
 {
-    public bool ServerEnable { get; set; }
+    public string HostServer { get; set; } = string.Empty;
-    public string HostServer { get; set; }
+    public int Port { get; set; } = 1883;
-    public int Port { get; set; }
+    public string UserName { get; set; } = "robotics";
-    public string UserName { get; set; }
+    public string Password { get; set; } = "robotics";
-    public string Password { get; set; }
+    public string Manufacturer { get; set; } = "PhenikaaX";
-    public string Manufacturer { get; set; }
+    public string Version { get; set; } = "0.0.1";
-    public string Version { get; set; }
+    public int PublishRepeat { get; set; } = 2;
-    public int Repeat { get; set; }
-    public int ConnectionTimeoutSeconds { get; set; }
-    public int ConnectionBacklog { set; get; }
-    public int KeepAliveInterval { get; set; }
-    public int CheckingRobotMsgTimout { get; set; }
 }

RobotApp.VDA5050/VDA5050Topic.cs

@@ -1,12 +1,46 @@
 namespace RobotApp.VDA5050;
 
-public static class VDA5050Topic
+public enum VDA5050Topic
 {
-    public const string Connection = "connection";
+    CONNECTION,
-    public const string Order = "order";
+    ORDER,
-    public const string InstantActions = "instantActions";
+    INSTANTACTIONS,
-    public const string State = "state";
+    STATE,
-    public const string Visualization = "visualization";
+    VISUALIZATION,
-    public const string Factsheet = "factsheet";
+    FACTSHEET
-    public const string FactsheetExtend = "factsheetExtend";    // custom by TungNV
+}
+
+public static class EnumExtensions
+{
+    private static readonly Dictionary<VDA5050Topic, string> TopicToStringMap = new()
+    {
+        { VDA5050Topic.CONNECTION, "connection" },
+        { VDA5050Topic.ORDER, "order" },
+        { VDA5050Topic.INSTANTACTIONS, "instantActions" },
+        { VDA5050Topic.STATE, "state" },
+        { VDA5050Topic.VISUALIZATION, "visualization" },
+        { VDA5050Topic.FACTSHEET, "factsheet" }
+    };
+
+    private static readonly Dictionary<string, VDA5050Topic> StringToTopicMap = new(StringComparer.OrdinalIgnoreCase)
+    {
+        { "connection", VDA5050Topic.CONNECTION },
+        { "order", VDA5050Topic.ORDER },
+        { "instantActions", VDA5050Topic.INSTANTACTIONS },
+        { "state", VDA5050Topic.STATE },
+        { "visualization", VDA5050Topic.VISUALIZATION },
+        { "factsheet", VDA5050Topic.FACTSHEET }
+    };
+
+    public static string ToTopicString(this VDA5050Topic type)
+    {
+        if (TopicToStringMap.TryGetValue(type, out var value)) return value;
+        throw new ArgumentException($"Invalid VDA5050Topic: {type}");
+    }
+
+    public static VDA5050Topic ToTopic(string value)
+    {
+        if (StringToTopicMap.TryGetValue(value, out var result)) return result;
+        throw new ArgumentException($"No VDA5050Topic with string value '{value}' found.");
+    }
 }

RobotApp.VDA5050/Visualization/AgvPosition.cs

@@ -2,8 +2,6 @@
 
 namespace RobotApp.VDA5050.Visualization;
 
-#nullable disable
-
 public class AgvPosition
 {
     [Required]
@@ -11,7 +9,7 @@ public class AgvPosition
     [Required]
     public double Y { get; set; }
     [Required]
-    public string MapId { get; set; }
+    public string MapId { get; set; } = string.Empty;
     [Required]
     public double Theta { get; set; }
     [Required]

RobotApp.VDA5050/Visualization/Visualizationmsg.cs

@@ -1,16 +1,16 @@
-namespace RobotApp.VDA5050.Visualization;
+using System.ComponentModel.DataAnnotations;
 
-#nullable disable
+namespace RobotApp.VDA5050.Visualization;
 
 public class VisualizationMsg
 {
     public uint HeaderId { get; set; }
-    public string Timestamp { get; set; }
+    public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
-    public string Version { get; set; }
+    public string Version { get; set; } = "1.0.0";
-    public string Manufacturer { get; set; }
+    public string Manufacturer { get; set; } = "PhenikaaX";
-    public string SerialNumber { get; set; }
+    public string SerialNumber { get; set; } = string.Empty;
-    public string MapId { get; set; }
+    public string MapId { get; set; } = string.Empty;
-    public string MapDescription { get; set; }
+    public string MapDescription { get; set; } = string.Empty;
     public AgvPosition AgvPosition { get; set; } = new();
     public Velocity Velocity { get; set; } = new();
 }

RobotApp.sln

@@ -9,6 +9,8 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp.Client", "RobotApp
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp.VDA5050", "RobotApp.VDA5050\RobotApp.VDA5050.csproj", "{617FD155-904A-44E6-AD1A-6BC878421F9F}"
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp.Common.Shares", "RobotApp.Common.Shares\RobotApp.Common.Shares.csproj", "{480F459B-F07C-44D9-A738-E8DF6C438A80}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -27,6 +29,10 @@ Global
 		{617FD155-904A-44E6-AD1A-6BC878421F9F}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{617FD155-904A-44E6-AD1A-6BC878421F9F}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{617FD155-904A-44E6-AD1A-6BC878421F9F}.Release|Any CPU.Build.0 = Release|Any CPU
+		{480F459B-F07C-44D9-A738-E8DF6C438A80}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{480F459B-F07C-44D9-A738-E8DF6C438A80}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{480F459B-F07C-44D9-A738-E8DF6C438A80}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{480F459B-F07C-44D9-A738-E8DF6C438A80}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

RobotApp/Interfaces/IBattery.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Interfaces;
+
+public interface IBattery
+{
+}

RobotApp/Interfaces/IDriver.cs

@@ -0,0 +1,37 @@
+namespace RobotApp.Interfaces;
+
+/// <summary>
+/// Interface điều khiển động cơ robot
+/// </summary>
+public interface IDriver
+{
+    /// <summary>
+    /// Trạng thái sẵn sàng nhận lệnh điều khiển
+    /// </summary>
+    bool IsReady { get; }
+
+    /// <summary>
+    /// Tốc độ động cơ bên trái
+    /// </summary>
+    double LeftVelocity { get; }
+
+    /// <summary>
+    /// Tốc độ động cơ bên phải
+    /// </summary>
+    double RightVelocity { get; }
+
+    /// <summary>
+    /// Điều khiển tốc độ động cơ trái và phải
+    /// </summary>
+    /// <param name="left">Tốc độ động cơ bên trái</param>
+    /// <param name="right">Tốc độ động cơ bên phải</param>
+    /// <returns></returns>
+    bool ControlVelocity(double left, double right);
+
+    /// <summary>
+    /// Dừng robot
+    /// </summary>
+    /// <param name="isFree">true nếu dừng thả trôi, false nếu dừng khóa cứng</param>
+    /// <returns></returns>
+    bool Stop(bool isFree);
+}

RobotApp/Interfaces/IError.cs

@@ -0,0 +1,9 @@
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Interfaces;
+
+public interface IError
+{
+    bool HasFatalError { get; }
+    void AddError(Error error, TimeSpan? clearAfter = null);
+}

RobotApp/Interfaces/IInfomation.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Interfaces;
+
+public interface IInfomation
+{
+}

RobotApp/Interfaces/IInstanceActions.cs

@@ -0,0 +1,14 @@
+using RobotApp.VDA5050.State;
+using Action = RobotApp.VDA5050.InstantAction.Action;
+
+namespace RobotApp.Interfaces;
+
+public interface IInstanceActions
+{
+    ActionState[] ActionStates { get; }
+    bool HasActionRunning { get; }
+    bool AddOrderActions(Action[] actions);
+    bool StartAction(string actionId);
+    bool AddInstanceAction(Action action);
+    bool StopAction();
+}

RobotApp/Interfaces/ILocalization.cs

@@ -0,0 +1,28 @@
+namespace RobotApp.Interfaces;
+
+public interface ILocalization
+{
+    /// <summary>
+    /// Vị trí hiện tại của robot trong hệ tọa độ toàn cục theo trục X (đơn vị: mét).
+    /// </summary>
+    double X { get; }
+
+    /// <summary>
+    /// Vị trí hiện tại của robot trong hệ tọa độ toàn cục theo trục Y (đơn vị: mét).
+    /// </summary>
+    double Y { get; }
+
+    /// <summary>
+    /// Hướng hiện tại của robot trong hệ tọa độ toàn cục (đơn vị: độ).
+    /// </summary>
+    double Theta { get; }
+
+    /// <summary>
+    /// Khởi tạo vị trí của robot trong hệ tọa độ toàn cục.
+    /// </summary>
+    /// <param name="x">đơn vị: mét</param>
+    /// <param name="y">đơn vị: mét</param>
+    /// <param name="theta">đơn vị: độ</param>
+    /// <returns></returns>
+    bool InitializePosition(double x, double y, double theta);
+}

RobotApp/Interfaces/INavigation.cs

@@ -0,0 +1,37 @@
+using RobotApp.VDA5050.Order;
+
+namespace RobotApp.Interfaces;
+
+public enum NavigationState
+{
+    None,
+    Initializing,
+    Initialized,
+    Idle,
+    Moving,
+    Rotating,
+    Paused,
+    Error
+}
+
+public enum NavigationProccess
+{
+    None,
+    InProgress,
+    Completed,
+    Failed,
+    Cancelled
+}
+
+public interface INavigation
+{
+    bool Driving { get; }
+    NavigationState State { get; }
+    void Move(Node[] nodes, Edge[] edges);
+    void MoveStraight(double x, double y);
+    void Rotate(double angle);
+    void Paused();
+    void Resume();
+    void UpdateOrder(int lastBaseSequence);
+    void CancelMovement();
+}

RobotApp/Interfaces/IOrder.cs

@@ -0,0 +1,19 @@
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Interfaces;
+
+public interface IOrder
+{
+    string OrderId { get; }
+    int OrderUpdateId { get; }
+    string LastNodeId { get; }
+    int LastNodeSequenceId { get; }
+
+    NodeState[] NodeStates { get; }
+    EdgeState[] EdgeStates { get; }
+
+    void StartOrder(string orderId, Node[] nodes, Edge[] edges);
+    void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
+    void StopOrder();
+}

RobotApp/Interfaces/IPeripheral.cs

@@ -0,0 +1,14 @@
+namespace RobotApp.Interfaces;
+
+public enum OperatingMode
+{
+    AUTOMATIC,
+    MANUAL,
+    SEMIAUTOMATIC,
+    TEACHIN,
+    SERVICE,
+}
+public interface IPeripheral
+{
+    OperatingMode OperatingMode { get; }
+}

RobotApp/Interfaces/IRFHandler.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Interfaces
+{
+    public class IRFHandler
+    {
+    }
+}

RobotApp/Interfaces/ISafety.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Interfaces;
+
+public interface ISafety
+{
+}

RobotApp/Interfaces/ISensorIMU.cs

@@ -0,0 +1,12 @@
+namespace RobotApp.Interfaces;
+
+/// <summary>
+/// Interface cảm biến IMU
+/// </summary>
+public interface ISensorIMU
+{
+    /// <summary>
+    /// Góc xoay của robot (đơn vị độ)
+    /// </summary>
+    double Angle { get; }
+}

RobotApp/Modbus/ModbusException.cs

@@ -0,0 +1,16 @@
+namespace RobotApp.Modbus;
+
+public class ModbusException : Exception
+{
+    public ModbusException()
+    {
+    }
+    public ModbusException(string message)
+        : base(message)
+    {
+    }
+    public ModbusException(string message, Exception inner)
+        : base(message, inner)
+    {
+    }
+}

RobotApp/Modbus/ModbusTcpClient.cs

@@ -0,0 +1,350 @@
+using Microsoft.AspNetCore.Connections;
+using System.Net;
+using System.Net.Sockets;
+
+namespace RobotApp.Modbus;
+
+public class ModbusTcpClient(string IpAddress, int Port, byte ClientId) : IDisposable
+{
+    public bool IsConnected => !disposed && tcpClient != null && tcpClient.Client.Connected && stream != null;
+
+    private TcpClient? tcpClient;
+    private NetworkStream? stream;
+    private bool disposed = false;
+
+    private uint transactionIdentifierInternal = 0;
+    private const int connectTimeout = 3000;
+    private const int readTimeout = 500;
+    private const int writeTimeout = 500;
+    private int numberOfRetries { get; set; } = 3;
+
+    ~ModbusTcpClient()
+    {
+        Dispose(false);
+    }
+
+    public static bool TryConnect(string ipAddress, int port, byte clientId, out ModbusTcpClient? client)
+    {
+        ModbusTcpClient modbusTcpClient = new(ipAddress, port, clientId);
+        try
+        {
+            if (modbusTcpClient.Connect())
+            {
+                client = modbusTcpClient;
+                return true;
+            }
+            else
+            {
+                modbusTcpClient?.Dispose();
+                client = null;
+                return false;
+            }
+        }
+        catch
+        {
+            modbusTcpClient.Dispose();
+            client = null;
+            return false;
+        }
+
+    }
+
+    public static bool TryConnect(string ipAddress, out ModbusTcpClient? client) => TryConnect(ipAddress, 502, 1, out client);
+
+    public bool Connect()
+    {
+        try
+        {
+            tcpClient = new TcpClient();
+            var result = tcpClient.BeginConnect(IpAddress, Port, null, null);
+            if (!result.AsyncWaitHandle.WaitOne(connectTimeout))
+            {
+                tcpClient?.Close();
+                tcpClient?.Dispose();
+                tcpClient = null;
+                return false;
+            }
+            tcpClient.EndConnect(result);
+
+            stream = tcpClient.GetStream();
+            stream.ReadTimeout = readTimeout;
+            return true;
+        }
+        catch (Exception ex)
+        {
+            Dispose();
+            throw new ModbusException("connection error", ex);
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (!disposed)
+        {
+            if (disposing)
+            {
+                stream?.Close();
+                stream?.Dispose();
+                tcpClient?.Close();
+                tcpClient?.Dispose();
+            }
+            stream = null;
+            tcpClient = null;
+            disposed = true;
+        }
+    }
+
+    private byte[] Write(byte functionCode, UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+        => Write(functionCode, startingAddress, quantity, [], cancellationToken);
+    private byte[] Write(byte functionCode, UInt16 startingAddress, UInt16 quantity, byte[] multipleData, CancellationToken? cancellationToken = null)
+    {
+        try
+        {
+            if (!IsConnected || stream is null)
+            {
+                if (!Connect() || !IsConnected || stream is null) throw new ConnectionAbortedException();
+            }
+
+            transactionIdentifierInternal++;
+            byte[] writeData = new byte[12 + multipleData.Length];
+            var dataLength = multipleData.Length + 6;
+            writeData[0] = (byte)(transactionIdentifierInternal >> 8);
+            writeData[1] = (byte)(transactionIdentifierInternal & 0xFF);
+            writeData[2] = 0x00;
+            writeData[3] = 0x00;
+            writeData[4] = (byte)(dataLength >> 8);
+            writeData[5] = (byte)(dataLength & 0xFF);
+            writeData[6] = ClientId;
+            writeData[7] = functionCode;
+            writeData[8] = (byte)(startingAddress >> 8);
+            writeData[9] = (byte)(startingAddress & 0xFF);
+            writeData[10] = (byte)(quantity >> 8);
+            writeData[11] = (byte)(quantity & 0xFF);
+            if (multipleData.Length > 0)
+            {
+                Array.Copy(multipleData, 0, writeData, 12, multipleData.Length);
+            }
+
+            stream.Write(writeData, 0, writeData.Length);
+
+            byte[] readData = new byte[256];
+            int NumberOfBytes = stream.Read(readData, 0, readData.Length);
+            int attempts = 0;
+            const int maxAttempts = writeTimeout / 10;
+            while (NumberOfBytes == 0 && attempts ++ < maxAttempts)
+            {
+                cancellationToken?.ThrowIfCancellationRequested();
+                NumberOfBytes = stream.Read(readData, 0, readData.Length);
+                if (NumberOfBytes == 0) Thread.Sleep(10);
+            }
+            if (NumberOfBytes == 0) throw new TimeoutException("No response from server");
+            if (writeData[0] != readData[0] || writeData[1] != readData[1]) throw new ModbusException("Transaction Identifier not match");
+            if (writeData[2] != readData[2] || writeData[3] != readData[3]) throw new ModbusException("Protocol Identifier not match");
+            if (writeData[6] != readData[6]) throw new ModbusException("Client ID not match");
+            if (writeData[7] + 0x80 == readData[7])
+            {
+                throw readData[8] switch
+                {
+                    0x01 => new ModbusException("Function code not supported by master"),
+                    0x02 => new ModbusException("Starting address invalid or starting address + quantity invalid"),
+                    0x03 => new ModbusException("quantity invalid"),
+                    0x04 => new ModbusException("error reading"),
+                    _ => new ModbusException($"Function code error: 0x{(int)readData[7]:X2}"),
+                };
+            }
+            else if (writeData[7] != readData[7])
+            {
+                throw new Exception("Function code not match");
+            }
+            dataLength = readData[4] << 8;
+            dataLength += readData[5];
+            if (dataLength != NumberOfBytes - 6) throw new Exception("Length Field not match");
+
+            var receiveData = new byte[NumberOfBytes - 9];
+            Array.Copy(readData, 9, receiveData, 0, receiveData.Length);
+            return receiveData;
+        }
+        catch (Exception ex) when (!(ex is ModbusException || ex is TimeoutException))
+        {
+            throw new ModbusException("Communication error", ex);
+        }
+    }
+
+    public bool[] ReadDiscreteInputs(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 2000)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 2000");
+        }
+        var data = Write(0x02, startingAddress, quantity, cancellationToken);
+        if (data.Length - 1 < ((quantity - 1) / 8)) return [];
+        bool[] response = new bool[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            int intData = data[i / 8];
+            int mask = Convert.ToInt32(Math.Pow(2, (i % 8)));
+            response[i] = Convert.ToBoolean((intData & mask) / mask);
+        }
+        return response;
+    }
+
+    public bool[] ReadCoils(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 2000)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 2000");
+        }
+        var data = Write(0x01, startingAddress, quantity, cancellationToken);
+        if (data.Length - 1 < ((quantity - 1) / 8)) return [];
+        bool[] response = new bool[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            int intData = data[i / 8];
+            int mask = Convert.ToInt32(Math.Pow(2, (i % 8)));
+            response[i] = Convert.ToBoolean((intData & mask) / mask);
+        }
+        return (response);
+    }
+
+    public int[] ReadHoldingRegisters(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 125)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 125");
+        }
+        var data = Write(0x03, startingAddress, quantity, cancellationToken);
+        if (data.Length < quantity * 2) return [];
+        int[] response = new int[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            response[i] = (data[i * 2] << 8) | data[i * 2 + 1];
+        }
+        return (response);
+    }
+
+    public int[] ReadInputRegisters(UInt16 startingAddress, UInt16 quantity, CancellationToken? cancellationToken = null)
+    {
+        if (startingAddress > 65535 | quantity > 125)
+        {
+            throw new ArgumentException("Starting address must be 0 - 65535; quantity must be 0 - 125");
+        }
+        var data = Write(0x04, startingAddress, quantity, cancellationToken);
+        if (data.Length < quantity * 2) return [];
+        int[] response = new int[quantity];
+        for (int i = 0; i < quantity; i++)
+        {
+            response[i] = (data[i * 2] << 8) | data[i * 2 + 1];
+        }
+        return (response);
+    }
+
+    public void WriteSingleCoil(UInt16 startingAddress, bool value, CancellationToken? cancellationToken = null)
+    {
+        Write(0x05, startingAddress, value ? (UInt16)0xFF00 : (UInt16)0x0000, cancellationToken);
+    }
+
+    public void WriteSingleRegister(UInt16 startingAddress, UInt16 value, CancellationToken? cancellationToken = null)
+    {
+        Write(0x06, startingAddress, value, cancellationToken);
+    }
+
+    public void WriteMultipleCoils(UInt16 startingAddress, bool[] values, CancellationToken? cancellationToken = null)
+    {
+        if (values == null || values.Length == 0)
+            throw new ArgumentException("Values cannot be null or empty", nameof(values));
+        if (values.Length > 1968)
+            throw new ArgumentException("Too many coils (max 1968)", nameof(values));
+        if (startingAddress > 65535)
+            throw new ArgumentException("Starting address must be 0-65535", nameof(startingAddress));
+        if (startingAddress + values.Length > 65536)
+            throw new ArgumentException("Address range exceeds 65535", nameof(startingAddress));
+
+        byte byteCount = (byte)((values.Length + 7) / 8);
+        var data = new byte[1 + byteCount];
+        data[0] = byteCount;
+        for (int i = 0; i < values.Length; i++)
+        {
+            if (values[i])
+            {
+                data[1 + (i / 8)] |= (byte)(1 << (i % 8));
+            }
+        }
+        Write(0x0F, startingAddress, (UInt16)values.Length, data, cancellationToken);
+    }
+
+    public void WriteMultipleRegisters(UInt16 startingAddress, UInt16[] values, CancellationToken? cancellationToken = null)
+    {
+        if (values == null || values.Length == 0)
+            throw new ArgumentException("Values cannot be null or empty", nameof(values));
+        if (values.Length > 123)
+            throw new ArgumentException("Too many registers (max 123)", nameof(values));
+        if (startingAddress > 65535)
+            throw new ArgumentException("Starting address must be 0-65535", nameof(startingAddress));
+        if (startingAddress + values.Length > 65536)
+            throw new ArgumentException("Address range exceeds 65535", nameof(startingAddress));
+
+        byte byteCount = (byte)(values.Length * 2);
+        var data = new byte[1 + byteCount];
+        data[0] = byteCount;
+        for (int i = 0; i < values.Length; i++)
+        {
+            data[1 + i * 2] = (byte)(values[i] >> 8);      
+            data[2 + i * 2] = (byte)(values[i] & 0xFF); 
+        }
+        Write(0x10, startingAddress, (UInt16)values.Length, data, cancellationToken);
+    }
+
+    public int[] ReadWriteMultipleRegisters(UInt16 startingAddressRead, UInt16 quantityRead, UInt16 startingAddressWrite, UInt16[] values, CancellationToken? cancellationToken = null)
+    {
+        if (values == null || values.Length == 0)
+            throw new ArgumentException("Values cannot be null or empty", nameof(values));
+        if (quantityRead == 0 || quantityRead > 125)
+            throw new ArgumentException("Read quantity must be 1-125", nameof(quantityRead));
+        if (values.Length > 121)
+            throw new ArgumentException("Write quantity must be 1-121", nameof(values));
+        if (startingAddressRead > 65535 || startingAddressWrite > 65535)
+            throw new ArgumentException("Addresses must be 0-65535");
+        if (startingAddressRead + quantityRead > 65536 || startingAddressWrite + values.Length > 65536)
+            throw new ArgumentException("Address ranges exceed 65535");
+
+        var writeData = new byte[5 + values.Length * 2];
+        writeData[0] = (byte)(startingAddressWrite >> 8);
+        writeData[1] = (byte)(startingAddressWrite & 0xFF);
+        writeData[2] = (byte)(values.Length >> 8);
+        writeData[3] = (byte)(values.Length & 0xFF);
+        writeData[4] = (byte)(values.Length * 2);
+        for (int i = 0; i < values.Length; i++)
+        {
+            writeData[5 + i * 2] = (byte)(values[i] >> 8);
+            writeData[6 + i * 2] = (byte)(values[i] & 0xFF);
+        }
+        var receivedData = Write(0x17, startingAddressRead, quantityRead, writeData, cancellationToken);
+        if (receivedData.Length < quantityRead * 2) return [];
+        var response = new int[quantityRead];
+        for (int i = 0; i < quantityRead; i++)
+        {
+            response[i] = (receivedData[i * 2] << 8) | receivedData[i * 2 + 1];
+        }
+        return response;
+    }
+
+    public bool Available(int timeout)
+    {
+        System.Net.NetworkInformation.Ping pingSender = new ();
+        IPAddress address = System.Net.IPAddress.Parse(IpAddress);
+
+        string data = "phenikaaX";
+        byte[] buffer = System.Text.Encoding.ASCII.GetBytes(data);
+
+        System.Net.NetworkInformation.PingReply reply = pingSender.Send(address, timeout, buffer);
+
+        return reply.Status == System.Net.NetworkInformation.IPStatus.Success;
+    }
+
+}

RobotApp/Modbus/RegisterOrder.cs

@@ -0,0 +1,8 @@
+namespace RobotApp.Modbus;
+
+
+public enum RegisterOrder 
+{
+    LowHigh = 0,
+    HighLow = 1 
+}

RobotApp/Program.cs

@@ -6,6 +6,7 @@ using RobotApp.Client.Pages;
 using RobotApp.Components;
 using RobotApp.Components.Account;
 using RobotApp.Data;
+using RobotApp.Services.Robot;
 
 var builder = WebApplication.CreateBuilder(args);
 
@@ -25,8 +26,10 @@ builder.Services.AddAuthorization();
 var connectionString = builder.Configuration.GetConnectionString("DefaultConnection")
     ?? throw new InvalidOperationException("Connection string 'DefaultConnection' not found.");
 
-builder.Services.AddDbContext<ApplicationDbContext>(options =>
+var connectionString = builder.Configuration.GetConnectionString("DefaultConnection") ?? throw new InvalidOperationException("Connection string 'DefaultConnection' not found.");
-    options.UseSqlServer(connectionString));
+Action<DbContextOptionsBuilder> appDbOptions = options => options.UseSqlite(connectionString, b => b.MigrationsAssembly("RobotApp"));
+
+builder.Services.AddDbContext<ApplicationDbContext>(appDbOptions);
 builder.Services.AddDatabaseDeveloperPageExceptionFilter();
 
 builder.Services.AddIdentity<ApplicationUser, ApplicationRole>(options =>
@@ -42,6 +45,10 @@ builder.Services.AddIdentity<ApplicationUser, ApplicationRole>(options =>
 
 builder.Services.AddSingleton<IEmailSender<ApplicationUser>, IdentityNoOpEmailSender>();
 
+builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>));
+builder.Services.AddSingleton<RobotConnection>();
+builder.Services.AddHostedService(sp => sp.GetRequiredService<RobotConnection>());
+
 var app = builder.Build();
 
 // Configure the HTTP request pipeline.

RobotApp/RobotApp.csproj

@@ -9,19 +9,23 @@
 
   <ItemGroup>
     <ProjectReference Include="..\RobotApp.Client\RobotApp.Client.csproj" />
+    <ProjectReference Include="..\RobotApp.Common.Shares\RobotApp.Common.Shares.csproj" />
+    <ProjectReference Include="..\RobotApp.VDA5050\RobotApp.VDA5050.csproj" />
     <PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Server" Version="9.0.9" />
     <PackageReference Include="Microsoft.AspNetCore.Diagnostics.EntityFrameworkCore" Version="9.0.9" />
     <PackageReference Include="Microsoft.AspNetCore.Identity.EntityFrameworkCore" Version="9.0.9" />
+    <PackageReference Include="Microsoft.EntityFrameworkCore.Sqlite" Version="9.0.9" />
     <PackageReference Include="Microsoft.EntityFrameworkCore.SqlServer" Version="9.0.9" />
     <PackageReference Include="Microsoft.EntityFrameworkCore.Tools" Version="9.0.9">
       <PrivateAssets>all</PrivateAssets>
       <IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
     </PackageReference>
-    <PackageReference Include="MudBlazor" Version="8.12.0" />
+    <PackageReference Include="MQTTnet" Version="5.0.1.1416" />
   </ItemGroup>
 
   <ItemGroup>
     <Folder Include="Layout\" />
+    <Folder Include="Services\Robot\Simulation\" />
     <Folder Include="wwwroot\lib\" />
   </ItemGroup>
 

RobotApp/Services/Exceptions/ActionException.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Services.Exceptions;
+
+public class ActionException : OrderException
+{
+}

RobotApp/Services/Exceptions/OrderException.cs

@@ -0,0 +1,15 @@
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Exceptions;
+
+public class OrderException : Exception
+{
+    public OrderException(string message) : base(message) { }
+    public OrderException(string message, Exception inner) : base(message, inner) { }
+    public OrderException() : base() { }
+    public OrderException(Error error) : base()
+    {
+        Error = error;
+    }
+    public Error? Error { get; set; }
+}

RobotApp/Services/Logger.cs

@@ -0,0 +1,108 @@
+namespace RobotApp.Services;
+
+public class Logger<T>(ILogger<T> Logger) where T : class
+{
+    public event Action? LoggerUpdate;
+    public void Write(string message, LogLevel level)
+    {
+        switch (level)
+        {
+            case LogLevel.Trace:
+                Logger.LogTrace("{mes}", message);
+                break;
+            case LogLevel.Debug:
+                Logger.LogDebug("{mes}", message);
+                break;
+            case LogLevel.Information:
+                Logger.LogInformation("{mes}", message);
+                break;
+            case LogLevel.Warning:
+                Logger.LogWarning("{mes}", message);
+                break;
+            case LogLevel.Error:
+                Logger.LogError("{mes}", message);
+                break;
+            case LogLevel.Critical:
+                Logger.LogCritical("{mes}", message);
+                break;
+        }
+        LoggerUpdate?.Invoke();
+    }
+
+    public void Write(string message)
+    {
+        Write(message, LogLevel.Information);
+    }
+
+    public async Task WriteAsync(string message)
+    {
+        var write = Task.Run(() => Write(message));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task TraceAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Trace));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task DebugAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Debug));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task InfoAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Information));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task WarningAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Warning));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task ErrorAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Error));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public async Task CriticalAsync(string message)
+    {
+        var write = Task.Run(() => Write(message, LogLevel.Critical));
+        await write.WaitAsync(CancellationToken.None);
+    }
+
+    public void Trace(string message)
+    {
+        Write(message, LogLevel.Trace);
+    }
+
+    public void Debug(string message)
+    {
+        Write(message, LogLevel.Debug);
+    }
+
+    public void Info(string message)
+    {
+        Write(message, LogLevel.Information);
+    }
+
+    public void Warning(string message)
+    {
+        Write(message, LogLevel.Warning);
+    }
+
+    public void Error(string message)
+    {
+        Write(message, LogLevel.Error);
+    }
+
+    public void Critical(string message)
+    {
+        Write(message, LogLevel.Critical);
+    }
+}

RobotApp/Services/MQTTClient.cs

@@ -0,0 +1,160 @@
+using MQTTnet;
+using MQTTnet.Protocol;
+using RobotApp.Common.Shares;
+using RobotApp.VDA5050;
+using System.Text;
+
+namespace RobotApp.Services;
+
+public class MQTTClient : IAsyncDisposable
+{
+    private readonly MqttClientFactory MqttClientFactory;
+    private readonly MqttClientOptions MqttClientOptions;
+    private readonly MqttClientSubscribeOptions MqttClientSubscribeOptions;
+    private IMqttClient? MqttClient;
+
+    private readonly Logger<MQTTClient> Logger;
+    private readonly VDA5050Setting VDA5050Setting;
+    private bool IsReconnecing;
+
+    public event Action<string>? OrderChanged;
+    public event Action<string>? InstanceActionsChanged;
+    public bool IsConnected => !IsReconnecing && MqttClient is not null && MqttClient.IsConnected;
+
+    public MQTTClient(string clientId, VDA5050Setting setting, Logger<MQTTClient> logger)
+    {
+        VDA5050Setting = setting;
+        Logger = logger;
+
+        MqttClientFactory = new MqttClientFactory();
+        MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
+                            .WithTcpServer(setting.HostServer, setting.Port)
+                            .WithCredentials(setting.UserName, setting.Password)
+                            .WithClientId(clientId)
+                            .WithCleanSession(true)
+                            .Build();
+        MqttClientSubscribeOptions = MqttClientFactory.CreateSubscribeOptionsBuilder()
+                            .WithTopicFilter(f => f.WithTopic(VDA5050Topic.ORDER.ToTopicString()))
+                            .WithTopicFilter(f => f.WithTopic(VDA5050Topic.INSTANTACTIONS.ToTopicString()))
+                            .Build();
+    }
+
+    public async Task ConnectAsync(CancellationToken cancellationToken = default)
+    {
+        MqttClient = MqttClientFactory.CreateMqttClient();
+        MqttClient.DisconnectedAsync += async delegate (MqttClientDisconnectedEventArgs args)
+        {
+            if (args.ClientWasConnected && !IsReconnecing)
+            {
+                IsReconnecing = true;
+                Logger.Warning("Mất kết nối tới broker, đang cố gắng kết nối lại...");
+                if (MqttClient.IsConnected) await MqttClient.DisconnectAsync();
+                MqttClient.Dispose();
+
+                await ConnectAsync();
+                await SubscribeAsync();
+                IsReconnecing = false;
+            }
+        };
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                var connection = await MqttClient.ConnectAsync(MqttClientOptions, cancellationToken);
+                if (connection.ResultCode != MqttClientConnectResultCode.Success || !MqttClient.IsConnected)
+                    Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");
+                else
+                {
+                    Logger.Info("Kết nối tới broker thành công");
+                    break;
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Error($"Lỗi khi tạo MQTT client: {ex.Message}");
+            }
+            await Task.Delay(3000, cancellationToken);
+        }
+    }
+
+    public async Task SubscribeAsync(CancellationToken cancellationToken = default)
+    {
+        if (MqttClient is null) throw new Exception("Kết nối tới broker chưa được khởi tạo nhưng đã yêu cầu subscribe");
+        if(!MqttClient.IsConnected) throw new Exception("Kết nối tới broker chưa thành công nhưng đã yêu cầu subscribe");
+
+        MqttClient.ApplicationMessageReceivedAsync += delegate (MqttApplicationMessageReceivedEventArgs args)
+        {
+            var stringData = Encoding.UTF8.GetString(args.ApplicationMessage.Payload);
+            VDA5050Topic topic = EnumExtensions.ToTopic(args.ApplicationMessage.Topic);
+            if (topic == VDA5050Topic.ORDER) OrderChanged?.Invoke(stringData);
+            else if (topic == VDA5050Topic.INSTANTACTIONS) InstanceActionsChanged?.Invoke(stringData);
+            return Task.CompletedTask;
+        };
+
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                var response = await MqttClient.SubscribeAsync(MqttClientSubscribeOptions, cancellationToken);
+                bool isSuccess = true;
+                foreach (var item in response.Items)
+                {
+                    if (item.ResultCode == MqttClientSubscribeResultCode.GrantedQoS0 ||
+                        item.ResultCode == MqttClientSubscribeResultCode.GrantedQoS1 ||
+                        item.ResultCode == MqttClientSubscribeResultCode.GrantedQoS2)
+                    {
+                        Logger.Info($"Subscribe thành công cho topic: {item.TopicFilter.Topic} với QoS: {item.ResultCode}");
+                    }
+                    else
+                    {
+                        Logger.Warning($"Subscribe thất bại cho topic: {item.TopicFilter.Topic}. Lý do: {response.ReasonString}");
+                        isSuccess = false;
+                        break;
+                    }
+                }
+                if (isSuccess) break;
+            }
+            catch (Exception ex)
+            {
+                Logger.Error($"Lỗi khi subscribe: {ex.Message}");
+            }
+            await Task.Delay(3000, cancellationToken);
+        }
+    }
+
+    public async Task<MessageResult> PublishAsync(string topic, string data)
+    {
+        var repeat = VDA5050Setting.PublishRepeat;
+        while (repeat-- > 0)
+        {
+            try
+            {
+                var applicationMessage = new MqttApplicationMessageBuilder()
+                                .WithTopic(topic)
+                                .WithPayload(data)
+                                .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
+                                .Build();
+                if (MqttClient is null || !IsConnected) return new(false, "Chưa có kết nối tới broker");
+                var publish = await MqttClient.PublishAsync(applicationMessage);
+                if (!publish.IsSuccess) continue;
+                return new(true);
+            }
+            catch (Exception ex)
+            {
+                Logger.Error($"Lỗi khi publish MQTT: {ex.Message}");
+            }
+        }
+        return new(false, "Không thể publish tới broker");
+    }
+
+    public async ValueTask DisposeAsync()
+    {
+        if (MqttClient is not null)
+        {
+            if (MqttClient.IsConnected) await MqttClient.DisconnectAsync();
+            MqttClient.Dispose();
+            MqttClient = null;
+        }
+        GC.SuppressFinalize(this);
+    }
+}

RobotApp/Services/Robot/Navigation/Algorithm/FuzzyLogic.cs

@@ -0,0 +1,252 @@
+namespace RobotApp.Services.Robot.Navigation.Algorithm;
+
+public class FuzzyLogic
+{
+    private double Gain_P = 0.5;
+    private double Gain_I = 0.01;
+    private double DiscreteTimeIntegrator_DSTATE;
+
+    public FuzzyLogic WithGainP(double gainP)
+    {
+        Gain_P = gainP;
+        return this;
+    }
+
+    public FuzzyLogic WithGainI(double gainI)
+    {
+        Gain_I = gainI;
+        return this;
+    }
+
+    private static double Fuzzy_trapmf(double x, double[] parame)
+    {
+        double b_y1;
+        double y2;
+        b_y1 = 0.0;
+        y2 = 0.0;
+        if (x >= parame[1])
+        {
+            b_y1 = 1.0;
+        }
+        if (x < parame[0])
+        {
+            b_y1 = 0.0;
+        }
+        if (parame[0] <= x && x < parame[1] && parame[0] != parame[1])
+        {
+            b_y1 = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
+        }
+        if (x <= parame[2])
+        {
+            y2 = 1.0;
+        }
+        if (x > parame[3])
+        {
+            y2 = 0.0;
+        }
+        if (parame[2] < x && x <= parame[3] && parame[2] != parame[3])
+        {
+            y2 = 1.0 / (parame[3] - parame[2]) * (parame[3] - x);
+        }
+        return b_y1 < y2 ? b_y1 : y2;
+    }
+
+    private static double Fuzzy_trimf(double x, double[] parame)
+    {
+        double y;
+        y = 0.0;
+        if (parame[0] != parame[1] && parame[0] < x && x < parame[1])
+        {
+            y = 1.0 / (parame[1] - parame[0]) * (x - parame[0]);
+        }
+        if (parame[1] != parame[2] && parame[1] < x && x < parame[2])
+        {
+            y = 1.0 / (parame[2] - parame[1]) * (parame[2] - x);
+        }
+        if (x == parame[1])
+        {
+            y = 1.0;
+        }
+        return y;
+    }
+
+    public (double wl, double wr) Fuzzy_step(double v, double w, double TimeSample)
+    {
+        (double wl, double wr) result = new();
+        double[] inputMFCache = new double[10];
+        double[] outputMFCache = new double[5];
+        double[] outputMFCache_0 = new double[5];
+        double[] tmp = new double[3];
+        double aggregatedOutputs;
+        double rtb_TmpSignalConversionAtSFun_0;
+        double rtb_antecedentOutputs_e;
+        double sumAntecedentOutputs;
+        int ruleID;
+        double[] f = [-1.0E+10, -1.0E+10, -1.0, -0.5];
+        double[] e = [0.5, 1.0, 1.0E+10, 1.0E+10];
+        double[] d = [0.75, 1.0, 1.0E+9, 1.0E+9];
+        double[] c = [-1.0E+9, -1.0E+9, 0.0, 0.25];
+        byte[] b = [ 1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
+                         4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 1, 2, 3,
+                         4, 5, 1, 2, 3, 4, 5, 3, 4, 5, 1, 2, 1, 2, 3, 4, 5 ];
+        byte[] b_0 = [1, 1, 2, 1, 1, 2, 3, 5, 1, 4, 5, 5, 5, 5, 5, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5];
+        byte[] b_1 = [  1, 1, 1, 1, 1, 2, 2, 2, 2, 2, 3, 3, 3, 3, 3, 4,
+                            4, 4, 4, 4, 5, 5, 5, 5, 5, 1, 2, 3, 4, 5, 4, 1,
+                            2, 3, 5, 3, 1, 2, 4, 5, 1, 2, 3, 4, 5, 1, 2, 4, 5, 3 ];
+        byte[] b_2 = [5, 5, 5, 5, 5, 1, 2, 3, 5, 4, 2, 1, 1, 1, 1, 5, 5, 5, 5, 5, 1, 1, 1, 1, 2];
+        double inputMFCache_tmp;
+        double inputMFCache_tmp_0;
+        double inputMFCache_tmp_1;
+        double inputMFCache_tmp_2;
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller' */
+        /* SignalConversion generated from: '<S4>/ SFunction ' incorporates:
+         *  Constant: '<Root>/w'
+         *  DiscreteIntegrator: '<Root>/Discrete-Time Integrator'
+         *  Gain: '<Root>/Gain1'
+         *  MATLAB Function: '<S1>/Evaluate Rule Antecedents'
+         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
+         *  SignalConversion generated from: '<S7>/ SFunction '
+         *  Sum: '<Root>/Sum'
+         */
+        DiscreteTimeIntegrator_DSTATE += Gain_I * w * TimeSample;
+        rtb_TmpSignalConversionAtSFun_0 = Gain_P * w + DiscreteTimeIntegrator_DSTATE;
+        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        /* MATLAB Function: '<S1>/Evaluate Rule Antecedents' incorporates:
+         *  Constant: '<Root>/v'
+         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
+         *  SignalConversion generated from: '<S4>/ SFunction '
+         */
+        sumAntecedentOutputs = 0.0;
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache_tmp = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, f);
+        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache[0] = inputMFCache_tmp;
+        tmp[0] = -0.5;
+        tmp[1] = 0.0;
+        tmp[2] = 0.5;
+        inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache_tmp_0 = Fuzzy_trapmf(rtb_TmpSignalConversionAtSFun_0, e);
+        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache[2] = inputMFCache_tmp_0;
+        tmp[0] = -1.0;
+        tmp[1] = -0.5;
+        tmp[2] = 0.0;
+        inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
+        tmp[0] = 0.0;
+        tmp[1] = 0.5;
+        tmp[2] = 1.0;
+        inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
+        tmp[0] = 0.0;
+        tmp[1] = 0.25;
+        tmp[2] = 0.5;
+        inputMFCache[5] = Fuzzy_trimf(v, tmp);
+        tmp[0] = 0.25;
+        tmp[1] = 0.5;
+        tmp[2] = 0.75;
+        inputMFCache[6] = Fuzzy_trimf(v, tmp);
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache_tmp_1 = Fuzzy_trapmf(v, d);
+        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache[7] = inputMFCache_tmp_1;
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache_tmp_2 = Fuzzy_trapmf(v, c);
+        /* End of Outputs for SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        inputMFCache[8] = inputMFCache_tmp_2;
+        tmp[0] = 0.5;
+        tmp[1] = 0.75;
+        tmp[2] = 1.0;
+        inputMFCache[9] = Fuzzy_trimf(v, tmp);
+        /* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
+        aggregatedOutputs = 0.0;
+        outputMFCache[0] = 0.0;
+        outputMFCache[1] = 0.25;
+        outputMFCache[2] = 0.5;
+        outputMFCache[3] = 0.75;
+        outputMFCache[4] = 1.0;
+        for (ruleID = 0; ruleID < 25; ruleID++)
+        {
+            /* MATLAB Function: '<S1>/Evaluate Rule Antecedents' */
+            rtb_antecedentOutputs_e = inputMFCache[b[ruleID + 25] + 4] * inputMFCache[b[ruleID] - 1];
+            sumAntecedentOutputs += rtb_antecedentOutputs_e;
+            /* MATLAB Function: '<S1>/Evaluate Rule Consequents' */
+            aggregatedOutputs += outputMFCache[b_0[ruleID] - 1] * rtb_antecedentOutputs_e;
+        }
+        /* MATLAB Function: '<S1>/Defuzzify Outputs' incorporates:
+         *  MATLAB Function: '<S1>/Evaluate Rule Antecedents'
+         *  MATLAB Function: '<S1>/Evaluate Rule Consequents'
+         */
+        if (sumAntecedentOutputs == 0.0)
+        {
+            result.wr = 0.5;
+        }
+        else
+        {
+            result.wr = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
+        }
+        /* Outputs for Atomic SubSystem: '<Root>/Fuzzy Logic  Controller1' */
+        /* MATLAB Function: '<S2>/Evaluate Rule Antecedents' incorporates:
+         *  Constant: '<Root>/v'
+         *  SignalConversion generated from: '<S7>/ SFunction '
+         */
+        sumAntecedentOutputs = 0.0;
+        inputMFCache[0] = inputMFCache_tmp;
+        tmp[0] = -0.5;
+        tmp[1] = 0.0;
+        tmp[2] = 0.5;
+        inputMFCache[1] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
+        inputMFCache[2] = inputMFCache_tmp_0;
+        tmp[0] = -1.0;
+        tmp[1] = -0.5;
+        tmp[2] = 0.0;
+        inputMFCache[3] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
+        tmp[0] = 0.0;
+        tmp[1] = 0.5;
+        tmp[2] = 1.0;
+        inputMFCache[4] = Fuzzy_trimf(rtb_TmpSignalConversionAtSFun_0, tmp);
+        tmp[0] = 0.0;
+        tmp[1] = 0.25;
+        tmp[2] = 0.5;
+        inputMFCache[5] = Fuzzy_trimf(v, tmp);
+        tmp[0] = 0.25;
+        tmp[1] = 0.5;
+        tmp[2] = 0.75;
+        inputMFCache[6] = Fuzzy_trimf(v, tmp);
+        inputMFCache[7] = inputMFCache_tmp_1;
+        inputMFCache[8] = inputMFCache_tmp_2;
+        tmp[0] = 0.5;
+        tmp[1] = 0.75;
+        tmp[2] = 1.0;
+        inputMFCache[9] = Fuzzy_trimf(v, tmp);
+        /* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
+        aggregatedOutputs = 0.0;
+        outputMFCache_0[0] = 0.0;
+        outputMFCache_0[1] = 0.25;
+        outputMFCache_0[2] = 0.5;
+        outputMFCache_0[3] = 0.75;
+        outputMFCache_0[4] = 1.0;
+        for (ruleID = 0; ruleID < 25; ruleID++)
+        {
+            /* MATLAB Function: '<S2>/Evaluate Rule Antecedents' */
+            rtb_antecedentOutputs_e = inputMFCache[b_1[ruleID + 25] + 4] * inputMFCache[b_1[ruleID] - 1];
+            sumAntecedentOutputs += rtb_antecedentOutputs_e;
+            /* MATLAB Function: '<S2>/Evaluate Rule Consequents' */
+            aggregatedOutputs += outputMFCache_0[b_2[ruleID] - 1] *
+              rtb_antecedentOutputs_e;
+        }
+        /* MATLAB Function: '<S2>/Defuzzify Outputs' incorporates:
+         *  MATLAB Function: '<S2>/Evaluate Rule Antecedents'
+         *  MATLAB Function: '<S2>/Evaluate Rule Consequents'
+         */
+        if (sumAntecedentOutputs == 0.0)
+        {
+            result.wl = 0.5;
+        }
+        else
+        {
+            result.wl = 1.0 / sumAntecedentOutputs * aggregatedOutputs;
+        }
+        return result;
+    }
+}

RobotApp/Services/Robot/Navigation/Algorithm/PID.cs

@@ -0,0 +1,47 @@
+namespace RobotApp.Services.Robot.Navigation.Algorithm;
+
+public class PID
+{
+    private double Kp = 0.3;
+    private double Ki = 0.0001;
+    private double Kd = 0.01;
+    private double Pre_Error;
+    private double Pre_Pre_Error;
+    private double Pre_Out;
+
+    private DateTime PreTime;
+
+    public PID WithKp(double kp)
+    {
+        Kp = kp;
+        return this;
+    }
+
+    public PID WithKi(double ki)
+    {
+        Ki = ki;
+        return this;
+    }
+
+    public PID WithKd(double kd)
+    {
+        Kd = kd;
+        return this;
+    }
+
+    public double PID_step(double error, double min, double max)
+    {
+        DateTime CurrentTime = DateTime.Now;
+        double TimeSample = (CurrentTime - PreTime).TotalSeconds;
+        double P_part = Kp * (error - Pre_Error);
+        double I_part = 0.5 * Ki * TimeSample * (error + Pre_Error);
+        double D_part = Kd / TimeSample * (error - 2 * Pre_Error + Pre_Pre_Error);
+        double Out = Pre_Out + P_part + I_part + D_part;
+        Pre_Pre_Error = Pre_Error;
+        Pre_Error = error;
+        Pre_Out = Out;
+        Out = Math.Clamp(Out, min, max);
+        PreTime = CurrentTime;
+        return Out;
+    }
+}

RobotApp/Services/Robot/Navigation/Algorithm/PurePursuit.cs

@@ -0,0 +1,156 @@
+//namespace RobotApp.Services.Navigation.Algorithm;
+//public class PurePursuit
+//{
+//    private double MaxAngularVelocity = 1.5;
+//    private double LookaheadDistance = 0.5;
+//    private List<NavigationNode> Waypoints_Value = [];
+//    public int OnNodeIndex = 0;
+//    public int GoalIndex = 0;
+//    public NavigationNode? Goal;
+
+//    public PurePursuit WithLookheadDistance(double distance)
+//    {
+//        LookaheadDistance = distance;
+//        return this;
+//    }
+
+//    public PurePursuit WithMaxAngularVelocity(double vel)
+//    {
+//        MaxAngularVelocity = vel;
+//        return this;
+//    }
+
+//    public PurePursuit WithPath(NavigationNode[] path)
+//    {
+//        Waypoints_Value = [.. path];
+//        return this;
+//    }
+
+//    public void UpdatePath(NavigationNode[] path)
+//    {
+//        Waypoints_Value = [.. path];
+//    }
+
+//    public void UpdateGoal(NavigationNode goal)
+//    {
+//        Goal = goal;
+//        GoalIndex = Waypoints_Value.IndexOf(Goal);
+//    }
+
+//    public (NavigationNode node, int index) GetOnNode(double x, double y)
+//    {
+//        double minDistance = double.MaxValue;
+//        NavigationNode onNode = Waypoints_Value[0];
+//        int index = 0;
+//        for (int i = 1; i < Waypoints_Value.Count; i++)
+//        {
+//            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
+//            if (distance < minDistance)
+//            {
+//                onNode = Waypoints_Value[i];
+//                minDistance = distance;
+//                index = i;
+//            }
+//        }
+//        return (onNode, index);
+//    }
+
+//    private (NavigationNode? node, int index) OnNode(double x, double y)
+//    {
+//        if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
+//        double minDistance = double.MaxValue;
+//        int index = 0;
+//        NavigationNode? onNode = null;
+//        if (Goal is null) return (onNode, index);
+//        for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); i++)
+//        {
+//            var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
+//            if (distance < minDistance)
+//            {
+//                onNode = Waypoints_Value[i];
+//                minDistance = distance;
+//                index = i;
+//            }
+//        }
+//        return (onNode, index);
+//    }
+
+//    public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
+//    {
+//        if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
+//        NavigationNode? lookaheadStartPt = null;
+//        var (onNode, index) = OnNode(X_Ref, Y_Ref);
+//        if (onNode is null || Goal is null) return 0;
+//        OnNodeIndex = index;
+//        double lookDistance = 0;
+//        for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
+//        {
+//            lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
+//            if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
+//            {
+//                lookaheadStartPt = Waypoints_Value[i];
+//                break;
+//            }
+//        }
+//        lookaheadStartPt ??= Goal;
+//        if (onNode.Direction == RobotDirection.BACKWARD)
+//        {
+//            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
+//            else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
+//            Angle_Ref += Math.PI;
+//            if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
+//        }
+//        var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
+
+//        if (Math.Abs(distance) > Math.PI)
+//        {
+//            double minDistance;
+//            if (distance + Math.PI == 0.0) minDistance = 0.0;
+//            else
+//            {
+//                double data = (distance + Math.PI) / (2 * Math.PI);
+//                if (data < 0) data = Math.Round(data + 0.5);
+//                else data = Math.Round(data - 0.5);
+//                minDistance = distance + Math.PI - data * (2 * Math.PI);
+//                double checker = 0;
+//                if (minDistance != 0.0)
+//                {
+//                    checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
+//                }
+//                if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
+//                {
+//                    minDistance = 0.0;
+//                }
+//                else if (distance + Math.PI < 0.0)
+//                {
+//                    minDistance += Math.PI * 2;
+//                }
+//            }
+//            if (minDistance == 0.0 && distance + Math.PI > 0.0)
+//            {
+//                minDistance = Math.PI * 2;
+//            }
+//            distance = minDistance - Math.PI;
+//        }
+
+//        var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
+//        if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
+//        {
+//            if (AngularVelocity < 0.0)
+//            {
+//                AngularVelocity = -1.0;
+//            }
+//            else if (AngularVelocity > 0.0)
+//            {
+//                AngularVelocity = 1.0;
+//            }
+//            else if (AngularVelocity == 0.0)
+//            {
+//                AngularVelocity = 0.0;
+//            }
+//            AngularVelocity *= MaxAngularVelocity;
+//        }
+//        return AngularVelocity;
+//    }
+
+//}

RobotApp/Services/Robot/Navigation/DifferentialNavigation.cs

@@ -0,0 +1,22 @@
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Navigation;
+
+public class DifferentialNavigation(Logger<NavigationController> navLogger,
+                                    Logger<DifferentialNavigation> Logger,
+                                    IDriver Driver,
+                                    ISafety Safety,
+                                    ISensorIMU SensorIMU) : NavigationController(navLogger)
+{
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            // Implement differential drive navigation logic here
+        }
+        catch (Exception ex)
+        {
+            Logger.Write($"Error in DifferentialNavigation: {ex.Message}", LogLevel.Error);
+        }
+    }
+}

RobotApp/Services/Robot/Navigation/NavigationController.cs

@@ -0,0 +1,61 @@
+using RobotApp.Interfaces;
+using RobotApp.VDA5050.Order;
+
+namespace RobotApp.Services.Robot.Navigation;
+
+public class NavigationController(Logger<NavigationController> Logger) : INavigation
+{
+    public NavigationState State { get; private set; } = NavigationState.None;
+    public bool Driving { get; private set; }
+
+    protected const int CycleHandlerMilliseconds = 20;
+    private WatchTimer<NavigationController>? NavigationTimer;
+
+    protected Node[] Nodes = [];
+    protected Edge[] Edges = [];
+
+    protected void HandleNavigationStart()
+    {
+        NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
+        NavigationTimer.Start();
+    }
+
+    protected void HandleNavigationStop()
+    {
+        NavigationTimer?.Dispose();
+    }
+
+    protected virtual void NavigationHandler() { }
+
+    public void CancelMovement()
+    {
+    }
+
+    public void Move(Node[] nodes, Edge[] edges)
+    {
+        Nodes = nodes;
+        Edges = edges;
+        State = NavigationState.Initializing;
+    }
+
+    public void MoveStraight(double x, double y)
+    {
+    }
+
+    public void Paused()
+    {
+    }
+
+    public void Resume()
+    {
+    }
+
+    public void Rotate(double angle)
+    {
+    }
+
+    public void UpdateOrder(int lastBaseSequence)
+    {
+        State = NavigationState.Initialized;
+    }
+}

RobotApp/Services/Robot/Navigation/NavigationManager.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Services.Robot.Navigation
+{
+    public class NavigationManager
+    {
+    }
+}

RobotApp/Services/Robot/Navigation/NavigationNode.cs

@@ -0,0 +1,13 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.Robot.Navigation;
+
+public class NavigationNode
+{
+    public string NodeId { get; set; } = string.Empty;
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double Theta { get; set; }
+    public RobotDirection Direction { get; set; }
+    public string Description { get; set; } = string.Empty;
+}

RobotApp/Services/Robot/RobotAction.cs

@@ -0,0 +1,31 @@
+using RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotAction : IInstanceActions
+{
+    public ActionState[] ActionStates { get; private set; } = [];
+
+    public bool HasActionRunning => throw new NotImplementedException();
+
+    public bool AddInstanceAction(VDA5050.InstantAction.Action action)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool AddOrderActions(VDA5050.InstantAction.Action[] actions)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool StartAction(string actionId)
+    {
+        throw new NotImplementedException();
+    }
+
+    public bool StopAction()
+    {
+        throw new NotImplementedException();
+    }
+}

RobotApp/Services/Robot/RobotBattery.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Services.Robot
+{
+    public class RobotBattery
+    {
+    }
+}

RobotApp/Services/Robot/RobotConfiguration.cs

@@ -0,0 +1,9 @@
+using RobotApp.Common.Shares.Enums;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotConfiguration
+{
+    public static string SerialNumber { get; set; } = "Robot-Demo";
+    public static NavigationType NavigationType { get; set; } = NavigationType.Differential;
+}

RobotApp/Services/Robot/RobotConnection.cs

@@ -0,0 +1,69 @@
+using RobotApp.Common.Shares;
+using RobotApp.VDA5050;
+using RobotApp.VDA5050.InstantAction;
+using RobotApp.VDA5050.Order;
+using System.Text.Json;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotConnection(Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger) : BackgroundService
+{
+    private readonly VDA5050Setting VDA5050Setting = new()
+    {
+        HostServer = "172.20.235.170",
+        Port = 1886,
+    };
+    private MQTTClient? MqttClient;
+
+    public bool IsConnected => MqttClient is not null && MqttClient.IsConnected;
+    public readonly static string SerialNumber = "RobotDemo";
+
+    private void OrderChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<OrderMsg>(data);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Nhận Order xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    private void InstanceActionsChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<InstantActionsMsg>(data);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Nhận InstanceActions xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    public async Task<MessageResult> Publish(string topic, string data)
+    {
+        if (MqttClient is not null && MqttClient.IsConnected) return await MqttClient.PublishAsync(topic, data);
+        return new(false, "Chưa có kết nối tới broker");
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+
+        MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger);
+        MqttClient.OrderChanged += OrderChanged;
+        MqttClient.InstanceActionsChanged += InstanceActionsChanged;
+        await MqttClient.ConnectAsync(stoppingToken);
+        await MqttClient.SubscribeAsync(stoppingToken);
+    }
+
+    public override async Task StopAsync(CancellationToken cancellationToken)
+    {
+        if(MqttClient is not null) await MqttClient.DisposeAsync();
+        await base.StopAsync(cancellationToken);
+    }
+}

RobotApp/Services/Robot/RobotController.cs

@@ -0,0 +1,96 @@
+using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotController(IOrder OrderManager,
+                             INavigation NavigationManager,
+                             IInstanceActions ActionManager,
+                             IBattery BatteryManager,
+                             ILocalization Localization,
+                             IPeripheral PeripheralManager,
+                             ISafety SafetyManager,
+                             IError ErrorManager,
+                             IInfomation InfomationManager,
+                             Logger<RobotController> Logger,
+                             RobotConnection ConnectionManager) : BackgroundService
+{
+    private readonly Mutex NewOrderMutex = new();
+    private readonly Mutex NewInstanceMutex = new();
+
+    private WatchTimer<RobotController>? UpdateStateTimer;
+    private const int UpdateStateInterval = 1000;
+
+    protected override Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
+        UpdateStateTimer.Start();
+        return Task.CompletedTask;
+    }
+
+    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 (!string.IsNullOrEmpty(OrderManager.OrderId))
+                {
+                    if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderManager.OrderId}, OrderId mới: {order.OrderId}"));
+                    OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges);
+                }
+                else if (PeripheralManager.OperatingMode != Interfaces.OperatingMode.AUTOMATIC) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1006", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi chế độ vận hành không phải là TỰ ĐỘNG. Chế độ hiện tại: {PeripheralManager.OperatingMode}"));
+                else if(ActionManager.HasActionRunning) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1007", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có action đang thực hiện. Vui lòng chờ hoàn thành các action hiện tại."));
+                else if(ErrorManager.HasFatalError) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1008", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có lỗi nghiêm trọng. Vui lòng kiểm tra và xử lý lỗi."));
+                else if(NavigationManager.Driving) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1009", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi robot đang di chuyển. Vui lòng dừng robot."));
+                else OrderManager.StartOrder(order.OrderId, order.Nodes, order.Edges);
+            }
+            catch (OrderException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
+            }
+            finally
+            {
+                NewOrderMutex.ReleaseMutex();
+            }
+        }
+    }
+
+    public void NewInstanceActionUpdated()
+    {
+        if (NewInstanceMutex.WaitOne(2000))
+        {
+            try
+            {
+
+            }
+            catch (ActionException acEx)
+            {
+
+            }
+            catch (Exception ex)
+            {
+
+            }
+            finally
+            {
+                NewInstanceMutex.ReleaseMutex();
+            }
+        }
+    }
+}

RobotApp/Services/Robot/RobotErrors.cs

@@ -0,0 +1,41 @@
+using RobotApp.VDA5050.State;
+using RobotApp.VDA5050.Type;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotErrors
+{
+    private readonly List<Error> Errors = [];
+
+    public void AddError(Error error, TimeSpan? clearAfter = null)
+    {
+        lock (Errors)
+        {
+            Errors.Add(error);
+        }
+        if (clearAfter is not null && clearAfter.HasValue)
+        {
+            if (clearAfter.Value < TimeSpan.Zero) throw new ArgumentOutOfRangeException(nameof(clearAfter), "TimeSpan cannot be negative.");
+            _ = Task.Run(async () =>
+            {
+                await Task.Delay(clearAfter.Value);
+                lock (Errors)
+                {
+                    Errors.RemoveAll(e => e.ErrorType == error.ErrorType);
+                }
+            });
+        }
+    }
+
+    public static Error CreateError(ErrorType type, string hint, ErrorLevel level, string description)
+    {
+        return new Error()
+        {
+            ErrorType = type.ToString(),
+            ErrorLevel = level.ToString(),
+            ErrorDescription = description,
+            ErrorHint = hint,
+            ErrorReferences = []
+        };
+    }
+}

RobotApp/Services/Robot/RobotFactsheet.cs

@@ -0,0 +1,34 @@
+
+using RobotApp.Common.Shares;
+using RobotApp.VDA5050;
+using RobotApp.VDA5050.Factsheet;
+using System.Text.Json;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService
+{
+    public async Task PubFactsheet()
+    {
+        if (!RobotConnection.IsConnected) return;
+        FactSheetMsg factSheet = new()
+        {
+            SerialNumber = RobotConnection.SerialNumber,
+            ProtocolFeatures = new()
+            {
+                AgvActions = [],
+            }
+        };
+        string factSheetJson = JsonSerializer.Serialize(factSheet, JsonOptionExtends.Write);
+        await RobotConnection.Publish(VDA5050Topic.FACTSHEET.ToTopicString(), factSheetJson);
+    }
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        while(!stoppingToken.IsCancellationRequested)
+        {
+            await Task.Delay(1000, stoppingToken);
+            if (RobotConnection.IsConnected) break;
+        }
+        await PubFactsheet();
+    }
+}

RobotApp/Services/Robot/RobotInfomations.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Services.Robot
+{
+    public class RobotInfomations
+    {
+    }
+}

RobotApp/Services/Robot/RobotLoads.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Services.Robot
+{
+    public class RobotLoads
+    {
+    }
+}

RobotApp/Services/Robot/RobotOrderController.cs

@@ -0,0 +1,182 @@
+using RobotApp.Interfaces;
+using RobotApp.Services.Exceptions;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+using Action = RobotApp.VDA5050.InstantAction.Action;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotOrderController(INavigation NavigationManager, IInstanceActions ActionManager, IError ErrorManager, Logger<RobotOrderController> Logger) : IOrder
+{
+    public string OrderId { get; private set; } = string.Empty;
+    public int OrderUpdateId { get; private set; }
+    public NodeState[] NodeStates { get; private set; } = [];
+    public EdgeState[] EdgeStates { get; private set; } = [];
+    public string LastNodeId { get; private set; } = string.Empty;
+    public int LastNodeSequenceId { get; private set; }
+
+    private readonly Dictionary<string, Action[]> OrderActions = [];
+    private readonly Mutex OrderMutex = new();
+
+    protected const int CycleHandlerMilliseconds = 100;
+    private WatchTimer<RobotOrderController>? OrderTimer;
+
+    private int BaseSequenceId = 0;
+    public void StartOrder(string orderId, Node[] nodes, Edge[] edges)
+    {
+        if (OrderMutex.WaitOne(2000))
+        {
+            try
+            {
+                if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderId}, OrderId mới: {orderId}"));
+                if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
+                if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
+                if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+                if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.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: {NavigationManager.State}"));
+
+                for (int i = 0; i < nodes.Length; i++)
+                {
+                    if (nodes[i].Actions is not null && nodes[i].Actions.Length > 0)
+                    {
+                        foreach (var item in nodes[i].Actions)
+                        {
+                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
+                        }
+                        OrderActions.Add(nodes[i].NodeId, nodes[i].Actions);
+                    }
+                    if (i < nodes.Length - 1 && edges[i] is not null && edges[i].Length > 0)
+                    {
+                        foreach (var item in edges[i].Actions)
+                        {
+                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
+                        }
+                        OrderActions.TryAdd(nodes[i].NodeId, edges[i].Actions);
+                    }
+                    if (nodes[i].SequenceId != i) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1010", ErrorLevel.WARNING, $"Order Nodes không đúng thứ tự. NodeId: {nodes[i].NodeId}, SequenceId: {nodes[i].SequenceId}, Vị trí đúng: {i}"));
+                    if (i < nodes.Length - 1 &&  edges[i].SequenceId != i) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edges[i].EdgeId}, SequenceId: {edges[i].SequenceId}, Vị trí đúng: {i}"));
+                    if (nodes[i].Released) BaseSequenceId = nodes[i].SequenceId;
+                }
+
+                NodeStates = nodes.Select(n => new NodeState
+                {
+                    NodeId = n.NodeId,
+                    Released = n.Released,
+                    SequenceId = n.SequenceId,
+                    NodeDescription = n.NodeDescription,
+                    NodePosition = new()
+                    {
+                        X = n.NodePosition.X,
+                        Y = n.NodePosition.Y,
+                        Theta = n.NodePosition.Theta,
+                        MapId = n.NodePosition.MapId
+                    }
+                }).ToArray();
+                EdgeStates = edges.Select(e => new EdgeState
+                {
+                    EdgeId = e.EdgeId,
+                    Released = e.Released,
+                    EdgeDescription = e.EdgeDescription,
+                    SequenceId = e.SequenceId,
+                    Trajectory = e.Trajectory
+                }).ToArray();
+
+                OrderId = orderId;
+                ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
+                NavigationManager.Move(nodes, edges);
+                HandleNavigationStart();
+            }
+            catch (OrderException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"Lỗi khi khởi tạo Order: {orEx.Error.ErrorDescription}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Lỗi khi khởi tạo Order: {ex.Message}");
+            }
+            finally
+            {
+                OrderMutex.ReleaseMutex();
+            }
+        }
+        Logger.Warning($"Lỗi khi khởi tạo Order: có order đang được khởi tạo chưa xong");
+    }
+
+    public void UpdateOrder(int orderUpdateId,Node[] nodes, Edge[] edges)
+    {
+        if (OrderMutex.WaitOne(2000))
+        {
+            try
+            {
+                if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1005", ErrorLevel.WARNING, $"Không có order đang được thực hiện."));
+                if (orderUpdateId > OrderUpdateId)
+                {
+                    OrderUpdateId = orderUpdateId;
+                    // Check Order Update hợp lệ
+                    // Check Order update Hoziron hay không
+                }
+            }
+            catch (OrderException orEx)
+            {
+                if (orEx.Error is not null)
+                {
+                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
+                    Logger.Warning($"Lỗi khi cập nhật Order: {orEx.Error.ErrorDescription}");
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Lỗi khi cập nhật Order: {ex.Message}");
+            }
+            finally
+            {
+                OrderMutex.ReleaseMutex();
+            }
+        }
+        Logger.Warning($"Lỗi khi cập nhật Order: có order đang được cập nhật chưa xong");
+    }
+
+    public void StopOrder()
+    {
+        HandleNavigationStop();
+        NavigationManager.CancelMovement();
+
+        OrderId = string.Empty;
+        OrderActions.Clear();
+        OrderUpdateId = 0;
+        BaseSequenceId = 0;
+        LastNodeSequenceId = 0;
+        NodeStates = [];
+        EdgeStates = [];
+    }
+
+    private void HandleNavigationStart()
+    {
+        OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
+        OrderTimer.Start();
+    }
+
+    private void HandleNavigationStop()
+    {
+        OrderTimer?.Dispose();
+    }
+
+    private void OrderHandler()
+    {
+        if(!string.IsNullOrEmpty(OrderId) && BaseSequenceId > 0)
+        {
+            if(NavigationManager.State == NavigationState.Initializing)
+            {
+                // khởi tạo Order
+                
+            }
+            else
+            {
+                // xử lí khi có Order
+            }
+        }
+    }
+}

RobotApp/Services/Robot/RobotPathPlanner.cs

@@ -0,0 +1,88 @@
+using RobotApp.Common.Shares;
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Common.Shares.Model;
+using RobotApp.Services.Exceptions;
+using RobotApp.Services.Robot.Navigation;
+using RobotApp.VDA5050.Order;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
+
+public class RobotPathPlanner(IConfiguration Configuration)
+{
+    private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1);
+
+    public NavigationNode[] GetNavigationNode(double currentTheta, Node[] nodes, Edge[] edges)
+    {
+        if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
+        if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
+        if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+
+        return [];
+    }
+
+    public NavigationNode[] PathSplit(NavigationNode[] nodes, Edge[] edges)
+    {
+        if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
+        if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
+        if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
+
+        List<NavigationNode> navigationNode = [new()
+        {
+            NodeId = nodes[0].NodeId,
+            X = nodes[0].X,
+            Y = nodes[0].Y,
+            Theta = nodes[0].Theta,
+            Direction = nodes[0].Direction,
+            Description = nodes[0].Description
+        }];
+        foreach (var edge in edges)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
+            var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
+            if (startNode is null) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge chứa StartNodeId không tồn tại trong Nodes . StartNodeId: {edge.StartNodeId}"));
+            if (endNode is null) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1015", ErrorLevel.WARNING, $"Edge chứa EndNodeId không tồn tại trong Nodes . EndNodeId: {edge.EndNodeId}"));
+
+            var EdgeCalculatorModel = new EdgeCalculatorModel()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].X : 0,
+                ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].Y : 0,
+                ControlPoint2X = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].X : 0,
+                ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].Y : 0,
+                TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two :TrajectoryDegree.Three
+            };
+
+            double length = EdgeCalculatorModel.GetEdgeLength();
+            if (length <= 0) continue;
+            double step = ResolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = EdgeCalculatorModel.Curve(t);
+                navigationNode.Add(new()
+                {
+                    NodeId = string.Empty,
+                    X = x,
+                    Y = y,
+                    Theta = startNode.Theta,
+                    Direction = startNode.Direction,
+                    Description = string.Empty,
+                });
+            }
+            navigationNode.Add(new()
+            {
+                NodeId = endNode.NodeId,
+                X = endNode.X,
+                Y = endNode.Y,
+                Theta = endNode.Theta,
+                Direction = endNode.Direction,
+                Description = nodes[0].Description
+            });
+        }
+        return [..navigationNode];
+    }
+}

RobotApp/Services/Robot/RobotSafety.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Services.Robot
+{
+    public class RobotSafety
+    {
+    }
+}

RobotApp/Services/Robot/RobotStates.cs

@@ -0,0 +1,5 @@
+namespace RobotApp.Services.Robot;
+
+public class RobotStates
+{
+}

RobotApp/Services/Robot/RobotVisualization.cs

@@ -0,0 +1,6 @@
+namespace RobotApp.Services.Robot
+{
+    public class RobotVisualization
+    {
+    }
+}