first commit -push

RobotNet.RobotManager/Controllers/OpenACSSettingsController.cs

@@ -0,0 +1,103 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotShares.OpenACS;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[Authorize]
+public class OpenACSSettingsController(OpenACSManager OpenACSManager, LoggerController<OpenACSSettingsController> Logger) : ControllerBase
+{
+    [HttpGet]
+    public Task<MessageResult<OpenACSSettingsDto>> GetOpenACSSettings()
+    {
+        try
+        {
+            var settings = new OpenACSSettingsDto
+            {
+                PublishSetting = new(OpenACSManager.PublishEnable, OpenACSManager.PublishURL, OpenACSManager.PublishURLUsed, OpenACSManager.PublishInterval),
+                TrafficSetting = new(OpenACSManager.TrafficEnable, OpenACSManager.TrafficURL, OpenACSManager.TrafficURLUsed)
+            };
+            return Task.FromResult<MessageResult<OpenACSSettingsDto>>(new(true, "") { Data = settings });
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Lấy cấu hình OpenACS xảy ra lỗi: {ex.Message}");
+            return Task.FromResult<MessageResult<OpenACSSettingsDto>>(new(false, "Hệ thống có lỗi xảy ra"));
+        }
+    }
+
+    [HttpPost]
+    [Route("publish")]
+    public async Task<MessageResult<OpenACSPublishSettingDto>> UpdatePublishSetting([FromBody] OpenACSPublishSettingModel settingModel)
+    {
+        try
+        {
+            await OpenACSManager.UpdatePublishURL(settingModel.Url);
+            await OpenACSManager.UpdatePublishInterval(settingModel.Interval);
+            return new(true, "") { Data = new(OpenACSManager.PublishEnable, OpenACSManager.PublishURL, OpenACSManager.PublishURLUsed, OpenACSManager.PublishInterval) };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Cập nhật cấu hình publish OpenACS xảy ra lỗi: {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpPost]
+    [Route("traffic")]
+    public async Task<MessageResult<OpenACSTrafficSettingDto>> UpdateTrafficSetting([FromBody] OpenACSTrafficSettingModel settingModel)
+    {
+        try
+        {
+            await OpenACSManager.UpdateTrafficURL(settingModel.Url);
+            return new(true, "") { Data = new(OpenACSManager.TrafficEnable, OpenACSManager.TrafficURL, OpenACSManager.TrafficURLUsed) };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Cập nhật cấu hình traffic OpenACS xảy ra lỗi: {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpGet]
+    [Route("publish")]
+    public async Task<MessageResult> UpdatePublishEnable([FromQuery] bool enable)
+    {
+        try
+        {
+            await OpenACSManager.UpdatePublishEnable(enable);
+            return new(true, "");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Cập nhật cấu hình publish OpenACS xảy ra lỗi: {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpGet]
+    [Route("publish/count")]
+    [AllowAnonymous]
+    public int GetStatusPublishCount([FromServices] OpenACSPublisher publisher) => publisher.PublishCount;
+
+    [HttpGet]
+    [Route("traffic")]
+    public async Task<MessageResult> UpdateTrafficEnable([FromQuery] bool enable)
+    {
+        try
+        {
+            await OpenACSManager.UpdateTrafficEnable(enable);
+            return new(true, "");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Cập nhật cấu hình publish OpenACS xảy ra lỗi: {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+}

RobotNet.RobotManager/Controllers/RobotManagerController.cs

@@ -0,0 +1,179 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using Microsoft.EntityFrameworkCore;
+using RobotNet.MapShares.Models;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotShares.Models;
+using RobotNet.Script.Expressions;
+using RobotNet.Shares;
+using Serialize.Linq.Serializers;
+using System.Linq.Expressions;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[AllowAnonymous]
+public class RobotManagerController(RobotEditorDbContext RobotDb, MapManager MapManager, Services.RobotManager RobotManager, ILogger<RobotManagerController> Logger) : ControllerBase
+{
+    private static readonly ExpressionSerializer expressionSerializer;
+
+    static RobotManagerController()
+    {
+        var jss = new Serialize.Linq.Serializers.JsonSerializer();
+        expressionSerializer = new ExpressionSerializer(jss);
+
+        expressionSerializer.AddKnownType(typeof(RobotState));
+    }
+
+    [HttpPost]
+    [Route("MoveToNode")]
+    public async Task<MessageResult> MoveToNode([FromBody] RobotMoveToNodeModel model)
+    {
+        try
+        {
+            if (string.IsNullOrEmpty(model.NodeName)) return new(false, "NodeName cannot be empty..");
+
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == model.RobotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var map = await MapManager.GetMapData(robot.MapId);
+            if (!map.IsSuccess) return new(false, map.Message);
+            if (map is null || map.Data is null) return new(false, "The robot has not been assigned a map.");
+
+            var node = map.Data.Nodes.FirstOrDefault(n => n.Name == model.NodeName && n.MapId == map.Data.Id);
+            if (node is null) return new(false, "This Node does not exist.");
+
+            if (!robotController.StateMsg.NewBaseRequest) return new(false, "The robot is busy.");
+
+            var move = await robotController.MoveToNode(node.Name, model.Actions, model.LastAngle);
+            if (move.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} di chuyển tới node {model.NodeName} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} di chuyển tới node {model.NodeName} không thành công thành công do {move.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi MoveToNode Robot {RobotId} di chuyển tới node {NodeName} xảy ra ngoại lệ: {Message}", model.RobotId, model.NodeName, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    [HttpDelete]
+    [Route("MoveToNode/{robotId}")]
+    public async Task<MessageResult> Cancel(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == robotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var cancel = await robotController.CancelOrder();
+            if (cancel.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Hủy bỏ nhiệm vụ của Robot {robotId} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Hủy bỏ nhiệm vụ của Robot {robotId} không thành công do {cancel.Message}");
+            return new(false, "Request failed.");
+
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi Cancel Robot {RobotId} xảy ra ngoại lệ: {Message}", robotId, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    [HttpPost]
+    [Route("InstantActions")]
+    public async Task<MessageResult> InstantAction([FromBody] RobotInstantActionModel model)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == model.RobotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var instantAction = await robotController.InstantAction(model.Action, false);
+            if (instantAction.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Gửi Action Robot {model.RobotId} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Gửi Action Robot {model.RobotId} không thành công do {instantAction.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi InstantAction Robot {RobotId}, Action type {action} xảy ra ngoại lệ: {Message}", model.RobotId, model.Action.ActionType, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    [HttpGet]
+    [Route("State/{robotId}")]
+    public async Task<MessageResult<RobotStateModel>> GetState(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == robotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null) return new(false, "The robot has not logged into the system.");
+
+            return new(true)
+            {
+                Data = new()
+                {
+                    RobotId = robotId,
+                    MapId = robot.MapId,
+                    IsOnline = robotController.IsOnline,
+                    State = robotController.State,
+                    OrderState = robotController.OrderState,
+                    NewBaseRequest = robotController.StateMsg.NewBaseRequest,
+                    NodeStates = [.. robotController.StateMsg.NodeStates],
+                    EdgeStates = [.. robotController.StateMsg.EdgeStates],
+                    Loads = [.. robotController.StateMsg.Loads],
+                    ActionStates = robotController.ActionStates,
+                    BatteryState = robotController.StateMsg.BatteryState,
+                    Errors = [.. robotController.StateMsg.Errors],
+                    Information = [.. robotController.StateMsg.Information],
+                    SafetyState = robotController.StateMsg.SafetyState,
+                    AgvPosition = robotController.VisualizationMsg.AgvPosition,
+                    Velocity = robotController.VisualizationMsg.Velocity,
+                }
+            };
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi GetState Robot {RobotId} xảy ra ngoại lệ: {Message}", robotId, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    [HttpPost]
+    [Route("Search")]
+    public async Task<MessageResult<string[]>> SearchRobot([FromBody] RobotSearchExpressionModel model)
+    {
+        var expr = expressionSerializer.DeserializeText(model.Expression);
+        var lambda = (Expression<Func<RobotState, bool>>)expr;
+
+        // Compile và chạy:
+        var func = lambda.Compile();
+        return await RobotManager.SearchRobot(model.ModelName, model.MapName, func);
+    }
+}

RobotNet.RobotManager/Controllers/RobotManagerLoggerController.cs

@@ -0,0 +1,87 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using RobotNet.RobotManager.Services;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[AllowAnonymous]
+public class RobotManagerLoggerController(LoggerController<RobotManagerLoggerController> Logger) : ControllerBase
+{
+    private readonly string LoggerDirectory = "robotManagerlogs";
+    private readonly string OpenACSLoggerDirectory = "openACSlogs";
+
+    [HttpGet]
+    public async Task<IEnumerable<string>> GetLogs([FromQuery(Name = "date")] DateTime date)
+    {
+        string temp = "";
+        try
+        {
+            string fileName = $"{date:yyyy-MM-dd}.log";
+            string path = Path.Combine(LoggerDirectory, fileName);
+            if (!Path.GetFullPath(path).StartsWith(Path.GetFullPath(LoggerDirectory)))
+            {
+                Logger.Warning($"GetLogs: phát hiện đường dẫn không hợp lệ.");
+                return [];
+            }
+
+            if (!System.IO.File.Exists(path))
+            {
+                Logger.Warning($"GetLogs: không tìm thấy file log của ngày {date.ToShortDateString()} - {path}.");
+                return [];
+            }
+
+            temp = Path.Combine(LoggerDirectory, $"{Guid.NewGuid()}.log");
+            System.IO.File.Copy(path, temp);
+
+            return await System.IO.File.ReadAllLinesAsync(temp);
+        }
+        catch(Exception ex)
+        {
+            Logger.Warning($"GetLogs: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return [];
+        }
+        finally
+        {
+            if (System.IO.File.Exists(temp)) System.IO.File.Delete(temp);
+        }
+    }
+
+    [HttpGet]
+    [Route("open-acs")]
+    public async Task<IEnumerable<string>> GetLogsACS([FromQuery(Name = "date")] DateTime date)
+    {
+        string temp = "";
+        try
+        {
+            string fileName = $"{date:yyyy-MM-dd}.log";
+            string path = Path.Combine(OpenACSLoggerDirectory, fileName);
+            if (!Path.GetFullPath(path).StartsWith(Path.GetFullPath(OpenACSLoggerDirectory)))
+            {
+                Logger.Warning($"GetLogs: phát hiện đường dẫn không hợp lệ.");
+                return [];
+            }
+
+            if (!System.IO.File.Exists(path))
+            {
+                Logger.Warning($"GetLogs: không tìm thấy file log của ngày {date.ToShortDateString()} - {path}.");
+                return [];
+            }
+
+            temp = Path.Combine(OpenACSLoggerDirectory, $"{Guid.NewGuid()}.log");
+            System.IO.File.Copy(path, temp);
+
+            return await System.IO.File.ReadAllLinesAsync(temp);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetLogs: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return [];
+        }
+        finally
+        {
+            if (System.IO.File.Exists(temp)) System.IO.File.Delete(temp);
+        }
+    }
+}

RobotNet.RobotManager/Controllers/RobotModelsController.cs

@@ -0,0 +1,258 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using Microsoft.EntityFrameworkCore;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[Authorize]
+public class RobotModelsController(RobotEditorDbContext RobotDb, RobotEditorStorageRepository RobotStorage, IHttpClientFactory HttpClientFactory, LoggerController<RobotManagerController> Logger) : ControllerBase
+{
+    [HttpGet]
+    public async Task<IEnumerable<RobotModelDto>> GetRobotModels([FromQuery(Name = "txtSearch")] string? txtSearch)
+    {
+        try
+        {
+            if (string.IsNullOrWhiteSpace(txtSearch))
+            {
+                return await (from robotmodel in RobotDb.RobotModels
+                              select new RobotModelDto()
+                              {
+                                  Id = robotmodel.Id,
+                                  ModelName = robotmodel.ModelName,
+                                  OriginX = robotmodel.OriginX,
+                                  OriginY = robotmodel.OriginY,
+                                  ImageWidth = robotmodel.ImageWidth,
+                                  ImageHeight = robotmodel.ImageHeight,
+                                  Width = Math.Round(robotmodel.Width, 2),
+                                  Length = Math.Round(robotmodel.Length, 2),
+                                  NavigationType = robotmodel.NavigationType,
+                              }).ToListAsync();
+            }
+            else
+            {
+                return await (from robotmodel in RobotDb.RobotModels
+                              where !string.IsNullOrEmpty(robotmodel.ModelName) && robotmodel.ModelName.Contains(txtSearch)
+                              select new RobotModelDto()
+                              {
+                                  Id = robotmodel.Id,
+                                  ModelName = robotmodel.ModelName,
+                                  OriginX = robotmodel.OriginX,
+                                  OriginY = robotmodel.OriginY,
+                                  ImageWidth = robotmodel.ImageWidth,
+                                  ImageHeight = robotmodel.ImageHeight,
+                                  Width = Math.Round(robotmodel.Width, 2),
+                                  Length = Math.Round(robotmodel.Length, 2),
+                                  NavigationType = robotmodel.NavigationType,
+                              }).ToListAsync();
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetRobotModels: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return [];
+        }
+    }
+
+    [HttpGet]
+    [Route("{robotModelId}")]
+    public async Task<MessageResult<RobotModelDto?>> GetRobotModel(Guid robotModelId)
+    {
+        try
+        {
+            var robotmodel = await RobotDb.RobotModels.FirstOrDefaultAsync(model => model.Id == robotModelId);
+            var robotmodelDto = robotmodel is null ? null : new RobotModelDto()
+            {
+                Id = robotmodel.Id,
+                ModelName = robotmodel.ModelName,
+                OriginX = robotmodel.OriginX,
+                OriginY = robotmodel.OriginY,
+                ImageWidth = robotmodel.ImageWidth,
+                ImageHeight = robotmodel.ImageHeight,
+                Width = Math.Round(robotmodel.Width, 2),
+                Length = Math.Round(robotmodel.Length, 2),
+                NavigationType = robotmodel.NavigationType,
+            };
+            return new(true) { Data = robotmodelDto };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetRobotModel {robotModelId}: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpGet]
+    [AllowAnonymous]
+    [Route("image/{robotmodelId}")]
+    public async Task<IActionResult> GetImage(Guid robotmodelId)
+    {
+        try
+        {
+            var (usingLocal, url) = RobotStorage.GetUrl("RobotModels", robotmodelId.ToString());
+            if (!usingLocal)
+            {
+                var http = HttpClientFactory.CreateClient();
+                var imageBytes = await http.GetByteArrayAsync(url);
+                if (imageBytes != null && imageBytes.Length > 0) return File(imageBytes, "image/png");
+                else return NotFound("Không thể lấy được ảnh map.");
+            }
+            if (System.IO.File.Exists(url)) return PhysicalFile(url, "image/png");
+            else return NotFound();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetImage {robotmodelId}: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return NotFound();
+        }
+    }
+
+    [HttpPost]
+    public async Task<MessageResult<RobotModelDto>> CreateRobotModel([FromForm] RobotModelCreateModel robotmodel, [FromForm(Name = "Image")] IFormFile imageUpload)
+    {
+        try
+        {
+            if (imageUpload.ContentType != "image/png") return new(false, "Robot image format chỉ hỗ trợ định dạng image/png");
+            if (await RobotDb.RobotModels.AnyAsync(model => model.ModelName == robotmodel.ModelName)) return new(false, "Tên của robot model đã tồn tại, Hãy đặt tên khác!");
+
+            var image = SixLabors.ImageSharp.Image.Load(imageUpload.OpenReadStream());
+            var entityRobotModel = await RobotDb.RobotModels.AddAsync(new RobotModel()
+            {
+                ModelName = robotmodel.ModelName,
+                OriginX = robotmodel.OriginX,
+                OriginY = robotmodel.OriginY,
+                ImageHeight = image.Height,
+                ImageWidth = image.Width,
+                Length = robotmodel.Length,
+                Width = robotmodel.Width,
+                NavigationType = robotmodel.NavigationType,
+            });
+
+            await RobotDb.SaveChangesAsync();
+
+            var (isSuccess, message) = await RobotStorage.UploadAsync("RobotModels", $"{entityRobotModel.Entity.Id}", imageUpload.OpenReadStream(), imageUpload.Length, imageUpload.ContentType, CancellationToken.None);
+            if (!isSuccess)
+            {
+                RobotDb.RobotModels.Remove(entityRobotModel.Entity);
+                await RobotDb.SaveChangesAsync();
+                return new(false, message);
+            }
+
+            return new(true)
+            {
+                Data = new()
+                {
+                    Id = entityRobotModel.Entity.Id,
+                    ModelName = entityRobotModel.Entity.ModelName,
+                    OriginX = entityRobotModel.Entity.OriginX,
+                    OriginY = entityRobotModel.Entity.OriginY,
+                    ImageWidth = entityRobotModel.Entity.ImageWidth,
+                    ImageHeight = entityRobotModel.Entity.ImageHeight,
+                    Width = Math.Round(entityRobotModel.Entity.Width, 2),
+                    Length = Math.Round(entityRobotModel.Entity.Length, 2),
+                    NavigationType = entityRobotModel.Entity.NavigationType,
+                }
+            };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"CreateRobotModel : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpPut]
+    public async Task<MessageResult<RobotModelDto>> UpdateRobotModel([FromBody] RobotModelUpdateModel robotmodel)
+    {
+        try
+        {
+            var robotModelDb = await RobotDb.RobotModels.FindAsync(robotmodel.Id);
+            if (robotModelDb == null) return new(false, $"Không tồn tại robot model id = {robotmodel.Id}");
+
+            if (robotmodel.ModelName != robotModelDb.ModelName && await RobotDb.RobotModels.AnyAsync(m => m.ModelName == robotmodel.ModelName))
+            {
+                return new(false, "Tên của robot model đã tồn tại, Hãy đặt tên khác!");
+            }
+
+            robotModelDb.ModelName = robotmodel.ModelName;
+            robotModelDb.OriginX = robotmodel.OriginX;
+            robotModelDb.OriginY = robotmodel.OriginY;
+            robotModelDb.Length = robotmodel.Length;
+            robotModelDb.Width = robotmodel.Width;
+
+            await RobotDb.SaveChangesAsync();
+            return new(true)
+            {
+                Data = new()
+                {
+                    ModelName = robotModelDb.ModelName,
+                    OriginX = robotModelDb.OriginX,
+                    OriginY = robotModelDb.OriginY,
+                    NavigationType = robotModelDb.NavigationType,
+                    ImageWidth = robotModelDb.ImageWidth,
+                    ImageHeight = robotModelDb.ImageHeight,
+                    Width = Math.Round(robotModelDb.Width, 2),
+                    Length = Math.Round(robotModelDb.Length, 2),
+                }
+            };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"UpdateRobotModel : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpDelete]
+    [Route("{robotModelId}")]
+    public async Task<MessageResult> DeleteRobotModel(Guid robotModelId)
+    {
+        try
+        {
+            var robotModelDb = await RobotDb.RobotModels.FindAsync(robotModelId);
+            if (robotModelDb == null) return new(false, $"Không tồn tại robot model id = {robotModelId}");
+
+            if (RobotDb.Robots.Any(r => r.ModelId == robotModelId)) return new(false, "Tồn tại robot đang sử dụng model này");
+
+            await RobotStorage.DeleteAsync("RobotModels", robotModelId.ToString(), CancellationToken.None);
+            RobotDb.RobotModels.Remove(robotModelDb);
+            await RobotDb.SaveChangesAsync();
+            return new(true, "");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"DeleteRobotModel {robotModelId} : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpPut]
+    [Route("image/{robotModelId}")]
+    public async Task<MessageResult> UpdateImage(Guid robotModelId, [FromForm(Name = "image")] IFormFile image)
+    {
+        try
+        {
+            var robotModel = await RobotDb.RobotModels.FindAsync(robotModelId);
+            if (robotModel == null) return new(false, $"Không tồn tại robot model id = {robotModelId}");
+
+            var imageStream = image.OpenReadStream();
+            var imageUpdate = SixLabors.ImageSharp.Image.Load(imageStream);
+            robotModel.ImageWidth = imageUpdate.Width;
+            robotModel.ImageHeight = imageUpdate.Height;
+            await RobotDb.SaveChangesAsync();
+
+            var (isSuccess, message) = await RobotStorage.UploadAsync("RobotModels", robotModelId.ToString(), image.OpenReadStream(), image.Length, image.ContentType, CancellationToken.None);
+            return new(true, "");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"UpdateImage: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+}

RobotNet.RobotManager/Controllers/RobotsController.cs

@@ -0,0 +1,246 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using Microsoft.EntityFrameworkCore;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[Authorize]
+public class RobotsController(RobotEditorDbContext RobotDb, Services.RobotManager RobotManager, MapManager MapManager, LoggerController<RobotsController> Logger) : ControllerBase
+{
+    [HttpGet]
+    public async Task<MessageResult<IEnumerable<RobotDto>>> GetRobots()
+    {
+        try
+        {
+            var robots = await (from robot in RobotDb.Robots
+                                where !string.IsNullOrEmpty(robot.Name) && !string.IsNullOrEmpty(robot.RobotId)
+                                join robotmodel in RobotDb.RobotModels on robot.ModelId equals robotmodel.Id
+                                select new RobotDto()
+                                {
+                                    Id = robot.Id,
+                                    RobotId = robot.RobotId,
+                                    Name = robot.Name,
+                                    ModelName = robotmodel.ModelName,
+                                    ModelId = robotmodel.Id,
+                                    MapId = robot.MapId,
+                                    NavigationType = robotmodel.NavigationType,
+                                    Online = false,
+                                }).ToListAsync();
+            foreach (var robot in robots)
+            {
+                var map = await MapManager.GetMapInfo(robot.MapId);
+                if (map is not null && map.Data is not null) robot.MapName = map.Data.Name;
+                else robot.MapName = string.Empty;
+                var robotController = RobotManager[robot.RobotId];
+                robot.Online = robotController is not null && robotController.IsOnline;
+                robot.State = robotController is not null ? robotController.State : "OFFLINE";
+            }
+            return new(true) { Data = robots };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetRobots: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpGet]
+    [Route("{robotId}")]
+    public async Task<MessageResult<RobotDto?>> GetRobot(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(model => model.RobotId == robotId);
+            if (robot is not null)
+            {
+                var map = await MapManager.GetMapInfo(robot.MapId);
+                var robotModel = await RobotDb.RobotModels.FirstOrDefaultAsync(m => m.Id == robot.ModelId);
+                return new(true)
+                {
+                    Data = new RobotDto()
+                    {
+                        Id = robot.Id,
+                        RobotId = robot.RobotId,
+                        Name = robot.Name,
+                        ModelName = robotModel is not null ? robotModel.ModelName : string.Empty,
+                        ModelId = robot.ModelId,
+                        MapId = robot.MapId,
+                        MapName = map is not null && map.Data is not null ? map.Data.Name : string.Empty,
+                        NavigationType = robotModel is not null ? robotModel.NavigationType : NavigationType.Differential,
+                        Online = RobotManager[robot.RobotId] is not null,
+                    }
+                };
+            }
+            return new(false, "RobotId không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetRobot: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpPost]
+    public async Task<MessageResult<RobotDto>> CreateRobot([FromBody] RobotCreateModel robot)
+    {
+        try
+        {
+            if (await RobotDb.Robots.AnyAsync(r => r.Name == robot.Name)) return new(false, "Tên của robot đã tồn tại, Hãy đặt tên khác!");
+            if (await RobotDb.Robots.AnyAsync(r => r.RobotId == robot.RobotId)) return new(false, "Robot Id đã tồn tại, Hãy đặt tên khác!");
+
+            var robotModel = await RobotDb.RobotModels.FirstOrDefaultAsync(m => m.Id == robot.ModelId);
+            if (robotModel is null) return new(false, "Robot Model không tồn tại");
+
+            var entityRobotModel = await RobotDb.Robots.AddAsync(new Robot()
+            {
+                RobotId = robot.RobotId,
+                Name = robot.Name,
+                ModelId = robot.ModelId,
+            });
+
+            await RobotDb.SaveChangesAsync();
+
+            return new(true)
+            {
+                Data = new RobotDto()
+                {
+                    Id = entityRobotModel.Entity.Id,
+                    RobotId = entityRobotModel.Entity.RobotId,
+                    Name = entityRobotModel.Entity.Name,
+                    ModelId = entityRobotModel.Entity.ModelId,
+                    ModelName = robotModel.ModelName,
+                    NavigationType = robotModel.NavigationType,
+                }
+            };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"CreateRobot : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpPut]
+    public async Task<MessageResult> UpdateRobot([FromBody] RobotUpdateModel robot)
+    {
+        try
+        {
+            var robotDb = await RobotDb.Robots.FindAsync(robot.RobotId);
+            if (robotDb == null) return new(false, $"Không tồn tại robot model id = {robot.RobotId}");
+
+            if (robotDb.Name != robot.Name && await RobotDb.Robots.AnyAsync(r => r.Name == robot.Name))
+            {
+                return new(false, "Tên của robot đã tồn tại, Hãy đặt tên khác!");
+            }
+
+            robotDb.Name = robot.Name;
+            robotDb.ModelId = robot.ModelId;
+            robotDb.MapId = robot.MapId;
+
+            await RobotDb.SaveChangesAsync();
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"UpdateRobot : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpDelete]
+    [Route("{robotId}")]
+    public async Task<MessageResult> DeleteRobot(string robotId)
+    {
+        try
+        {
+            var robotDb = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == robotId);
+            if (robotDb == null) return new(false, $"Không tồn tại robot id = {robotId}");
+
+            RobotManager.DeleteRobot(robotId);
+            RobotDb.Robots.Remove(robotDb);
+            await RobotDb.SaveChangesAsync();
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"DeleteRobot : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpGet]
+    [Route("Model/{robotId}")]
+    public async Task<MessageResult<RobotModelDto?>> GetModel(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(model => model.RobotId == robotId);
+            if (robot is not null)
+            {
+                var robotModel = await RobotDb.RobotModels.FirstOrDefaultAsync(m => m.Id == robot.ModelId);
+                if (robotModel is null) return new(false, "Robot model không tồn tại");
+                return new(true)
+                {
+                    Data = new RobotModelDto()
+                    {
+                        Id = robotModel.Id,
+                        ModelName = robotModel.ModelName,
+                        OriginX = robotModel.OriginX,
+                        OriginY = robotModel.OriginY,
+                        NavigationType = robotModel.NavigationType,
+                        ImageWidth = robotModel.ImageWidth,
+                        ImageHeight = robotModel.ImageHeight,
+                        Width = robotModel.Width,
+                        Length = robotModel.Length
+                    }
+                };
+            }
+            return new(false, "RobotId không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetModel : Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpPost]
+    [Route("simulation")]
+    public MessageResult CreateRobotSimulation([FromBody] IEnumerable<string> robotIds)
+    {
+        try
+        {
+            RobotManager.AddRobotSimulation(robotIds);
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"CreateRobotSimulation: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpGet]
+    [Route("simulation")]
+    [AllowAnonymous]
+    public MessageResult DeleteRobotSimulation([FromQuery] string robotId)
+    {
+        try
+        {
+            RobotManager.DeleteRobot(robotId);
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"CreateRobotSimulation: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+}

RobotNet.RobotManager/Controllers/TrafficACSRequestController.cs

@@ -0,0 +1,25 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotShares.OpenACS;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[Authorize]
+public class TrafficACSRequestController(TrafficACS TrafficACS, Services.RobotManager RobotManager) : ControllerBase
+{
+    [HttpPost]
+    public async Task<MessageResult<bool>> UpdateTrafficSetting([FromBody] TrafficACSRequestModel model)
+        => model.Type switch
+        {
+            TrafficRequestType.IN => await TrafficACS.RequestIn(model.RobotId, model.ZoneId),
+            TrafficRequestType.OUT => await TrafficACS.RequestOut(model.RobotId, model.ZoneId),
+            _ => await TrafficACS.RequestOut(model.RobotId, model.ZoneId),
+        };
+
+    [HttpGet]
+    public RobotACSLockedDto[] GetRobotLocked() => RobotManager.GetRobotACSLocked();
+}

RobotNet.RobotManager/Controllers/TrafficManagerController.cs

@@ -0,0 +1,52 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.Mvc;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Models;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Controllers;
+
+[Route("api/[controller]")]
+[ApiController]
+[Authorize]
+public class TrafficManagerController(TrafficManager TrafficManager, LoggerController<TrafficManagerController> Logger) : ControllerBase
+{
+    [HttpPut]
+    public MessageResult UpdateLocker(UpdateAgentLockerModel model)
+    {
+        try
+        {
+            if (TrafficManager.TrafficMaps.TryGetValue(model.MapId, out TrafficMap? trafficmap) && trafficmap is not null)
+            {
+                trafficmap.UpdateLocker(model.AgentId, [.. model.LockedNodes]);
+            }
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"UpdateLocker: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    [HttpDelete]
+    [Route("{mapId}/{robotId}")]
+    public MessageResult DeleteAgent([FromRoute] Guid mapId, [FromRoute] string robotId)
+    {
+        try
+        {
+            if (TrafficManager.TrafficMaps.TryGetValue(mapId, out TrafficMap? trafficmap) && trafficmap is not null)
+            {
+                trafficmap.Locked.Remove(robotId);
+            }
+            return TrafficManager.DeleteAgent(robotId);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"DeleteAgent: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, $"Hệ thống có lỗi xảy ra");
+        }
+    }
+}

RobotNet.RobotManager/Data/Migrations/20250509040621_AddRobotDb.Designer.cs

@@ -0,0 +1,131 @@
+// <auto-generated />
+using System;
+using Microsoft.EntityFrameworkCore;
+using Microsoft.EntityFrameworkCore.Infrastructure;
+using Microsoft.EntityFrameworkCore.Metadata;
+using Microsoft.EntityFrameworkCore.Migrations;
+using Microsoft.EntityFrameworkCore.Storage.ValueConversion;
+using RobotNet.RobotManager.Data;
+
+#nullable disable
+
+namespace RobotNet.RobotManager.Data.Migrations
+{
+    [DbContext(typeof(RobotEditorDbContext))]
+    [Migration("20250509040621_AddRobotDb")]
+    partial class AddRobotDb
+    {
+        /// <inheritdoc />
+        protected override void BuildTargetModel(ModelBuilder modelBuilder)
+        {
+#pragma warning disable 612, 618
+            modelBuilder
+                .HasAnnotation("ProductVersion", "9.0.4")
+                .HasAnnotation("Relational:MaxIdentifierLength", 128);
+
+            SqlServerModelBuilderExtensions.UseIdentityColumns(modelBuilder);
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.Robot", b =>
+                {
+                    b.Property<Guid>("Id")
+                        .ValueGeneratedOnAdd()
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("Id");
+
+                    b.Property<string>("ChargerNode")
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("ChargerNode");
+
+                    b.Property<string>("HomeNode")
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("HomeNode");
+
+                    b.Property<Guid>("MapId")
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("MapId");
+
+                    b.Property<Guid>("ModelId")
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("ModelId");
+
+                    b.Property<string>("Name")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("Name");
+
+                    b.Property<string>("RobotId")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("RobotId");
+
+                    b.HasKey("Id");
+
+                    b.HasIndex("MapId");
+
+                    b.ToTable("Robots");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.RobotModel", b =>
+                {
+                    b.Property<Guid>("Id")
+                        .ValueGeneratedOnAdd()
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("Id");
+
+                    b.Property<int>("ImageHeight")
+                        .HasColumnType("int")
+                        .HasColumnName("ImageHeight");
+
+                    b.Property<int>("ImageWidth")
+                        .HasColumnType("int")
+                        .HasColumnName("ImageWidth");
+
+                    b.Property<double>("Length")
+                        .HasColumnType("float")
+                        .HasColumnName("Length");
+
+                    b.Property<string>("ModelName")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("ModelName");
+
+                    b.Property<int>("NavigationType")
+                        .HasColumnType("int")
+                        .HasColumnName("NavigationType");
+
+                    b.Property<double>("OriginX")
+                        .HasColumnType("float")
+                        .HasColumnName("OriginX");
+
+                    b.Property<double>("OriginY")
+                        .HasColumnType("float")
+                        .HasColumnName("OriginY");
+
+                    b.Property<double>("Width")
+                        .HasColumnType("float")
+                        .HasColumnName("Width");
+
+                    b.HasKey("Id");
+
+                    b.ToTable("RobotModels");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.Robot", b =>
+                {
+                    b.HasOne("RobotNet.RobotManager.Data.RobotModel", "Model")
+                        .WithMany("Robots")
+                        .HasForeignKey("MapId")
+                        .OnDelete(DeleteBehavior.Restrict)
+                        .IsRequired();
+
+                    b.Navigation("Model");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.RobotModel", b =>
+                {
+                    b.Navigation("Robots");
+                });
+#pragma warning restore 612, 618
+        }
+    }
+}

RobotNet.RobotManager/Data/Migrations/20250509040621_AddRobotDb.cs

@@ -0,0 +1,72 @@
+using System;
+using Microsoft.EntityFrameworkCore.Migrations;
+
+#nullable disable
+
+namespace RobotNet.RobotManager.Data.Migrations
+{
+    /// <inheritdoc />
+    public partial class AddRobotDb : Migration
+    {
+        /// <inheritdoc />
+        protected override void Up(MigrationBuilder migrationBuilder)
+        {
+            migrationBuilder.CreateTable(
+                name: "RobotModels",
+                columns: table => new
+                {
+                    Id = table.Column<Guid>(type: "uniqueidentifier", nullable: false),
+                    ModelName = table.Column<string>(type: "varchar(127)", nullable: false),
+                    OriginX = table.Column<double>(type: "float", nullable: false),
+                    OriginY = table.Column<double>(type: "float", nullable: false),
+                    Length = table.Column<double>(type: "float", nullable: false),
+                    Width = table.Column<double>(type: "float", nullable: false),
+                    ImageWidth = table.Column<int>(type: "int", nullable: false),
+                    ImageHeight = table.Column<int>(type: "int", nullable: false),
+                    NavigationType = table.Column<int>(type: "int", nullable: false)
+                },
+                constraints: table =>
+                {
+                    table.PrimaryKey("PK_RobotModels", x => x.Id);
+                });
+
+            migrationBuilder.CreateTable(
+                name: "Robots",
+                columns: table => new
+                {
+                    Id = table.Column<Guid>(type: "uniqueidentifier", nullable: false),
+                    RobotId = table.Column<string>(type: "varchar(127)", nullable: false),
+                    Name = table.Column<string>(type: "varchar(127)", nullable: false),
+                    ModelId = table.Column<Guid>(type: "uniqueidentifier", nullable: false),
+                    MapId = table.Column<Guid>(type: "uniqueidentifier", nullable: false),
+                    HomeNode = table.Column<string>(type: "varchar(127)", nullable: true),
+                    ChargerNode = table.Column<string>(type: "varchar(127)", nullable: true)
+                },
+                constraints: table =>
+                {
+                    table.PrimaryKey("PK_Robots", x => x.Id);
+                    table.ForeignKey(
+                        name: "FK_Robots_RobotModels_MapId",
+                        column: x => x.MapId,
+                        principalTable: "RobotModels",
+                        principalColumn: "Id",
+                        onDelete: ReferentialAction.Restrict);
+                });
+
+            migrationBuilder.CreateIndex(
+                name: "IX_Robots_MapId",
+                table: "Robots",
+                column: "MapId");
+        }
+
+        /// <inheritdoc />
+        protected override void Down(MigrationBuilder migrationBuilder)
+        {
+            migrationBuilder.DropTable(
+                name: "Robots");
+
+            migrationBuilder.DropTable(
+                name: "RobotModels");
+        }
+    }
+}

RobotNet.RobotManager/Data/Migrations/20250509071716_fixRobotDb.Designer.cs

@@ -0,0 +1,131 @@
+// <auto-generated />
+using System;
+using Microsoft.EntityFrameworkCore;
+using Microsoft.EntityFrameworkCore.Infrastructure;
+using Microsoft.EntityFrameworkCore.Metadata;
+using Microsoft.EntityFrameworkCore.Migrations;
+using Microsoft.EntityFrameworkCore.Storage.ValueConversion;
+using RobotNet.RobotManager.Data;
+
+#nullable disable
+
+namespace RobotNet.RobotManager.Data.Migrations
+{
+    [DbContext(typeof(RobotEditorDbContext))]
+    [Migration("20250509071716_fixRobotDb")]
+    partial class fixRobotDb
+    {
+        /// <inheritdoc />
+        protected override void BuildTargetModel(ModelBuilder modelBuilder)
+        {
+#pragma warning disable 612, 618
+            modelBuilder
+                .HasAnnotation("ProductVersion", "9.0.4")
+                .HasAnnotation("Relational:MaxIdentifierLength", 128);
+
+            SqlServerModelBuilderExtensions.UseIdentityColumns(modelBuilder);
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.Robot", b =>
+                {
+                    b.Property<Guid>("Id")
+                        .ValueGeneratedOnAdd()
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("Id");
+
+                    b.Property<string>("ChargerNode")
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("ChargerNode");
+
+                    b.Property<string>("HomeNode")
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("HomeNode");
+
+                    b.Property<Guid>("MapId")
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("MapId");
+
+                    b.Property<Guid>("ModelId")
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("ModelId");
+
+                    b.Property<string>("Name")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("Name");
+
+                    b.Property<string>("RobotId")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("RobotId");
+
+                    b.HasKey("Id");
+
+                    b.HasIndex("ModelId");
+
+                    b.ToTable("Robots");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.RobotModel", b =>
+                {
+                    b.Property<Guid>("Id")
+                        .ValueGeneratedOnAdd()
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("Id");
+
+                    b.Property<int>("ImageHeight")
+                        .HasColumnType("int")
+                        .HasColumnName("ImageHeight");
+
+                    b.Property<int>("ImageWidth")
+                        .HasColumnType("int")
+                        .HasColumnName("ImageWidth");
+
+                    b.Property<double>("Length")
+                        .HasColumnType("float")
+                        .HasColumnName("Length");
+
+                    b.Property<string>("ModelName")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("ModelName");
+
+                    b.Property<int>("NavigationType")
+                        .HasColumnType("int")
+                        .HasColumnName("NavigationType");
+
+                    b.Property<double>("OriginX")
+                        .HasColumnType("float")
+                        .HasColumnName("OriginX");
+
+                    b.Property<double>("OriginY")
+                        .HasColumnType("float")
+                        .HasColumnName("OriginY");
+
+                    b.Property<double>("Width")
+                        .HasColumnType("float")
+                        .HasColumnName("Width");
+
+                    b.HasKey("Id");
+
+                    b.ToTable("RobotModels");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.Robot", b =>
+                {
+                    b.HasOne("RobotNet.RobotManager.Data.RobotModel", "Model")
+                        .WithMany("Robots")
+                        .HasForeignKey("ModelId")
+                        .OnDelete(DeleteBehavior.Restrict)
+                        .IsRequired();
+
+                    b.Navigation("Model");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.RobotModel", b =>
+                {
+                    b.Navigation("Robots");
+                });
+#pragma warning restore 612, 618
+        }
+    }
+}

RobotNet.RobotManager/Data/Migrations/20250509071716_fixRobotDb.cs

@@ -0,0 +1,60 @@
+using Microsoft.EntityFrameworkCore.Migrations;
+
+#nullable disable
+
+namespace RobotNet.RobotManager.Data.Migrations
+{
+    /// <inheritdoc />
+    public partial class fixRobotDb : Migration
+    {
+        /// <inheritdoc />
+        protected override void Up(MigrationBuilder migrationBuilder)
+        {
+            migrationBuilder.DropForeignKey(
+                name: "FK_Robots_RobotModels_MapId",
+                table: "Robots");
+
+            migrationBuilder.DropIndex(
+                name: "IX_Robots_MapId",
+                table: "Robots");
+
+            migrationBuilder.CreateIndex(
+                name: "IX_Robots_ModelId",
+                table: "Robots",
+                column: "ModelId");
+
+            migrationBuilder.AddForeignKey(
+                name: "FK_Robots_RobotModels_ModelId",
+                table: "Robots",
+                column: "ModelId",
+                principalTable: "RobotModels",
+                principalColumn: "Id",
+                onDelete: ReferentialAction.Restrict);
+        }
+
+        /// <inheritdoc />
+        protected override void Down(MigrationBuilder migrationBuilder)
+        {
+            migrationBuilder.DropForeignKey(
+                name: "FK_Robots_RobotModels_ModelId",
+                table: "Robots");
+
+            migrationBuilder.DropIndex(
+                name: "IX_Robots_ModelId",
+                table: "Robots");
+
+            migrationBuilder.CreateIndex(
+                name: "IX_Robots_MapId",
+                table: "Robots",
+                column: "MapId");
+
+            migrationBuilder.AddForeignKey(
+                name: "FK_Robots_RobotModels_MapId",
+                table: "Robots",
+                column: "MapId",
+                principalTable: "RobotModels",
+                principalColumn: "Id",
+                onDelete: ReferentialAction.Restrict);
+        }
+    }
+}

RobotNet.RobotManager/Data/Migrations/RobotEditorDbContextModelSnapshot.cs

@@ -0,0 +1,128 @@
+// <auto-generated />
+using System;
+using Microsoft.EntityFrameworkCore;
+using Microsoft.EntityFrameworkCore.Infrastructure;
+using Microsoft.EntityFrameworkCore.Metadata;
+using Microsoft.EntityFrameworkCore.Storage.ValueConversion;
+using RobotNet.RobotManager.Data;
+
+#nullable disable
+
+namespace RobotNet.RobotManager.Data.Migrations
+{
+    [DbContext(typeof(RobotEditorDbContext))]
+    partial class RobotEditorDbContextModelSnapshot : ModelSnapshot
+    {
+        protected override void BuildModel(ModelBuilder modelBuilder)
+        {
+#pragma warning disable 612, 618
+            modelBuilder
+                .HasAnnotation("ProductVersion", "9.0.4")
+                .HasAnnotation("Relational:MaxIdentifierLength", 128);
+
+            SqlServerModelBuilderExtensions.UseIdentityColumns(modelBuilder);
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.Robot", b =>
+                {
+                    b.Property<Guid>("Id")
+                        .ValueGeneratedOnAdd()
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("Id");
+
+                    b.Property<string>("ChargerNode")
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("ChargerNode");
+
+                    b.Property<string>("HomeNode")
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("HomeNode");
+
+                    b.Property<Guid>("MapId")
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("MapId");
+
+                    b.Property<Guid>("ModelId")
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("ModelId");
+
+                    b.Property<string>("Name")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("Name");
+
+                    b.Property<string>("RobotId")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("RobotId");
+
+                    b.HasKey("Id");
+
+                    b.HasIndex("ModelId");
+
+                    b.ToTable("Robots");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.RobotModel", b =>
+                {
+                    b.Property<Guid>("Id")
+                        .ValueGeneratedOnAdd()
+                        .HasColumnType("uniqueidentifier")
+                        .HasColumnName("Id");
+
+                    b.Property<int>("ImageHeight")
+                        .HasColumnType("int")
+                        .HasColumnName("ImageHeight");
+
+                    b.Property<int>("ImageWidth")
+                        .HasColumnType("int")
+                        .HasColumnName("ImageWidth");
+
+                    b.Property<double>("Length")
+                        .HasColumnType("float")
+                        .HasColumnName("Length");
+
+                    b.Property<string>("ModelName")
+                        .IsRequired()
+                        .HasColumnType("varchar(127)")
+                        .HasColumnName("ModelName");
+
+                    b.Property<int>("NavigationType")
+                        .HasColumnType("int")
+                        .HasColumnName("NavigationType");
+
+                    b.Property<double>("OriginX")
+                        .HasColumnType("float")
+                        .HasColumnName("OriginX");
+
+                    b.Property<double>("OriginY")
+                        .HasColumnType("float")
+                        .HasColumnName("OriginY");
+
+                    b.Property<double>("Width")
+                        .HasColumnType("float")
+                        .HasColumnName("Width");
+
+                    b.HasKey("Id");
+
+                    b.ToTable("RobotModels");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.Robot", b =>
+                {
+                    b.HasOne("RobotNet.RobotManager.Data.RobotModel", "Model")
+                        .WithMany("Robots")
+                        .HasForeignKey("ModelId")
+                        .OnDelete(DeleteBehavior.Restrict)
+                        .IsRequired();
+
+                    b.Navigation("Model");
+                });
+
+            modelBuilder.Entity("RobotNet.RobotManager.Data.RobotModel", b =>
+                {
+                    b.Navigation("Robots");
+                });
+#pragma warning restore 612, 618
+        }
+    }
+}

RobotNet.RobotManager/Data/Robot.cs

@@ -0,0 +1,40 @@
+using System.ComponentModel.DataAnnotations;
+using System.ComponentModel.DataAnnotations.Schema;
+
+namespace RobotNet.RobotManager.Data;
+
+#nullable disable
+
+[Table("Robots")]
+public class Robot
+{
+    [Column("Id", TypeName = "uniqueidentifier")]
+    [DatabaseGenerated(DatabaseGeneratedOption.Identity)]
+    [Key]
+    [Required]
+    public Guid Id { get; set; }
+
+    [Column("RobotId", TypeName = "varchar(127)")]
+    [Required]
+    public string RobotId { get; set; } = string.Empty;
+
+    [Column("Name", TypeName = "varchar(127)")]
+    [Required]
+    public string Name { get; set; }
+
+    [Column("ModelId", TypeName = "uniqueidentifier")]
+    [Required]
+    public Guid ModelId { get; set; }
+
+    [Column("MapId", TypeName = "uniqueidentifier")]
+    [Required]
+    public Guid MapId { get; set; }
+
+    [Column("HomeNode", TypeName = "varchar(127)")]
+    public string HomeNode { get; set; }
+
+    [Column("ChargerNode", TypeName = "varchar(127)")]
+    public string ChargerNode { get; set; }
+
+    public RobotModel Model { get; set; }
+}

RobotNet.RobotManager/Data/RobotEditorDbContext.cs

@@ -0,0 +1,21 @@
+using Microsoft.EntityFrameworkCore;
+
+namespace RobotNet.RobotManager.Data;
+
+public class RobotEditorDbContext(DbContextOptions<RobotEditorDbContext> options) : DbContext(options)
+{
+    public DbSet<Robot> Robots { get; private set; }
+    public DbSet<RobotModel> RobotModels { get; private set; }
+
+    protected override void OnModelCreating(ModelBuilder modelBuilder)
+    {
+        modelBuilder.Entity<RobotModel>()
+            .HasMany(e => e.Robots)
+            .WithOne(e => e.Model)
+            .HasForeignKey(e => e.ModelId)
+            .OnDelete(DeleteBehavior.Restrict)
+            .IsRequired();
+
+        base.OnModelCreating(modelBuilder);
+    }
+}

RobotNet.RobotManager/Data/RobotManagerDbExtensions.cs

@@ -0,0 +1,17 @@
+using Microsoft.EntityFrameworkCore;
+
+namespace RobotNet.RobotManager.Data;
+
+public static class RobotManagerDbExtensions
+{
+    public static async Task SeedRobotManagerDbAsync(this IServiceProvider serviceProvider)
+    {
+        using var scope = serviceProvider.GetRequiredService<IServiceScopeFactory>().CreateScope();
+
+        using var appDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+
+        await appDb.Database.MigrateAsync();
+        //await appDb.Database.EnsureCreatedAsync();
+        await appDb.SaveChangesAsync();
+    }
+}

RobotNet.RobotManager/Data/RobotModel.cs

@@ -0,0 +1,51 @@
+using RobotNet.RobotShares.Enums;
+using System.ComponentModel.DataAnnotations;
+using System.ComponentModel.DataAnnotations.Schema;
+
+namespace RobotNet.RobotManager.Data;
+
+#nullable disable
+
+[Table("RobotModels")]
+public class RobotModel
+{
+    [Column("Id", TypeName = "uniqueidentifier")]
+    [DatabaseGenerated(DatabaseGeneratedOption.Identity)]
+    [Key]
+    [Required]
+    public Guid Id { get; set; }
+
+    [Column("ModelName", TypeName = "varchar(127)")]
+    [Required]
+    public string ModelName { get; set; }
+
+    [Column("OriginX", TypeName = "float")]
+    [Required]
+    public double OriginX { get; set; }
+
+    [Column("OriginY", TypeName = "float")]
+    [Required]
+    public double OriginY { get; set; }
+
+    [Column("Length", TypeName = "float")]
+    [Required]
+    public double Length { get; set; }
+
+    [Column("Width", TypeName = "float")]
+    [Required]
+    public double Width { get; set; }
+
+    [Column("ImageWidth", TypeName = "int")]
+    [Required]
+    public int ImageWidth { get; set; }
+
+    [Column("ImageHeight", TypeName = "int")]
+    [Required]
+    public int ImageHeight { get; set; }
+
+    [Column("NavigationType", TypeName = "int")]
+    [Required]
+    public NavigationType NavigationType { get; set; }
+
+    public virtual ICollection<Robot> Robots { get; } = [];
+}

RobotNet.RobotManager/Dockerfile

@@ -0,0 +1,73 @@
+FROM alpine:3.22 AS base
+WORKDIR /app
+
+FROM mcr.microsoft.com/dotnet/sdk:9.0 AS build
+WORKDIR /src
+
+COPY ["RobotNet.RobotManager/RobotNet.RobotManager.csproj", "RobotNet.RobotManager/"]
+COPY ["RobotNet.RobotShares/RobotNet.RobotShares.csproj", "RobotNet.RobotShares/"]
+COPY ["RobotNet.MapShares/RobotNet.MapShares.csproj", "RobotNet.MapShares/"]
+COPY ["RobotNet.Script/RobotNet.Script.csproj", "RobotNet.Script/"]
+COPY ["RobotNet.Script.Expressions/RobotNet.Script.Expressions.csproj", "RobotNet.Script.Expressions/"]
+COPY ["RobotNet.Shares/RobotNet.Shares.csproj", "RobotNet.Shares/"]
+COPY ["RobotNet.OpenIddictClient/RobotNet.OpenIddictClient.csproj", "RobotNet.OpenIddictClient/"]
+COPY ["RobotNet.Clients/RobotNet.Clients.csproj", "RobotNet.Clients/"]
+
+# RUN dotnet package remove "Microsoft.EntityFrameworkCore.Tools" --project "RobotNet.RobotManager/RobotNet.RobotManager.csproj"
+RUN dotnet restore "RobotNet.RobotManager/RobotNet.RobotManager.csproj"
+
+COPY RobotNet.RobotManager/ RobotNet.RobotManager/
+COPY RobotNet.RobotShares/ RobotNet.RobotShares/
+COPY RobotNet.MapShares/ RobotNet.MapShares/
+COPY RobotNet.Script/ RobotNet.Script/
+COPY RobotNet.Script.Expressions/ RobotNet.Script.Expressions/
+COPY RobotNet.Shares/ RobotNet.Shares/
+COPY RobotNet.OpenIddictClient/ RobotNet.OpenIddictClient/
+COPY RobotNet.Clients/ RobotNet.Clients/
+
+RUN rm -rf ./RobotNet.RobotManager/bin
+RUN rm -rf ./RobotNet.RobotManager/obj
+RUN rm -rf ./RobotNet.RobotShares/bin
+RUN rm -rf ./RobotNet.RobotShares/obj
+RUN rm -rf ./RobotNet.MapShares/bin
+RUN rm -rf ./RobotNet.MapShares/obj
+RUN rm -rf ./RobotNet.Script/bin
+RUN rm -rf ./RobotNet.Script/obj
+RUN rm -rf ./RobotNet.Script.Expressions/bin
+RUN rm -rf ./RobotNet.Script.Expressions/obj
+RUN rm -rf ./RobotNet.Shares/bin
+RUN rm -rf ./RobotNet.Shares/obj
+RUN rm -rf ./RobotNet.OpenIddictClient/bin
+RUN rm -rf ./RobotNet.OpenIddictClient/obj
+RUN rm -rf ./RobotNet.Clients/bin
+RUN rm -rf ./RobotNet.Clients/obj
+
+WORKDIR "/src/RobotNet.RobotManager"
+RUN dotnet build "RobotNet.RobotManager.csproj" -c Release -o /app/build
+
+FROM build AS publish
+WORKDIR /src/RobotNet.RobotManager
+RUN dotnet publish "RobotNet.RobotManager.csproj" \ 
+    -c Release \
+    -o /app/publish \
+    --runtime linux-musl-x64 \
+    --self-contained true \
+    /p:PublishTrimmed=false \
+    /p:PublishReadyToRun=true
+
+FROM base AS final
+WORKDIR /app
+COPY --from=publish /app/publish ./
+
+RUN apk add --no-cache icu-libs tzdata ca-certificates
+
+RUN echo '#!/bin/sh' >> ./start.sh
+RUN echo 'update-ca-certificates' >> ./start.sh
+RUN echo 'exec ./RobotNet.RobotManager' >> ./start.sh
+
+RUN chmod +x ./RobotNet.RobotManager
+RUN chmod +x ./start.sh
+
+# Use the start script to ensure certificates are updated before starting the application
+EXPOSE 443
+ENTRYPOINT ["./start.sh"]

RobotNet.RobotManager/HubClients/MapHubClient.cs

@@ -0,0 +1,41 @@
+using Microsoft.AspNetCore.SignalR.Client;
+using OpenIddict.Client;
+using RobotNet.MapShares.Dtos;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.HubClients;
+
+public class MapHubClient(IHttpClientFactory HttpFactory, OpenIddictClientService openIddictClient) 
+    : OpenIddictHubClient(new Uri($"{HttpFactory.CreateClient("MapManagerAPI").BaseAddress}hubs/map/data").ToString(), openIddictClient, ["robotnet-map-api"])
+{
+    public event Action<Guid>? MapUpdated;
+    private IDisposable? disMapUpdated;
+
+
+    new public async Task StartAsync()
+    {
+        disMapUpdated = Connection.On<Guid>("MapUpdated", mapId => MapUpdated?.Invoke(mapId));
+
+        await base.StartAsync();
+    }
+
+    new public async Task StopAsync()
+    {
+        if (disMapUpdated != null)
+        {
+            disMapUpdated.Dispose();
+            disMapUpdated = null;
+        }
+        await base.StopAsync();
+    }
+
+    public async Task<MessageResult<MapDataDto>> GetMapData(Guid mapId)
+        => IsConnected ? await Connection.InvokeAsync<MessageResult<MapDataDto>>(nameof(GetMapData), mapId) : new(false, "Kết nối thất bại");
+
+    public async Task<MessageResult<ElementDto[]>> GetElementsState(Guid mapId)
+        => IsConnected ? await Connection.InvokeAsync<MessageResult<ElementDto[]>>(nameof(GetElementsState), mapId) : new(false, "Kết nối thất bại");
+    public async Task<MessageResult<MapInfoDto>> GetMapInfoByName(string name)
+        => IsConnected ? await Connection.InvokeAsync<MessageResult<MapInfoDto>>(nameof(GetMapInfoByName), name) : new(false, "Kết nối thất bại");
+    public async Task<MessageResult<MapInfoDto>> GetMapInfoById(Guid mapId)
+        => IsConnected ? await Connection.InvokeAsync<MessageResult<MapInfoDto>>(nameof(GetMapInfoById), mapId) : new(false, "Kết nối thất bại");
+}

RobotNet.RobotManager/HubClients/OpenIddictHubClient.cs

@@ -0,0 +1,23 @@
+using OpenIddict.Client;
+using RobotNet.Clients;
+
+namespace RobotNet.RobotManager.HubClients;
+
+public class OpenIddictHubClient(string url, OpenIddictClientService openIddictClient, string[] scopes) : HubClient(new Uri(url),
+    async () =>
+    {
+        var result = await openIddictClient.AuthenticateWithClientCredentialsAsync(new()
+        {
+            Scopes = [.. scopes],
+        });
+        if (result == null || result.AccessToken == null || result.AccessTokenExpirationDate == null)
+        {
+            return null;
+        }
+        else
+        {
+            return result.AccessToken;
+        }
+    })
+{
+}

RobotNet.RobotManager/Hubs/RobotHub.cs

@@ -0,0 +1,178 @@
+using Azure.Core.Serialization;
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.SignalR;
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotShares.VDA5050.InstantAction;
+using RobotNet.Shares;
+using System.Text.Json;
+
+namespace RobotNet.RobotManager.Hubs;
+
+[Authorize]
+public class RobotHub(RobotPublisher RobotPublisher, Services.RobotManager RobotManager, RobotEditorDbContext RobotDb, LoggerController<RobotHub> Logger) : Hub
+{
+    public async Task MapActive(Guid mapId)
+    {
+        var keysToRemove = RobotPublisher.MapActive.Where(kvp => kvp.Value == Context.ConnectionId)
+                            .Select(kvp => kvp.Key)
+                            .ToList();
+        foreach (var key in keysToRemove)
+        {
+            RobotPublisher.MapActive.Remove(key);
+        }
+
+        RobotPublisher.MapActive[mapId] = Context.ConnectionId;
+        await Clients.AllExcept([.. RobotPublisher.MapActive.Values]).SendAsync("MapDeactive");
+    }
+
+    public async Task RobotDetailActive(string robotId)
+    {
+        var keysToRemove = RobotPublisher.RobotDetailActive.Where(kvp => kvp.Value == Context.ConnectionId)
+                            .Select(kvp => kvp.Key)
+                            .ToList();
+
+        foreach (var key in keysToRemove)
+        {
+            RobotPublisher.RobotDetailActive.Remove(key);
+        }
+
+        RobotPublisher.RobotDetailActive[robotId] = Context.ConnectionId;
+        await Clients.AllExcept([.. RobotPublisher.RobotDetailActive.Values]).SendAsync("RobotDetailDeactive");
+    }
+
+    public MessageResult SetInitialPose(string robotId, double x, double y, double yaw)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            if (robot is not null)
+            {
+                robot.Initialize(x, y, yaw * 180 / Math.PI);
+                return new(true);
+            }
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"SetInitialPose: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+    public MessageResult MoveStraight(string robotId, double x, double y, double theta)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            if (robot is not null) return robot.MoveStraight(x, y);
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"MoveStraight: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult> MoveToNode(string robotId, string nodename)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            if (robot is not null) return await robot.MoveToNode(nodename);
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"MoveToNode: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult> MoveRandom(string robotId, List<string> nodes)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            if (robot is not null) return await robot.MoveRandom(nodes);
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"MoveToNode: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult> CancelNavigation(string robotId)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            if (robot is not null) return await robot.CancelOrder();
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"CancelNavigation: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult> SetMap(string robotId, Guid mapId)
+    {
+        try
+        {
+            var robot = RobotDb.Robots.FirstOrDefault(x => x.RobotId == robotId);
+            if (robot is not null)
+            {
+                robot.MapId = mapId;
+                await RobotDb.SaveChangesAsync();
+                return new(true);
+            }
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"SetMap: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult> SendAction(string robotId, ActionDto action)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            var actionVDA = JsonSerializer.Deserialize<RobotShares.VDA5050.InstantAction.Action>(action.Content, JsonOptionExtends.Read);
+            if (robot is not null)
+            {
+                if (actionVDA is null) return new(false, "Action không hợp lệ"); 
+                var send = await robot.InstantAction(actionVDA, false);
+                return new(send.IsSuccess, send.Message);
+            }
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"MoveToNode: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult> CancelAction(string robotId)
+    {
+        try
+        {
+            var robot = RobotManager[robotId];
+            if (robot is not null) return await robot.CancelAction();
+            return new(false, "Robot không tồn tại");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"CancelAction: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+}

RobotNet.RobotManager/Hubs/RobotManagerHub.cs

@@ -0,0 +1,244 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.SignalR;
+using Microsoft.EntityFrameworkCore;
+using RobotNet.RobotManager.Controllers;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotShares.Models;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Hubs;
+
+[Authorize]
+public class RobotManagerHub(RobotEditorDbContext RobotDb, MapManager MapManager, Services.RobotManager RobotManager, TrafficACS TrafficACS, ILogger<RobotManagerController> Logger) : Hub
+{
+    public async Task<MessageResult> MoveToNode(RobotMoveToNodeModel model)
+    {
+        try
+        {
+            if (string.IsNullOrEmpty(model.NodeName)) return new(false, "NodeName cannot be empty..");
+
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == model.RobotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var map = await MapManager.GetMapData(robot.MapId);
+            if (!map.IsSuccess) return new(false, map.Message);
+            if (map is null || map.Data is null) return new(false, "The robot has not been assigned a map.");
+
+            var node = map.Data.Nodes.FirstOrDefault(n => n.Name == model.NodeName && n.MapId == map.Data.Id);
+            if (node is null) return new(false, "This Node does not exist.");
+
+            if (robotController.IsWorking) return new(false, "The robot is busy.");
+
+            var move = await robotController.MoveToNode(node.Name, model.Actions, model.OverrideLastAngle ? model.LastAngle : null);
+            if (move.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} di chuyển tới node {model.NodeName} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} di chuyển tới node {model.NodeName} không thành công thành công do {move.Message}");
+            return new(false, move.Message);
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi MoveToNode Robot {RobotId} di chuyển tới node {NodeName} xảy ra lỗi: {Message}", model.RobotId, model.NodeName, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<MessageResult> MoveStraight(RobotMoveStraightModel model)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == model.RobotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+            if (robotController.IsWorking) return new(false, "The robot is busy.");
+
+            var move = robotController.MoveStraight(model.X, model.Y);
+            if (move.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} di chuyển tới tọa độ [{model.X} - {model.Y}] thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} di chuyển tới tọa độ [{model.X} - {model.Y}] không thành công thành công do {move.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi Rotate Robot {RobotId} di chuyển tới tọa độ [{model.X} - {model.Y}] xảy ra lỗi: {Message}", model.RobotId, model.X, model.Y, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<MessageResult> Rotate(RobotRotateModel model)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == model.RobotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+            if (robotController.IsWorking) return new(false, "The robot is busy.");
+
+            var move = robotController.Rotate(model.Angle);
+            if (move.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} quay tới góc {model.Angle} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Goi Robot {model.RobotId} quay tới góc {model.Angle} không thành công thành công do {move.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi Rotate Robot {RobotId} quay tới góc {model.Angle} xảy ra lỗi: {Message}", model.RobotId, model.Angle, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<MessageResult> CancelOrder(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == robotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var cancel = await robotController.CancelOrder();
+            if (cancel.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Hủy bỏ nhiệm vụ của Robot {robotId} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Hủy bỏ nhiệm vụ của Robot {robotId} không thành công do {cancel.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi Cancel Order Robot {RobotId} xảy ra lỗi: {Message}", robotId, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<MessageResult> CancelAction(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == robotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var cancel = await robotController.CancelAction();
+            if (cancel.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Hủy bỏ nhiệm vụ của Robot {robotId} thành công ");
+                return new(true);
+            }
+            robotController.Log($"RobotManager API Controller Log: Hủy bỏ nhiệm vụ của Robot {robotId} không thành công do {cancel.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi Cancel Order Robot {RobotId} xảy ra lỗi: {Message}", robotId, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<MessageResult<string>> InstantAction(RobotInstantActionModel model)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == model.RobotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null || !robotController.IsOnline) return new(false, "The robot is not online.");
+
+            var instantAction = await robotController.InstantAction(model.Action, false);
+            if (instantAction.IsSuccess)
+            {
+                robotController.Log($"RobotManager API Controller Log: Gửi Action Robot {model.RobotId} thành công ");
+                return instantAction;
+            }
+            robotController.Log($"RobotManager API Controller Log: Gửi Action Robot {model.RobotId} không thành công do {instantAction.Message}");
+            return new(false, "Request failed.");
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi InstantAction Robot {RobotId}, Action type {action} xảy ra lỗi: {Message}", model.RobotId, model.Action.ActionType, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<MessageResult<RobotStateModel>> GetState(string robotId)
+    {
+        try
+        {
+            var robot = await RobotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == robotId);
+            if (robot is null) return new(false, "RobotId does not exist.");
+
+            var robotController = RobotManager[robot.RobotId];
+            if (robotController is null) return new(true, "")
+            {
+                Data = new()
+                {
+                    RobotId = robotId,
+                    IsOnline = false,
+                    MapId = robot.MapId,
+                }
+            };
+
+            return new(true)
+            {
+                Data = new()
+                {
+                    RobotId = robotId,
+                    MapId = robot.MapId,
+                    IsOnline = robotController.IsOnline,
+                    State = robotController.State,
+                    OrderState = robotController.OrderState,
+                    NewBaseRequest = robotController.StateMsg.NewBaseRequest,
+                    NodeStates = [.. robotController.StateMsg.NodeStates],
+                    EdgeStates = [.. robotController.StateMsg.EdgeStates],
+                    Loads = [.. robotController.StateMsg.Loads],
+                    ActionStates = robotController.ActionStates,
+                    BatteryState = robotController.StateMsg.BatteryState,
+                    Errors = [.. robotController.StateMsg.Errors],
+                    Information = [.. robotController.StateMsg.Information],
+                    SafetyState = robotController.StateMsg.SafetyState,
+                    AgvPosition = robotController.VisualizationMsg.AgvPosition,
+                    Velocity = robotController.VisualizationMsg.Velocity,
+                }
+            };
+        }
+        catch (Exception ex)
+        {
+            Logger.LogError("RobotManager API Controller Log: Goi GetState Robot {RobotId} xảy ra lỗi: {Message}", robotId, ex.Message);
+            return new(false, "An error occurred.");
+        }
+    }
+
+    public async Task<bool> RequestACSIn(string robotId, string id)
+    {
+        var result = await TrafficACS.RequestIn(robotId, id);
+        return result.Data;
+    }
+
+    public async Task<bool> RequestACSOut(string robotId, string id)
+    {
+        var result = await TrafficACS.RequestOut(robotId, id);
+        return result.Data;
+    }
+}

RobotNet.RobotManager/Hubs/TrafficHub.cs

@@ -0,0 +1,70 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.SignalR;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Hubs;
+
+[Authorize]
+public class TrafficHub(TrafficPublisher TrafficPublisher, TrafficManager TrafficManager, MapManager MapManager, LoggerController<RobotHub> Logger) : Hub
+{
+    public async Task TrafficActive(Guid mapId)
+    {
+        var keysToRemove = TrafficPublisher.TrafficMapActive.Where(kvp => kvp.Value == Context.ConnectionId)
+                            .Select(kvp => kvp.Key)
+                            .ToList();
+        foreach (var key in keysToRemove)
+        {
+            TrafficPublisher.TrafficMapActive.Remove(key);
+        }
+
+        TrafficPublisher.TrafficMapActive[mapId] = Context.ConnectionId;
+        await Clients.AllExcept([.. TrafficPublisher.TrafficMapActive.Values]).SendAsync("TrafficManagerDeactive");
+    }
+
+    public async Task<MessageResult<IEnumerable<TrafficMapDto>>> LoadTrafficMaps()
+    {
+        try
+        {
+            List<TrafficMapDto> trafficMaps = [];
+            foreach (var trafficMap in TrafficManager.TrafficMaps)
+            {
+                var map = await MapManager.GetMapData(trafficMap.Key);
+                trafficMaps.Add(new()
+                {
+                    MapId = trafficMap.Key,
+                    Agents = TrafficPublisher.GetAgents(trafficMap.Key),
+                    MapName = map.Data is null ? "" : map.Data.Name,
+                });
+            }
+            return new(true) { Data = trafficMaps };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"LoadTrafficMaps: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public async Task<MessageResult<TrafficMapDto>> LoadTrafficMap(Guid mapId)
+    {
+        try
+        {
+            var map = await MapManager.GetMapData(mapId);
+            var trafficMap = new TrafficMapDto()
+            {
+                MapId = mapId,
+                Agents = TrafficPublisher.GetAgents(mapId),
+                MapName = map.Data is null ? "" : map.Data.Name,
+            };
+            return new(true) { Data = trafficMap };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"LoadTrafficMaps: Hệ thống có lỗi xảy ra - {ex.Message}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+}

RobotNet.RobotManager/Program.cs

@@ -0,0 +1,159 @@
+using Microsoft.EntityFrameworkCore;
+using NLog.Web;
+using OpenIddict.Client;
+using OpenIddict.Validation.AspNetCore;
+using RobotNet.OpenIddictClient;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.HubClients;
+using RobotNet.RobotManager.Hubs;
+using RobotNet.RobotManager.Services;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotManager.Services.Traffic;
+using System.Net.Http.Headers;
+using static OpenIddict.Abstractions.OpenIddictConstants;
+
+var builder = WebApplication.CreateBuilder(args);
+builder.Host.UseNLog();
+
+//builder.AddServiceDefaults();
+
+// Add services to the container.
+
+builder.Services.AddControllers();
+builder.Services.AddSignalR();
+
+var openIddictOption = builder.Configuration.GetSection(nameof(OpenIddictClientProviderOptions)).Get<OpenIddictClientProviderOptions>()
+    ?? throw new InvalidOperationException("OpenID configuration not found or invalid format.");
+
+builder.Services.AddOpenIddict()
+    .AddValidation(options =>
+    {
+        // Note: the validation handler uses OpenID Connect discovery
+        // to retrieve the address of the introspection endpoint.
+        options.SetIssuer(openIddictOption.Issuer);
+        options.AddAudiences(openIddictOption.Audiences);
+
+        // Configure the validation handler to use introspection and register the client
+        // credentials used when communicating with the remote introspection endpoint.
+        options.UseIntrospection()
+               .SetClientId(openIddictOption.ClientId)
+               .SetClientSecret(openIddictOption.ClientSecret);
+
+        // Register the System.Net.Http integration.
+        if (builder.Environment.IsDevelopment())
+        {
+            options.UseSystemNetHttp(httpOptions =>
+            {
+                httpOptions.ConfigureHttpClientHandler(context =>
+                {
+                    context.ServerCertificateCustomValidationCallback = (message, cert, chain, sslPolicyErrors) => true;
+                });
+            });
+        }
+        else
+        {
+            options.UseSystemNetHttp();
+        }
+
+        // Register the ASP.NET Core host.
+        options.UseAspNetCore();
+    })
+     .AddClient(options =>
+     {
+         // Allow grant_type=client_credentials to be negotiated.
+         options.AllowClientCredentialsFlow();
+
+         // Disable token storage, which is not necessary for non-interactive flows like
+         // grant_type=password, grant_type=client_credentials or grant_type=refresh_token.
+         options.DisableTokenStorage();
+
+         // Register the System.Net.Http integration and use the identity of the current
+         // assembly as a more specific user agent, which can be useful when dealing with
+         // providers that use the user agent as a way to throttle requests (e.g Reddit).
+         options.UseSystemNetHttp()
+                .SetProductInformation(typeof(Program).Assembly);
+
+         var registration = new OpenIddictClientRegistration
+         {
+             Issuer = new Uri(openIddictOption.Issuer, UriKind.Absolute),
+             GrantTypes = { GrantTypes.ClientCredentials },
+             ClientId = openIddictOption.ClientId,
+             ClientSecret = openIddictOption.ClientSecret,
+         };
+
+         foreach (var scope in openIddictOption.Scopes)
+         {
+             registration.Scopes.Add(scope);
+         }
+
+         // Add a client registration matching the client application definition in the server project.
+         options.AddRegistration(registration);
+     });
+
+builder.Services.AddAuthentication(OpenIddictValidationAspNetCoreDefaults.AuthenticationScheme);
+builder.Services.AddAuthorization();
+
+builder.Services.AddCors(options =>
+{
+    options.AddDefaultPolicy(policy =>
+    {
+        policy.AllowAnyOrigin()
+            .AllowAnyMethod()
+            .AllowAnyHeader();
+    });
+});
+
+var mapManagerOptions = builder.Configuration.GetSection("MapManager").Get<OpenIddictResourceOptions>()
+    ?? throw new InvalidOperationException("OpenID configuration not found or invalid format.");
+
+//builder.Services.AddHttpContextAccessor();
+builder.Services.AddSingleton<MapManagerAccessTokenHandler>();
+builder.Services.AddHttpClient("MapManagerAPI", client =>
+{
+    client.BaseAddress = new Uri(mapManagerOptions.Url);
+    client.DefaultRequestHeaders.Accept.Add(new MediaTypeWithQualityHeaderValue("application/json"));
+}).AddHttpMessageHandler<MapManagerAccessTokenHandler>();
+
+var connectionString = builder.Configuration.GetConnectionString("DefaultConnection") ?? throw new InvalidOperationException("Connection string 'DefaultConnection' not found.");
+Action<DbContextOptionsBuilder> appDbOptions = options => options.UseSqlServer(connectionString, b => b.MigrationsAssembly("RobotNet.RobotManager"));
+builder.Services.AddDbContext<RobotEditorDbContext>(appDbOptions);
+
+builder.Services.AddTransient<MapHubClient>();
+builder.Services.AddSingleton<RobotEditorStorageRepository>();
+builder.Services.AddSingleton(typeof(LoggerController<>));
+builder.Services.AddSingleton<TrafficManager>();
+builder.Services.AddHostedService<MqttBroker>();
+builder.Services.AddSingleton<PathPlanner>();
+builder.Services.AddSingleton<RobotManager>();
+builder.Services.AddSingleton<MapManager>();
+builder.Services.AddSingleton<RobotPublisher>();
+builder.Services.AddSingleton<TrafficPublisher>();
+builder.Services.AddHostedService(sp => sp.GetRequiredService<TrafficPublisher>());
+builder.Services.AddHostedService(sp => sp.GetRequiredService<RobotPublisher>());
+builder.Services.AddHostedService(sp => sp.GetRequiredService<RobotManager>());
+builder.Services.AddHostedService(sp => sp.GetRequiredService<MapManager>());
+builder.Services.AddHostedService(sp => sp.GetRequiredService<TrafficManager>());
+builder.Services.AddSingleton<TrafficACS>();
+builder.Services.AddSingleton<OpenACSManager>();
+builder.Services.AddSingleton<OpenACSPublisher>();
+builder.Services.AddHostedService(sp => sp.GetRequiredService<OpenACSManager>());
+builder.Services.AddHostedService(sp => sp.GetRequiredService<OpenACSPublisher>());
+
+var app = builder.Build();
+await app.Services.SeedRobotManagerDbAsync();
+// Configure the HTTP request pipeline.
+
+app.UseHttpsRedirection();
+
+app.UseCors();
+
+app.UseAuthentication();
+app.UseAuthorization();
+
+app.MapControllers();
+
+app.MapHub<RobotHub>("/hubs/robot/online");
+app.MapHub<TrafficHub>("/hubs/traffic");
+app.MapHub<RobotManagerHub>("/hubs/robot-manager");
+
+app.Run();

RobotNet.RobotManager/Properties/launchSettings.json

@@ -0,0 +1,15 @@
+{
+  "$schema": "https://json.schemastore.org/launchsettings.json",
+  "profiles": {
+    "https": {
+      "commandName": "Project",
+      "dotnetRunMessages": true,
+      "launchBrowser": false,
+      "workingDirectory": "$(TargetDir)",
+      "applicationUrl": "https://localhost:7179",
+      "environmentVariables": {
+        "ASPNETCORE_ENVIRONMENT": "Development"
+      }
+    }
+  }
+}

RobotNet.RobotManager/RobotNet.RobotManager.csproj

@@ -0,0 +1,35 @@
+<Project Sdk="Microsoft.NET.Sdk.Web">
+
+	<PropertyGroup>
+		<TargetFramework>net9.0</TargetFramework>
+		<Nullable>enable</Nullable>
+		<ImplicitUsings>enable</ImplicitUsings>
+	</PropertyGroup>
+
+	<ItemGroup>
+		<ProjectReference Include="..\RobotNet.Clients\RobotNet.Clients.csproj" />
+		<ProjectReference Include="..\RobotNet.MapShares\RobotNet.MapShares.csproj" />
+		<ProjectReference Include="..\RobotNet.OpenIddictClient\RobotNet.OpenIddictClient.csproj" />
+		<ProjectReference Include="..\RobotNet.RobotShares\RobotNet.RobotShares.csproj" />
+	</ItemGroup>
+
+	<ItemGroup>
+		<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Authentication" Version="9.0.8" />
+		<PackageReference Include="Microsoft.EntityFrameworkCore.SqlServer" Version="9.0.8" />
+		<PackageReference Include="Microsoft.EntityFrameworkCore.Tools" Version="9.0.8">
+			<PrivateAssets>all</PrivateAssets>
+			<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
+		</PackageReference>
+		<PackageReference Include="NLog" Version="6.0.3" />
+		<PackageReference Include="NLog.Web.AspNetCore" Version="6.0.3" />
+		<PackageReference Include="Minio" Version="6.0.5" />
+		<PackageReference Include="MQTTnet" Version="5.0.1.1416" />
+		<PackageReference Include="MQTTnet.Server" Version="5.0.1.1416" />
+		<PackageReference Include="OpenIddict.Client.SystemNetHttp" Version="7.0.0" />
+		<PackageReference Include="OpenIddict.Validation.AspNetCore" Version="7.0.0" />
+		<PackageReference Include="OpenIddict.Validation.SystemNetHttp" Version="7.0.0" />
+		<PackageReference Include="Serialize.Linq" Version="4.0.167" />
+		<PackageReference Include="SixLabors.ImageSharp" Version="3.1.11" />
+	</ItemGroup>
+
+</Project>

RobotNet.RobotManager/RobotNet.RobotManager.http

@@ -0,0 +1,6 @@
+@RobotNet.RobotManager_HostAddress = http://localhost:5167
+
+GET {{RobotNet.RobotManager_HostAddress}}/weatherforecast/
+Accept: application/json
+
+###

RobotNet.RobotManager/Services/IRobotController.cs

@@ -0,0 +1,36 @@
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.VDA5050.Factsheet;
+using RobotNet.RobotShares.VDA5050.FactsheetExtend;
+using RobotNet.RobotShares.VDA5050.State;
+using RobotNet.RobotShares.VDA5050.Visualization;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services;
+
+public interface IRobotController
+{
+    string SerialNumber { get; }
+    bool IsOnline { get; set; }
+    bool IsWorking { get; }
+    string State { get; }
+    RobotOrderDto OrderState { get; }
+    RobotActionDto[] ActionStates { get; }
+    AutoResetEvent RobotUpdated { get; set; }
+    StateMsg StateMsg { get; set; }
+    VisualizationMsg VisualizationMsg { get; set; }
+    FactSheetMsg FactSheetMsg { get; set; }
+    FactsheetExtendMsg FactsheetExtendMsg { get; set; }
+    NavigationPathEdge[] BasePath { get; }
+    NavigationPathEdge[] FullPath { get; }
+    string[] CurrentZones { get; }
+    void Log(string message, LogLevel level = LogLevel.Information);
+    Task<MessageResult<string>> InstantAction(RobotNet.RobotShares.VDA5050.InstantAction.Action action, bool waittingFisished);
+    Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<RobotShares.VDA5050.InstantAction.Action>>? actions = null, double? lastAngle = null);
+    Task<MessageResult> MoveRandom(List<string> nodes);
+    Task<MessageResult> CancelOrder();
+    Task<MessageResult> CancelAction();
+    void Initialize(double x, double y, double theta);
+    MessageResult Rotate(double angle);
+    MessageResult MoveStraight(double x, double y);
+    void Dispose();
+}

RobotNet.RobotManager/Services/IRobotOrder.cs

@@ -0,0 +1,15 @@
+using RobotNet.RobotShares.Dtos;
+
+namespace RobotNet.RobotManager.Services;
+
+public interface IRobotOrder
+{
+    bool IsError { get; }
+    bool IsCompleted { get; }
+    bool IsProcessing { get; }
+    bool IsCanceled { get; }
+    string[] Errors { get; }
+    NavigationPathEdge[] FullPath { get; }
+    NavigationPathEdge[] BasePath { get; }
+    void CreateComledted();
+}

RobotNet.RobotManager/Services/JsonOptionExtends.cs

@@ -0,0 +1,18 @@
+using System.Text.Json;
+
+namespace RobotNet.RobotManager.Services;
+
+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,
+    };
+}

RobotNet.RobotManager/Services/LoggerController.cs

@@ -0,0 +1,108 @@
+namespace RobotNet.RobotManager.Services;
+
+public class LoggerController<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);
+    }
+}

RobotNet.RobotManager/Services/MapManager.cs

@@ -0,0 +1,267 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.HubClients;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services;
+
+public class MapManager(IServiceProvider ServiceProvider, LoggerController<MapManager> Logger) : BackgroundService
+{
+    private MapHubClient? MapHub;
+    private readonly Dictionary<Guid, MapDataDto> MapData = [];
+
+    public async Task<MessageResult<MapDataDto?>> GetMapData(Guid mapId)
+    {
+        try
+        {
+            if (MapData.TryGetValue(mapId, out MapDataDto? mapData) && mapData is not null)
+            {
+                if (!mapData.Active) return new(false, "Bản đồ chưa được active");
+                return new(true) { Data = mapData };
+            }
+            if (MapHub is null || !MapHub.IsConnected)
+                return new(false, "Chưa khởi tạo kết nối với quản lí bản đồ");
+            var mapDataResult = await MapHub.GetMapData(mapId);
+            if (mapDataResult is not null && mapDataResult.Data is not null)
+            {
+                if (!mapDataResult.IsSuccess) return new(false, mapDataResult.Message);
+                MapData[mapId] = mapDataResult.Data;
+                return new(true) { Data = mapDataResult.Data };
+            }
+            return new(false, "Không thể lấy dữ liệu bản đồ");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetMapData: Lấy dữ liệu bản đồ có lỗi xảy ra: {ex.Message}");
+            return new(false, $"GetMapData: Lấy dữ liệu bản đồ có lỗi xảy ra: {ex.Message}");
+        }
+    }
+
+    public async Task<MessageResult<MapInfoDto>> GetMapInfo(Guid mapId)
+    {
+        try
+        {
+            if (MapHub is null || !MapHub.IsConnected)
+                return new(false, "Chưa khởi tạo kết nối với quản lí bản đồ");
+            var mapInfoResult =await MapHub.GetMapInfoById(mapId);
+            if (mapInfoResult is not null && mapInfoResult.Data is not null)
+            {
+                if (!mapInfoResult.IsSuccess) return new(false, mapInfoResult.Message);
+                return new(true) { Data = mapInfoResult.Data };
+            }
+            return new(false, "Không thể lấy dữ liệu bản đồ");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetMapInfo: Lấy dữ liệu bản đồ có lỗi xảy ra: {ex.Message}");
+            return new(false, $"GetMapInfo: Lấy dữ liệu bản đồ có lỗi xảy ra: {ex.Message}");
+        }
+    }
+
+    public async Task<MessageResult<MapInfoDto>> GetMapInfo(string mapName)
+    {
+        try
+        {
+            if (MapHub is null || !MapHub.IsConnected)
+                return new(false, "Chưa khởi tạo kết nối với quản lí bản đồ");
+            var mapInfoResult = await MapHub.GetMapInfoByName(mapName);
+            if (mapInfoResult is not null && mapInfoResult.Data is not null)
+            {
+                if (!mapInfoResult.IsSuccess) return new(false, mapInfoResult.Message);
+                return new(true) { Data = mapInfoResult.Data };
+            }
+            return new(false, "Không thể lấy dữ liệu bản đồ");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"GetMapInfo: Lấy dữ liệu bản đồ có lỗi xảy ra: {ex.Message}");
+            return new(false, $"GetMapInfo: Lấy dữ liệu bản đồ có lỗi xảy ra: {ex.Message}");
+        }
+    }
+
+    public NodeDto[] GetNegativeNodes(Guid mapId, Guid nodeId)
+    {
+        var mapData = Task.Run(async () => await GetMapData(mapId));
+        mapData.Wait(); 
+        if (mapData is null || !mapData.Result.IsSuccess || mapData.Result.Data is null) return [];
+        var map = mapData.Result.Data;  
+        var ListNodesNegative = new List<NodeDto>();
+        var ListPaths = map.Edges.Where(p => p.EndNodeId == nodeId || p.StartNodeId == nodeId);
+        foreach (var path in ListPaths)
+        {
+            if (path.StartNodeId == nodeId && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                var nodeAdd = map.Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
+                if (nodeAdd is not null) ListNodesNegative.Add(nodeAdd);
+                continue;
+            }
+            if (path.EndNodeId == nodeId && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                var nodeAdd = map.Nodes.FirstOrDefault(p => p.Id == path.StartNodeId);
+                if (nodeAdd is not null) ListNodesNegative.Add(nodeAdd);
+                continue;
+            }
+        }
+        return [.. ListNodesNegative];
+    }
+
+    public EdgeDto[] GetEdges(Guid mapId, NodeDto[] nodes)
+    {
+        if (nodes.Length < 2) return [];
+        if (MapData.TryGetValue(mapId, out MapDataDto? mapData) && mapData is not null)
+        {
+            List<EdgeDto> edges = [];
+            for (int i = 0; i < nodes.Length - 1; i++)
+            {
+                var edge = mapData.Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
+                                                        e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
+                if (edge is null)
+                {
+                    if (i != 0) return [];
+                    edges.Add(new EdgeDto()
+                    {
+                        Id = Guid.NewGuid(),
+                        StartNodeId = nodes[i].Id,
+                        EndNodeId = nodes[i + 1].Id,
+                        DirectionAllowed = DirectionAllowed.Both,
+                        TrajectoryDegree = TrajectoryDegree.One,
+                    });
+                    continue;
+                }
+                bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three;
+                edges.Add(new()
+                {
+                    Id = edge.Id,
+                    StartNodeId = nodes[i].Id,
+                    EndNodeId = nodes[i + 1].Id,
+                    DirectionAllowed = edge.DirectionAllowed,
+                    TrajectoryDegree = edge.TrajectoryDegree,
+                    ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                    ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                    ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                    ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+                });
+            }
+            return [.. edges];
+        }
+        return [];
+    }
+
+    public EdgeDto? GetEdge(Guid startNodeId, Guid endNodeId, Guid mapId)
+    {
+        if (MapData.TryGetValue(mapId, out MapDataDto? mapData) && mapData is not null)
+        {
+            var edge = mapData.Edges.FirstOrDefault(e => (e.StartNodeId == startNodeId && e.EndNodeId == endNodeId) || (e.StartNodeId == endNodeId && e.EndNodeId == startNodeId));
+            if (edge is not null)
+            {
+                bool isReverse = startNodeId != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three;
+                return new EdgeDto
+                {
+                    Id = edge.Id,
+                    StartNodeId = startNodeId,
+                    EndNodeId = endNodeId,
+                    TrajectoryDegree = edge.TrajectoryDegree,
+                    ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                    ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                    ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                    ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+                };
+            }
+        }
+        return null;
+    }
+
+    public NodeDto[][] GetNegativePaths(Guid mapId, NodeDto node, double distance)
+    {
+        var negativePaths = new Dictionary<Guid, NodeDto[]>();
+        var currentPath = new List<NodeDto> { node };
+        var visitedNodes = new Dictionary<Guid, double> { { node.Id, 0 } };
+
+        void DFS(NodeDto currentNode, double currentDistance, NodeDto[] path)
+        {
+            var negatvieNodes = GetNegativeNodes(mapId, currentNode.Id);
+            foreach (var negativeNode in negatvieNodes)
+            {
+                if (IsHasElement(mapId, negativeNode.Id)) continue;
+                NodeDto[] newPath = [.. path, negativeNode];
+                var newDistance = currentDistance + Math.Sqrt(Math.Pow(currentNode.X - negativeNode.X, 2) + Math.Pow(currentNode.Y - negativeNode.Y, 2));
+                if (visitedNodes.TryGetValue(negativeNode.Id, out double oldDistance))
+                {
+                    if (oldDistance > newDistance)
+                    {
+                        visitedNodes.Remove(negativeNode.Id);
+                        visitedNodes.Add(negativeNode.Id, newDistance);
+                        negativePaths.Remove(negativeNode.Id);
+                        if (newDistance >= distance) negativePaths.Add(negativeNode.Id, [.. newPath]);
+                        else DFS(negativeNode, newDistance, newPath);
+                    }
+                }
+                else
+                {
+                    visitedNodes.Add(negativeNode.Id, newDistance);
+                    if (newDistance >= distance) negativePaths.Add(negativeNode.Id, [.. newPath]);
+                    else DFS(negativeNode, newDistance, newPath);
+                }
+            }
+        }
+        DFS(node, 0, [.. currentPath]);
+        //foreach (var key in negativePaths.Keys.ToList())
+        //{
+        //    var path = negativePaths[key];
+        //    if (path != null && path.Length > 0) negativePaths[key] = [..path.Skip(1)];
+        //    else negativePaths.Remove(key);
+        //}
+        return [.. negativePaths.Values];
+    }
+
+    private bool IsHasElement(Guid mapId, Guid nodeId)
+    {
+        var mapData = Task.Run(async () => await GetMapData(mapId));
+        mapData.Wait();
+        if (mapData is null || !mapData.Result.IsSuccess || mapData.Result.Data is null) return false;
+        var map = mapData.Result.Data;
+        return map.Elements.Any(e => e.NodeId == nodeId);
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            try
+            {
+                using var scope = ServiceProvider.CreateAsyncScope();
+                MapHub = scope.ServiceProvider.GetRequiredService<MapHubClient>();
+
+                MapHub.MapUpdated += MapHub_Updated;
+                await MapHub.StartAsync();
+                break;
+            }
+
+            catch (Exception ex)
+            {
+                Logger.Warning($"Map Manager Execute: Khởi tạo kết nối với MapManager có lỗi xảy ra: {ex.Message}");
+                await Task.Delay(2000, stoppingToken);
+            }
+        }
+    }
+
+    private void MapHub_Updated(Guid mapId)
+    {
+        Logger.Info($"Map update active: {mapId}");
+        if (MapHub is null || !MapHub.IsConnected) return;
+        var mapDataResult = MapHub.GetMapData(mapId).Result;
+        if (mapDataResult is not null && mapDataResult.Data is not null)
+        {
+            if (!mapDataResult.IsSuccess) return;
+            MapData[mapId] = mapDataResult.Data;
+        }
+    }
+
+    public override async Task StopAsync(CancellationToken cancellationToken)
+    {
+        if (MapHub is not null && MapHub.IsConnected) await MapHub.StopAsync();
+        await base.StopAsync(cancellationToken);
+    }
+}

RobotNet.RobotManager/Services/MapManagerAccessTokenHandler.cs

@@ -0,0 +1,33 @@
+using OpenIddict.Client;
+using RobotNet.OpenIddictClient;
+using System.Net.Http.Headers;
+
+namespace RobotNet.RobotManager.Services;
+
+public class MapManagerAccessTokenHandler(OpenIddictClientService openIddictClient, IConfiguration configuration) : DelegatingHandler
+{
+    private readonly OpenIddictResourceOptions options = configuration.GetSection("MapManager").Get<OpenIddictResourceOptions>() ?? throw new InvalidOperationException("OpenID configuration not found or invalid format.");
+    private string? CachedToken;
+    private DateTime TokenExpiry;
+
+    protected override async Task<HttpResponseMessage> SendAsync(HttpRequestMessage request, CancellationToken cancellationToken)
+    {
+        if (string.IsNullOrEmpty(CachedToken) || DateTime.UtcNow >= TokenExpiry)
+        {
+            var result = await openIddictClient.AuthenticateWithClientCredentialsAsync(new()
+            {
+                Scopes = [.. options.Scopes],
+            });
+
+            if (result != null && !string.IsNullOrEmpty(result.AccessToken) && result.AccessTokenExpirationDate != null)
+            {
+                TokenExpiry = result.AccessTokenExpirationDate.Value.UtcDateTime;
+                CachedToken = result.AccessToken;
+            }
+        }
+
+        request.Headers.Authorization = new AuthenticationHeaderValue("Bearer", CachedToken);
+
+        return await base.SendAsync(request, cancellationToken);
+    }
+}

RobotNet.RobotManager/Services/MqttBroker.cs

@@ -0,0 +1,119 @@
+using MQTTnet;
+using MQTTnet.Protocol;
+using MQTTnet.Server;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotShares.VDA5050;
+using RobotNet.RobotShares.VDA5050.Connection;
+using System.Text.Json;
+
+namespace RobotNet.RobotManager.Services;
+
+public class MqttBroker : IHostedService
+{
+    private readonly VDA5050Setting VDA5050Setting = new();
+
+    public event Action<string>? NewClientConnected;
+    public event Action<string>? NewClientDisconnected;
+    public uint ConnectionHeaderId = 0;
+
+    private MqttServer? MqttServer;
+    private readonly IServiceProvider ServiceProvider;
+
+    public MqttBroker(IServiceProvider serviceProvider, IConfiguration configuration)
+    {
+        configuration.Bind("VDA5050Setting", VDA5050Setting);
+        ServiceProvider = serviceProvider;
+    }
+
+    public MqttConnectReasonCode ValidatingConnection(ValidatingConnectionEventArgs arg)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+        if (!RobotDb.Robots.Any(robot => arg.ClientId.ToLower() == robot.RobotId.ToString().ToLower()) && arg.ClientId != "FleetManager" && arg.ClientId != "OpenACS") 
+            return MqttConnectReasonCode.ClientIdentifierNotValid;
+        if (!arg.UserName.Equals(VDA5050Setting.UserName, StringComparison.Ordinal) || !arg.Password.Equals(VDA5050Setting.Password, StringComparison.Ordinal))
+            return MqttConnectReasonCode.BadUserNameOrPassword;
+        return MqttConnectReasonCode.Success;
+    }
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        if (!VDA5050Setting.ServerEnable) return;
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                var mqttFactory = new MqttServerFactory();
+
+                var mqttServerOptions = new MqttServerOptionsBuilder().WithDefaultEndpoint()
+                                        .WithDefaultCommunicationTimeout(TimeSpan.FromSeconds(VDA5050Setting.ConnectionTimeoutSeconds))
+                                        .WithTcpKeepAliveInterval(VDA5050Setting.KeepAliveInterval)
+                                        .WithConnectionBacklog(VDA5050Setting.ConnectionBacklog)
+                                        .Build();
+
+                MqttServer = mqttFactory.CreateMqttServer(mqttServerOptions);
+                MqttServer.ValidatingConnectionAsync += e =>
+                {
+                    var validate = ValidatingConnection(e);
+                    if (validate != MqttConnectReasonCode.Success) e.ReasonCode = validate;
+
+                    return Task.CompletedTask;
+                };
+
+                var connection = new ConnectionMsg()
+                {
+                    Manufacturer = VDA5050Setting.Manufacturer,
+                    Version = VDA5050Setting.Version,
+                };
+
+                MqttServer.ClientConnectedAsync += e =>
+                {
+                    connection.HeaderId = ++ConnectionHeaderId;
+                    connection.SerialNumber = e.ClientId;
+                    connection.Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
+                    connection.ConnectionState = ConnectionState.ONLINE.ToString();
+                    var payload = JsonSerializer.Serialize(connection);
+                    var applicationMessage = new MqttApplicationMessageBuilder()
+                        .WithTopic("connection")
+                        .WithPayload(payload)
+                        .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
+                        .Build();
+                    MqttServer.InjectApplicationMessage(new(applicationMessage));
+                    NewClientConnected?.Invoke(e.ClientId);
+                    return Task.CompletedTask;
+                };
+
+                MqttServer.ClientDisconnectedAsync += e =>
+                {
+                    connection.HeaderId = ++ConnectionHeaderId;
+                    connection.SerialNumber = e.ClientId;
+                    connection.Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
+                    connection.ConnectionState = ConnectionState.OFFLINE.ToString();
+
+                    var payload = JsonSerializer.Serialize(connection);
+                    var applicationMessage = new MqttApplicationMessageBuilder()
+                        .WithTopic("connection")
+                        .WithPayload(payload)
+                        .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
+                        .Build();
+                    MqttServer.InjectApplicationMessage(new(applicationMessage));
+                    NewClientDisconnected?.Invoke(e.ClientId);
+                    return Task.CompletedTask;
+                };
+
+                await MqttServer.StartAsync();
+                return;
+            }
+            catch
+            {
+                await Task.Delay(1000, cancellationToken);
+                continue;
+            }
+        }
+    }
+
+    public async Task StopAsync(CancellationToken cancellationToken)
+    {
+        await MqttServer.StopAsync();
+    }
+}

RobotNet.RobotManager/Services/OpenACS/ACSHeader.cs

@@ -0,0 +1,16 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class ACSHeader(string messageName, string time)
+{
+    [JsonPropertyName("msgname")]
+    [Required]
+    public string MessageName { get; set; } = messageName;
+
+    [JsonPropertyName("time")]
+    [Required]
+    public string Time { get; set; } = time;
+
+}

RobotNet.RobotManager/Services/OpenACS/ACSStatusResponse.cs

@@ -0,0 +1,24 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class ACSStatusBodyResponse
+{
+    [JsonPropertyName("result")]
+    [Required]
+    public string Result { get; set; } = string.Empty;
+
+}
+
+public class ACSStatusResponse
+{
+    [JsonPropertyName("header")]
+    [Required]
+    public ACSHeader Header { get; set; } = new("", DateTime.MinValue.ToString("yyyy-MM-dd HH:mm:ss.fff"));
+
+
+    [JsonPropertyName("body")]
+    [Required]
+    public ACSStatusBodyResponse Body { get; set; } = new();
+}

RobotNet.RobotManager/Services/OpenACS/AGVLocation.cs

@@ -0,0 +1,23 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class AGVLocation
+{
+    [JsonPropertyName("world_x")]
+    [Required]
+    public string X { get; set; } = "0";
+
+    [JsonPropertyName("world_y")]
+    [Required]
+    public string Y { get; set; } = "0";
+
+    [JsonPropertyName("world_z")]
+    [Required]
+    public string Z { get; set; } = "0";
+
+    [JsonPropertyName("direction")]
+    [Required]
+    public string Direction { get; set; } = "0";
+}

RobotNet.RobotManager/Services/OpenACS/AGVState.cs

@@ -0,0 +1,15 @@
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public enum AGVState
+{
+    Offline = -1,
+    Error = 0,
+    Idle = 1,
+    Processing = 2,
+    Pause = 3,
+    DockingFail = 4,
+    NoPose = 5,
+    Charging = 6,
+    Run = 7,
+    Stop = 8,
+}

RobotNet.RobotManager/Services/OpenACS/OpenACSException.cs

@@ -0,0 +1,8 @@
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class OpenACSException : Exception
+{
+    public OpenACSException() { }
+    public OpenACSException(string message) : base(message) { }
+    public OpenACSException(string message, Exception innerException) : base(message, innerException) { }
+}

RobotNet.RobotManager/Services/OpenACS/OpenACSManager.cs

@@ -0,0 +1,102 @@
+using System.Text.Json;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class OpenACSManager : BackgroundService
+{
+    public bool TrafficEnable => Config.TrafficEnable;
+    public string TrafficURL => Config.TrafficURL ?? "";
+    public string[] TrafficURLUsed => [..Config.TrafficURLUsed ?? []];
+    public bool PublishEnable => Config.PublishEnable;
+    public string PublishURL => Config.PublishURL ?? "";
+    public string[] PublishURLUsed => [..Config.PublishURLUsed ?? []];
+    public int PublishInterval => Config.PublishInterval;
+    public event Action? PublishIntervalChanged;
+
+    private ConfigData Config;
+    private const string DataPath = "openACSConfig.json";
+    private struct ConfigData
+    {
+        public string PublishURL { get; set; }
+        public List<string> PublishURLUsed { get; set; } 
+        public bool PublishEnable { get; set; }
+        public string TrafficURL { get; set; }
+        public List<string> TrafficURLUsed { get; set; }
+        public bool TrafficEnable { get; set; }
+        public int PublishInterval { get; set; }
+    }
+
+    public async Task UpdateTrafficURL(string url)
+    {
+        if (url == Config.TrafficURL) return;
+
+        Config.TrafficURL = url;
+        Config.TrafficURLUsed ??= [];
+        var urlUsed = Config.TrafficURLUsed.FirstOrDefault(u => u.Equals(url, StringComparison.CurrentCultureIgnoreCase));
+        if (urlUsed is not null)
+        {
+            Config.TrafficURLUsed.Remove(urlUsed);
+        }
+        else if (Config.PublishURLUsed.Count >= 10) Config.TrafficURLUsed.Remove(Config.TrafficURLUsed.Last());
+        Config.TrafficURLUsed.Insert(0, url);
+        await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config));
+    }
+
+    public async Task UpdateTrafficEnable(bool enable)
+    {
+        if (enable == Config.TrafficEnable) return;
+
+        Config.TrafficEnable = enable;
+        await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config));
+    }
+
+    public async Task UpdatePublishURL(string url)
+    {
+        if (url == Config.PublishURL) return;
+
+        Config.PublishURL = url;
+        Config.PublishURLUsed ??= [];
+        var urlUsed = Config.PublishURLUsed.FirstOrDefault(u => u.Equals(url, StringComparison.OrdinalIgnoreCase));
+        if (urlUsed is not null)
+        {
+            Config.PublishURLUsed.Remove(urlUsed);
+        }
+        else if (Config.PublishURLUsed.Count >= 10) Config.PublishURLUsed.Remove(Config.PublishURLUsed.Last());
+        Config.PublishURLUsed.Insert(0, url);
+        await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config));
+    }
+
+    public async Task UpdatePublishEnable(bool enable)
+    {
+        if (enable == Config.PublishEnable) return;
+
+        Config.PublishEnable = enable;
+        await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config));
+    }
+
+    public async Task UpdatePublishInterval(int interval)
+    {
+        if (interval == Config.PublishInterval) return;
+
+        Config.PublishInterval = interval;
+        PublishIntervalChanged?.Invoke();
+        await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config));
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        if (File.Exists(DataPath))
+        {
+            try
+            {
+                Config = JsonSerializer.Deserialize<ConfigData>(await File.ReadAllTextAsync(DataPath, CancellationToken.None));
+                PublishIntervalChanged?.Invoke();
+            }
+            catch (JsonException)
+            {
+                await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config), CancellationToken.None);
+            }
+        }
+        else await File.WriteAllTextAsync(DataPath, JsonSerializer.Serialize(Config), CancellationToken.None);
+    }
+}

RobotNet.RobotManager/Services/OpenACS/OpenACSPublisher.cs

@@ -0,0 +1,171 @@
+using RobotNet.RobotShares.VDA5050.State;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class OpenACSPublisher(IConfiguration configuration,
+    LoggerController<OpenACSPublisher> Logger,
+    RobotManager RobotManager,
+    OpenACSManager OpenACSManager) : BackgroundService
+{
+    public int PublishCount { get; private set; }
+    private WatchTimerAsync<OpenACSPublisher>? Timer;
+    private readonly string ACSSiteCode = configuration["ACSStatusConfig:SiteCode"] ?? "VN03";
+    private readonly string ACSAreaCode = configuration["ACSStatusConfig:AreaCode"] ?? "DA3_FL1";
+    private readonly string ACSAreaName = configuration["ACSStatusConfig:AreaName"] ?? "DA3_WM";
+    private readonly double ACSExtendX = configuration.GetValue<double>("ACSStatusConfig:ExtendX");
+    private readonly double ACSExtendY = configuration.GetValue<double>("ACSStatusConfig:ExtendY");
+    private readonly double ACSExtendTheta = configuration.GetValue<double>("ACSStatusConfig:ExtendTheta");
+
+    private async Task TimerHandler()
+    {
+        if (OpenACSManager.PublishEnable && !string.IsNullOrEmpty(OpenACSManager.PublishURL))
+        {
+            try
+            {
+                using var HttpClient = new HttpClient() { Timeout = TimeSpan.FromSeconds(15) };
+                var robotIds = RobotManager.RobotSerialNumbers.ToArray();
+                foreach (var robotId in robotIds)
+                {
+                    var startTime = DateTime.Now;
+                    var robot = RobotManager[robotId];
+                    if (robot == null || robot.StateMsg == null || robot.StateMsg.AgvPosition == null) continue;
+                    if (!DateTime.TryParse(robot.StateMsg.Timestamp, out DateTime lastTimeUpdate)) continue;
+                    if ((startTime - lastTimeUpdate).TotalMilliseconds > OpenACSManager.PublishInterval) continue;
+
+                    int batLevel = (int)robot.StateMsg.BatteryState.BatteryHealth;
+                    if (batLevel <= 0) batLevel = 85;
+
+                    int batVol = (int)robot.StateMsg.BatteryState.BatteryVoltage;
+                    if (batVol <= 0) batVol = 24;
+                    var status = new RobotPublishStatusV2()
+                    {
+                        Header = new("AGV_STATUS", lastTimeUpdate.ToString("yyyy-MM-dd HH:mm:ss.fff")),
+                        Body = new()
+                        {
+                            Id = robot.SerialNumber,
+                            Location = new()
+                            {
+                                X = (robot.StateMsg.AgvPosition.X + ACSExtendX).ToString(),
+                                Y = (robot.StateMsg.AgvPosition.Y + ACSExtendY).ToString(),
+                                Z = "0",
+                                Direction = (robot.StateMsg.AgvPosition.Theta + ACSExtendTheta).ToString(),
+                            },
+                            SiteCode = ACSSiteCode,
+                            AreaCode = ACSAreaCode,
+                            AreaName = ACSAreaName,
+                            MarkerId = string.IsNullOrEmpty(robot.StateMsg.LastNodeId) ? null : robot.StateMsg.LastNodeId,
+                            BatteryId = null,
+                            BatteryLevel = batLevel.ToString(),
+                            BatteryVoltage = batVol.ToString(),
+                            BatterySOH = null,
+                            BatteryCurrent = "1.0",
+                            BatteryTemprature = "30",
+                            StationId = null,
+                            Loading = robot.StateMsg.Loads.Length != 0 ? "1" : "0",
+                            ErrorCode = GetErrorCode(robot.StateMsg.Errors ?? [])?.ToString() ?? null,
+                            State = GetStatus(robot.StateMsg).ToString(),
+                        }
+                    };
+
+                    var response = await HttpClient.PostAsJsonAsync(OpenACSManager.PublishURL, status);
+                    if (response.IsSuccessStatusCode)
+                    {
+                        var result = await response.Content.ReadFromJsonAsync<ACSStatusResponse>();
+                        if (result == null)
+                        {
+                            Logger.Error("Failed to convert response.Content to ACSStatusResponse");
+                        }
+                        else if (result.Header.MessageName == "AGV_STATUS_ACK" && result.Body.Result == "OK")
+                        {
+                            PublishCount++;
+                        }
+                        else
+                        {
+                            Logger.Warning($"ACS response is not OK: {System.Text.Json.JsonSerializer.Serialize(result)}");
+                        }
+                    }
+                    else
+                    {
+                        Logger.Warning($"Quá trình xuất bản tới {OpenACSManager.PublishURL} không thành công: {response.StatusCode}");
+                    }
+                }
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Quá trình xuất bản tới {OpenACSManager.PublishURL} có lỗi xảy ra: {ex.Message}");
+            }
+        }
+    }
+
+    private static int GetStatus(StateMsg state)
+    {
+        if (GetError(state) == ErrorLevel.FATAL || GetError(state) == ErrorLevel.WARNING) return (int)AGVState.Error;
+        else if (state.BatteryState.Charging) return (int)AGVState.Charging;
+        else if (state.Paused) return (int)AGVState.Pause;
+        else if (IsIdle(state)) return (int)AGVState.Idle;
+        else if (IsWorking(state)) return (int)AGVState.Run;
+        else return (int)AGVState.Stop;
+    }
+
+    private static string? GetErrorCode(Error[] errors)
+    {
+        var error = errors.FirstOrDefault();
+        if (error is not null && int.TryParse(error.ErrorType, out int errorCode)) return errorCode.ToString();
+        return null;
+    }
+
+    private static bool IsIdle(StateMsg state)
+    {
+        if (state.NodeStates.Length != 0 || state.EdgeStates.Length != 0) return false;
+        return true;
+    }
+
+    private static bool IsWorking(StateMsg state)
+    {
+        if (state.NodeStates.Length != 0 || state.EdgeStates.Length != 0) return true;
+        return false;
+    }
+
+    private static ErrorLevel GetError(StateMsg state)
+    {
+        if (state.Errors is not null)
+        {
+            if (state.Errors.Any(error => error.ErrorLevel == ErrorLevel.FATAL.ToString())) return ErrorLevel.FATAL;
+            if (state.Errors.Any(error => error.ErrorLevel == ErrorLevel.WARNING.ToString())) return ErrorLevel.WARNING;
+        }
+        return ErrorLevel.NONE;
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            try
+            {
+                OpenACSManager.PublishIntervalChanged += UpdateInterval;
+                Timer = new(OpenACSManager.PublishInterval <= 500 ? 500 : OpenACSManager.PublishInterval, TimerHandler, Logger);
+                Timer.Start();
+                break;
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Publisher ACS: Quá trình khởi tạo có lỗi xảy ra: {ex.Message}");
+                await Task.Delay(2000, stoppingToken);
+            }
+        }
+    }
+
+    public void UpdateInterval()
+    {
+        Timer?.Dispose();
+        Timer = new(OpenACSManager.PublishInterval <= 500 ? 500 : OpenACSManager.PublishInterval, TimerHandler, Logger);
+        Timer.Start();
+    }
+
+    public override Task StopAsync(CancellationToken cancellationToken)
+    {
+        Timer?.Dispose();
+        return Task.CompletedTask;
+    }
+}

RobotNet.RobotManager/Services/OpenACS/RobotPublishStatus.cs

@@ -0,0 +1,15 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class RobotPublishStatusV2
+{
+    [JsonPropertyName("header")]
+    [Required]
+    public ACSHeader Header { get; set; } = new("AGV_STATUS", DateTime.Today.ToString("yyyy-MM-dd HH:mm:ss.fff"));
+
+    [JsonPropertyName("body")]
+    [Required]
+    public RobotPublishStatusBody Body { get; set; } = new();
+}

RobotNet.RobotManager/Services/OpenACS/RobotPublishStatusBody.cs

@@ -0,0 +1,71 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class RobotPublishStatusBody
+{
+    [JsonPropertyName("agv_id")]
+    [Required]
+    public string Id { get; set; } = "";
+
+    [JsonPropertyName("state")]
+    [Required]
+    public string State { get; set; } = "-1";
+
+    [JsonPropertyName("site_code")]
+    [Required]
+    public string SiteCode { get; set; } = "";
+
+    [JsonPropertyName("area_code")]
+    [Required]
+    public string AreaCode { get; set; } = "";
+
+    [JsonPropertyName("area_name")]
+    [Required]
+    public string AreaName { get; set; } = "";
+
+    [JsonPropertyName("location")]
+    [Required]
+    public AGVLocation Location { get; set; } = new();
+
+    [JsonPropertyName("marker_id")]
+    [Required]
+    public string? MarkerId { get; set; }
+
+    [JsonPropertyName("battery_level")]
+    [Required]
+    public string BatteryLevel { get; set; } = "0";
+
+    [JsonPropertyName("battery_voltage")]
+    [Required]
+    public string BatteryVoltage { get; set; } = "0";
+
+    [JsonPropertyName("battery_current")]
+    [Required]
+    public string BatteryCurrent { get; set; } = "0";
+
+    [JsonPropertyName("battery_temperature")]
+    [Required]
+    public string BatteryTemprature { get; set; } = "0";
+
+    [JsonPropertyName("battery_id")]
+    [Required]
+    public string? BatteryId { get; set; }
+
+    [JsonPropertyName("battery_soh")]
+    [Required]
+    public string? BatterySOH { get; set; } = "0";
+
+    [JsonPropertyName("loading")]
+    [Required]
+    public string Loading { get; set; } = "0";
+
+    [JsonPropertyName("error_code")]
+    [Required]
+    public string? ErrorCode { get; set; }
+
+    [JsonPropertyName("station_id")]
+    [Required]
+    public string? StationId { get; set; }
+}

RobotNet.RobotManager/Services/OpenACS/TrafficACS.cs

@@ -0,0 +1,108 @@
+using NLog.Targets;
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotShares.OpenACS;
+using RobotNet.Shares;
+using System.Text.Json;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class TrafficACS(OpenACSManager OpenACSManager, IConfiguration Configuration, LoggerController<TrafficACS> Logger)
+{
+    public bool Enable => OpenACSManager.TrafficEnable;
+    private readonly double TrafficCheckingDistanceMin = Configuration.GetValue("TrafficConfig:CheckingDistanceMin", 3);
+    public readonly double DeviationDistance = Configuration.GetValue("TrafficConfig:DeviationDistance", 0.5);
+    private static readonly JsonSerializerOptions jsonSerializeOptions = new() { 
+        WriteIndented = true,
+        Encoder = System.Text.Encodings.Web.JavaScriptEncoder.UnsafeRelaxedJsonEscaping
+    };
+
+    public async Task<MessageResult<bool>> RequestIn(string robotId, string zoneId)
+    {
+        try
+        {
+            if (!OpenACSManager.TrafficEnable) return new(true, "Kết nối với hệ thống traffic ACS không được bật") { Data = true };
+            using var HttpClient = new HttpClient() { Timeout = TimeSpan.FromSeconds(15) };
+
+            var model = new TrafficACSRequestV2(robotId, zoneId, TrafficRequestType.IN.ToInOutString());
+
+            var response = await (await HttpClient.PostAsJsonAsync(OpenACSManager.TrafficURL, model)).Content.ReadFromJsonAsync<TrafficACSResponseV2>() ??
+                throw new OpenACSException("Lỗi giao tiếp với hệ thống traffic ACS");
+            if (response.Body.AgvId != robotId) throw new OpenACSException($"Dữ liệu hệ thống traffic ACS agv_id trả về {response.Body.AgvId} không trùng với dữ liệu gửi đi {robotId}");
+            if (response.Body.TrafficZoneId != zoneId) throw new OpenACSException($"Dữ liệu hệ thống traffic ACS traffic_zone_id trả về {response.Body.TrafficZoneId} không trùng với dữ liệu gửi đi {zoneId}");
+            if (response.Body.InOut != TrafficRequestType.IN.ToInOutString()) throw new OpenACSException($"Dữ liệu hệ thống traffic ACS inout trả về {response.Body.InOut} không trùng với dữ liệu gửi đi in");
+            Logger.Info($"{robotId} yêu cầu vào traffic zone {zoneId} \nRequest: {JsonSerializer.Serialize(model, jsonSerializeOptions)}\n trả về kết quả: {JsonSerializer.Serialize(response, jsonSerializeOptions)}");
+            return new(true, "Yêu cầu vào traffic zone thành công") { Data = response.Body.Result == TrafficACSResult.GO };
+        }
+        catch (OpenACSException ex)
+        {
+            Logger.Warning($"{robotId} request in xảy ra lỗi: {ex.Message}");
+            return new(false, ex.Message);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{robotId} request In xảy ra lỗi: {ex.Message}");
+            return new(false, "Lỗi giao tiếp với hệ thống traffic ACS");
+        }
+    }
+
+    public async Task<MessageResult<bool>> RequestOut(string robotId, string zoneId)
+    {
+        try
+        {
+            if (!OpenACSManager.TrafficEnable) return new(true, "Kết nối với hệ thống traffic ACS không được bật") { Data = true};
+            using var HttpClient = new HttpClient() { Timeout = TimeSpan.FromSeconds(15) };
+            var model = new TrafficACSRequestV2(robotId, zoneId, TrafficRequestType.OUT.ToInOutString());
+            var response = await (await HttpClient.PostAsJsonAsync(OpenACSManager.TrafficURL, model)).Content.ReadFromJsonAsync<TrafficACSResponseV2>() ??
+                throw new OpenACSException("Lỗi giao tiếp với hệ thống traffic ACS");
+            if (response.Body.AgvId != robotId) throw new OpenACSException($"Dữ liệu hệ thống traffic ACS agv_id trả về {response.Body.AgvId} không trùng với dữ liệu gửi đi {robotId}");
+            if (response.Body.TrafficZoneId != zoneId) throw new OpenACSException($"Dữ liệu hệ thống traffic ACS traffic_zone_id trả về {response.Body.TrafficZoneId} không trùng với dữ liệu gửi đi {zoneId}");
+            if (response.Body.InOut != TrafficRequestType.OUT.ToInOutString()) throw new OpenACSException($"Dữ liệu hệ thống traffic ACS inout trả về {response.Body.InOut} không trùng với dữ liệu gửi đi out");
+            Logger.Info($"{robotId} yêu cầu ra khỏi traffic zone {zoneId}  \nRequest: {JsonSerializer.Serialize(model, jsonSerializeOptions)}\n trả về kết quả: {JsonSerializer.Serialize(response, jsonSerializeOptions)}");
+            return new(true, "Yêu cầu ra khỏi traffic zone thành công") { Data = response.Body.Result == TrafficACSResult.GO };
+        }
+        catch (OpenACSException ex)
+        {
+            Logger.Warning($"{robotId} request out xảy ra lỗi: {ex.Message}");
+            return new(false, ex.Message);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{robotId} request Out xảy ra lỗi: {ex.Message}");
+            return new(false, "Lỗi giao tiếp với hệ thống traffic ACS");
+        }
+    }
+
+    public Dictionary<Guid, ZoneDto[]> GetZones(Guid inNodeId, NodeDto[] nodes, Dictionary<Guid, ZoneDto[]> zones)
+    {
+        int inNodeIndex = Array.FindIndex(nodes, n => n.Id == inNodeId);
+        if (inNodeId == Guid.Empty || (inNodeIndex != -1 && inNodeIndex < nodes.Length - 1))
+        {
+            List<NodeDto> baseNodes = [];
+            List<ZoneDto> basezones = [];
+            double distance = 0;
+            int index = inNodeIndex != -1 ? inNodeIndex + 1 : 1;
+            for (; index < nodes.Length; index++)
+            {
+                baseNodes.Add(nodes[index]);
+                distance += Math.Sqrt(Math.Pow(nodes[index].X - nodes[index - 1].X, 2) + Math.Pow(nodes[index].Y - nodes[index - 1].Y, 2));
+                if (distance > TrafficCheckingDistanceMin) break;
+            }
+
+            Dictionary<Guid, ZoneDto[]> nodeZones = [];
+
+            foreach (var node in baseNodes)
+            {
+                if (zones.TryGetValue(node.Id, out ZoneDto[]? zone) && zone is not null && zone.Length > 0) nodeZones.Add(node.Id, zone);
+                else nodeZones.Add(node.Id, []);
+            }
+
+            return nodeZones;
+        }
+        else if (inNodeIndex == nodes.Length - 1) return [];
+        else
+        {
+            Logger.Warning($"Không tìm thấy node {inNodeId} trong danh sách nodes hoặc node này.");
+            return [];
+        }
+    }
+}

RobotNet.RobotManager/Services/OpenACS/TrafficACSRequest.cs

@@ -0,0 +1,42 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+
+public class TrafficACSRequestBody(string agvId, string trafficZoneId, string inOut)
+{
+    [JsonPropertyName("agv_id")]
+    [Required]
+    public string AgvId { get; set; } = agvId;
+
+    [JsonPropertyName("traffic_zone_id")]
+    [Required]
+    public string TrafficZoneId { get; set; } = trafficZoneId;
+
+    [JsonPropertyName("inout")]
+    [Required]
+    public string InOut { get; set; } = inOut;
+}
+
+public class TrafficACSRequestV2
+{
+    [JsonPropertyName("header")]
+    [Required]
+    public ACSHeader Header { get; set; } = new("TRAFFIC_REQ", DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff"));
+
+    [JsonPropertyName("body")]
+    [Required]
+    public TrafficACSRequestBody Body { get; set; }
+
+    public TrafficACSRequestV2(string agvId, string trafficZoneId, string inOut)
+    {
+        if (string.IsNullOrWhiteSpace(agvId))
+            throw new ArgumentException("AGV ID không thể rỗng.", nameof(agvId));
+        if (string.IsNullOrWhiteSpace(trafficZoneId))
+            throw new ArgumentException("Traffic Zone ID không thể rỗng.", nameof(trafficZoneId));
+        if (string.IsNullOrWhiteSpace(inOut))
+            throw new ArgumentException("In OUT không thể rỗng.", nameof(inOut));
+
+        Body = new (agvId, trafficZoneId, inOut);
+    }
+}

RobotNet.RobotManager/Services/OpenACS/TrafficACSResponse.cs

@@ -0,0 +1,62 @@
+using System.ComponentModel.DataAnnotations;
+using System.Text.Json.Serialization;
+
+namespace RobotNet.RobotManager.Services.OpenACS;
+public class TrafficACSResult
+{
+    public static string GO => "go";
+    public static string NO => "no";
+}
+public class TrafficACSResponse
+{
+    [JsonPropertyName("time")]
+    public string Time { get; set; } = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff");
+
+    [JsonPropertyName("agv_id")]
+    public string AgvId { get; set; } = string.Empty;
+
+    [JsonPropertyName("traffic_zone_id")]
+    [Required]
+    public string TrafficZoneId { get; set; } = string.Empty;
+
+    [JsonPropertyName("inout")]
+    [Required]
+    public string InOut { get; set; } = string.Empty;
+
+    [JsonPropertyName("result")]
+    [Required]
+    public string Result { get; set; } = string.Empty;
+}
+
+
+public class TrafficACSResponseBody
+{
+
+    [JsonPropertyName("agv_id")]
+    [Required]
+    public string AgvId { get; set; } = string.Empty;
+
+    [JsonPropertyName("traffic_zone_id")]
+    [Required]
+    public string TrafficZoneId { get; set; } = string.Empty;
+
+    [JsonPropertyName("inout")]
+    [Required]
+    public string InOut { get; set; } = string.Empty;
+
+    [JsonPropertyName("result")]
+    [Required]
+    public string Result { get; set; } = string.Empty;
+}
+
+public class TrafficACSResponseV2
+{
+
+    [JsonPropertyName("header")]
+    [Required]
+    public ACSHeader Header { get; set; } = new("TRAFFIC_RES", DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff"));
+
+    [JsonPropertyName("body")]
+    [Required]
+    public TrafficACSResponseBody Body { get; set; } = new();
+}

RobotNet.RobotManager/Services/PathPlanner.cs

@@ -0,0 +1,351 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+using System.Collections.Generic;
+
+namespace RobotNet.RobotManager.Services;
+
+public enum PathPlanningType
+{
+    None,
+    Angle,
+    StartDirection,
+    EndDirection,
+}
+
+public class PathPlanner(IConfiguration Configuration, MapManager MapManager)
+{
+    private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1);
+    private readonly PathPlanningType PathPlanningType = Configuration.GetValue("PathPlanning:Type", PathPlanningType.None);
+    private readonly RobotDirection StartDirection = Configuration.GetValue("PathPlanning:StartDirection", RobotDirection.NONE);
+    private readonly RobotDirection EndDirection = Configuration.GetValue("PathPlanning:EndDirection", RobotDirection.NONE);
+
+    public async Task<MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)>> Planning(double xRef, double yRef, double thetaRef, NavigationType type, Guid mapId, string nodeName)
+    {
+        var map = await MapManager.GetMapData(mapId);
+        if (!map.IsSuccess) return new(false, map.Message);
+        if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");
+
+        var goalNode = map.Data.Nodes.FirstOrDefault(n => n.Name == nodeName);
+        if (goalNode is null) return new(false, "Đích đến không tồn tại");
+
+        PathPlannerManager PathPlannerManager = new();
+        var PathPlanner = PathPlannerManager.GetPathPlanningService(type);
+        PathPlanner.SetData(map.Data.Nodes, map.Data.Edges);
+
+        if (type == NavigationType.Forklift) return PathPlanner.PathPlanningWithAngle(xRef, yRef, thetaRef, goalNode.Id);
+        else
+        {
+            return PathPlanningType switch
+            {
+                PathPlanningType.None => PathPlanner.PathPlanning(xRef, yRef, thetaRef, goalNode.Id),
+                PathPlanningType.Angle => PathPlanner.PathPlanningWithAngle(xRef, yRef, thetaRef, goalNode.Id),
+                PathPlanningType.StartDirection => PathPlanner.PathPlanningWithStartDirection(xRef, yRef, thetaRef, goalNode.Id, StartDirection),
+                PathPlanningType.EndDirection => PathPlanner.PathPlanningWithFinalDirection(xRef, yRef, thetaRef, goalNode.Id, EndDirection),
+                _ => PathPlanner.PathPlanning(xRef, yRef, thetaRef, goalNode.Id),
+            };
+        }
+    }
+
+    public async Task<MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)>> Planning(Guid startNodeId, double thetaRef, NavigationType type, Guid mapId, Guid goalId)
+    {
+        var map = await MapManager.GetMapData(mapId);
+        if (!map.IsSuccess) return new(false, map.Message);
+        if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");
+
+        PathPlannerManager PathPlannerManager = new();
+        var PathPlanner = PathPlannerManager.GetPathPlanningService(type);
+        PathPlanner.SetData(map.Data.Nodes, map.Data.Edges);
+
+        if (type == NavigationType.Forklift) return PathPlanner.PathPlanningWithAngle(startNodeId, thetaRef, goalId);
+        else
+        {
+            return PathPlanningType switch
+            {
+                PathPlanningType.None => PathPlanner.PathPlanning(startNodeId, thetaRef, goalId),
+                PathPlanningType.Angle => PathPlanner.PathPlanningWithAngle(startNodeId, thetaRef, goalId),
+                PathPlanningType.StartDirection => PathPlanner.PathPlanningWithStartDirection(startNodeId, thetaRef, goalId, StartDirection),
+                PathPlanningType.EndDirection => PathPlanner.PathPlanningWithFinalDirection(startNodeId, thetaRef, goalId, EndDirection),
+                _ => PathPlanner.PathPlanning(startNodeId, thetaRef, goalId),
+            };
+        }
+    }
+
+    public static MessageResult<NodeDto[]> PathSplit(NodeDto[] nodes, EdgeDto[] edges, double resolutionSplit)
+    {
+        if (nodes.Length < 1 || edges.Length == 0) return new(false, "Dữ liệu không hợp lệ");
+        List<NodeDto> pathSplit = [];
+        pathSplit.Add(new()
+        {
+            Id = nodes[0].Id,
+            X = nodes[0].X,
+            Y = nodes[0].Y,
+            Theta = nodes[0].Theta,
+            Direction = nodes[0].Direction,
+            Name = nodes[0].Name,
+            Actions = "CheckingNode"
+        });
+        foreach (var edge in edges)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+            var endNode = nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) return new(false, "Dữ liệu lỗi: Điểm đầu cuối của edge không có trong danh sách nodes");
+            NodeDto controlNode = new();
+
+            var EdgeCaculatorModel = new EdgeCaculatorModel()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y = edge.ControlPoint2Y,
+                TrajectoryDegree = edge.TrajectoryDegree,
+            };
+            var length = MapEditorHelper.GetEdgeLength(EdgeCaculatorModel);
+
+            if (length <= 0) continue;
+            double step = resolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = MapEditorHelper.Curve(t, EdgeCaculatorModel);
+                pathSplit.Add(new()
+                {
+                    Id = Guid.NewGuid(),
+                    X = x,
+                    Y = y,
+                    Theta = startNode.Theta,
+                    Direction = startNode.Direction,
+                });
+            }
+
+            pathSplit.Add(new()
+            {
+                Id = endNode.Id,
+                X = endNode.X,
+                Y = endNode.Y,
+                Theta = endNode.Theta,
+                Direction = endNode.Direction,
+                Name = endNode.Name,
+                Actions = "CheckingNode"
+            });
+        }
+        return new(true) { Data = [.. pathSplit] };
+    }
+
+    public MessageResult<NodeDto[]> PathSplit(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 1 || edges.Length == 0) return new(false, "Dữ liệu không hợp lệ");
+        List<NodeDto> pathSplit = [];
+        pathSplit.Add(new()
+        {
+            Id = nodes[0].Id,
+            X = nodes[0].X,
+            Y = nodes[0].Y,
+            Theta = nodes[0].Theta,
+            Direction = nodes[0].Direction,
+            Name = nodes[0].Name,
+            Actions = "CheckingNode"
+        });
+        foreach (var edge in edges)
+        {
+            var startNode = nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+            var endNode = nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) return new(false, "Dữ liệu lỗi: Điểm đầu cuối của edge không có trong danh sách nodes");
+            NodeDto controlNode = new();
+
+            var EdgeCaculatorModel = new EdgeCaculatorModel()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y = edge.ControlPoint2Y,
+                TrajectoryDegree = edge.TrajectoryDegree,
+            };
+            var length = MapEditorHelper.GetEdgeLength(EdgeCaculatorModel);
+
+            if (length <= 0) continue;
+            double step = ResolutionSplit / length;
+
+            for (double t = step; t <= 1 - step; t += step)
+            {
+                (double x, double y) = MapEditorHelper.Curve(t, EdgeCaculatorModel);
+                pathSplit.Add(new()
+                {
+                    Id = Guid.NewGuid(),
+                    X = x,
+                    Y = y,
+                    Theta = startNode.Theta,
+                    Direction = startNode.Direction,
+                });
+            }
+
+            pathSplit.Add(new()
+            {
+                Id = endNode.Id,
+                X = endNode.X,
+                Y = endNode.Y,
+                Theta = endNode.Theta,
+                Direction = endNode.Direction,
+                Name = endNode.Name,
+                Actions = "CheckingNode"
+            });
+        }
+        return new(true) { Data = [.. pathSplit] };
+    }
+
+    public static RobotDirection GetRobotStartDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, 0.1);
+
+        var RobotNearNode = new NodeDto()
+        {
+            X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
+            Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
+        };
+        return MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode) > 89 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+    }
+
+    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, NodeDto[] nodes, EdgeDto[] edges)
+    {
+        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodes.Length];
+        if (nodes.Length > 0) RobotDirectionInNode[0] = currentDirection;
+        if (nodes.Length > 2)
+        {
+            for (int i = 1; i < nodes.Length - 1; i++)
+            {
+                NodeDto nearLastNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i - 1], edges[i - 1], 0.1);
+                NodeDto nearFutureNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i + 1], edges[i], 0.1); ;
+                var angle = MapEditorHelper.GetAngle(nodes[i], nearLastNode, nearFutureNode);
+                if (angle < 89) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
+            }
+        }
+        if (nodes.Length > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
+        return RobotDirectionInNode;
+    }
+
+    public static double[] GetRobotThetaInPath(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 2 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return [];
+        double[] RobotThetaInNode = new double[nodes.Length];
+        if (nodes.Length > 1)
+        {
+            for (int i = 0; i < nodes.Length - 1; i++)
+            {
+                NodeDto nearFutureNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i + 1], edges[i], 0.1);
+                var angleForward = Math.Atan2(nearFutureNode.Y - nodes[i].Y, nearFutureNode.X - nodes[i].X) * 180 / Math.PI;
+                var angleBackward = Math.Atan2(nodes[i].Y - nearFutureNode.Y, nodes[i].X - nearFutureNode.X) * 180 / Math.PI;
+
+                RobotThetaInNode[i] = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
+            }
+            RobotThetaInNode[^1] = RobotThetaInNode[^2];
+        }
+        return RobotThetaInNode;
+    }
+
+    public static NodeDto[] CalculatorDirection(NodeDto[] nodes, EdgeDto[] edges) => CalculatorDirection(RobotDirection.FORWARD, nodes, edges);
+
+    public static NodeDto[] CalculatorDirection(RobotDirection startDirection, NodeDto[] nodes, EdgeDto[] edges)
+    {
+        var directions = GetRobotDirectionInPath(startDirection, nodes, edges);
+        NodeDto[] returnNodes = [.. nodes];
+        for (int i = 0; i < returnNodes.Length; i++)
+        {
+            returnNodes[i].Direction = MapCompute.GetNodeDirection(directions[i]);
+        }
+        var thetas = GetRobotThetaInPath(returnNodes, edges);
+        for (int i = 0; i < returnNodes.Length; i++)
+        {
+            returnNodes[i].Theta = thetas[i];
+        }
+        return returnNodes;
+    }
+
+    public static NodeDto[] CalculatorDirection(double theta, NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 2) return nodes;
+        var RobotNearNode = new NodeDto()
+        {
+            X = nodes[0].X + Math.Cos(theta * Math.PI / 180),
+            Y = nodes[0].Y + Math.Sin(theta * Math.PI / 180),
+        };
+        nodes[0].Direction = MapEditorHelper.GetAngle(nodes[0], RobotNearNode, nodes[1]) > 89 ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD;
+        var directions = GetRobotDirectionInPath(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
+        NodeDto[] returnNodes = [.. nodes];
+        for (int i = 0; i < returnNodes.Length; i++)
+        {
+            returnNodes[i].Direction = MapCompute.GetNodeDirection(directions[i]);
+        }
+        var thetas = GetRobotThetaInPath(returnNodes, edges);
+        for (int i = 0; i < returnNodes.Length; i++)
+        {
+            returnNodes[i].Theta = thetas[i];
+        }
+        return returnNodes;
+    }
+
+    public static NodeDto[] CalculatorDirection(double theta, NodeDto[] nodes)
+    {
+        if (nodes.Length < 2) return nodes;
+        var RobotNearNode = new NodeDto()
+        {
+            X = nodes[0].X + Math.Cos(theta * Math.PI / 180),
+            Y = nodes[0].Y + Math.Sin(theta * Math.PI / 180),
+        };
+        nodes[0].Direction = MapEditorHelper.GetAngle(nodes[0], RobotNearNode, nodes[1]) > 89 ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD;
+        for (int i = 1; i < nodes.Length - 1; i++)
+        {
+            if (nodes[i].X == nodes[i - 1].X && nodes[i].Y == nodes[i - 1].Y) nodes[i].Direction = nodes[i - 1].Direction;
+            else if (nodes[i].X == nodes[i + 1].X && nodes[i].Y == nodes[i + 1].Y) nodes[i].Direction = nodes[i - 1].Direction;
+            else
+            {
+                var angle = MapEditorHelper.GetAngle(nodes[i], nodes[i - 1], nodes[i + 1]);
+                nodes[i].Direction = angle > 89 ? nodes[i - 1].Direction : (nodes[i - 1].Direction == MapShares.Enums.Direction.FORWARD ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD);
+            }
+        }
+        nodes[^1].Direction = nodes[^2].Direction;
+        return nodes;
+    }
+
+    public async Task<MessageResult<Dictionary<Guid, ZoneDto[]>>> GetZones(Guid mapId, NodeDto[] nodes)
+    {
+        var map = await MapManager.GetMapData(mapId);
+        if (!map.IsSuccess) return new(false, map.Message);
+        if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");
+
+        Dictionary<Guid, ZoneDto[]> NodeInZones = [];
+        foreach (var node in nodes)
+        {
+            List<ZoneDto> zones = [];
+            foreach(var zone in map.Data.Zones)
+            {
+                if (MapEditorHelper.IsPointInside(node.X, node.Y, zone)) zones.Add(zone);
+            }
+            if (zones.Count > 0) NodeInZones.Add(node.Id, [..zones]);
+        }
+        return new(true, "") { Data = NodeInZones };
+    }
+
+    public static ZoneDto[] GetZones(NodeDto[] nodes, Dictionary<Guid, ZoneDto[]> zones)
+    {
+        List<ZoneDto> returnZones = [];
+        foreach (var node in nodes)
+        {
+            if (zones.TryGetValue(node.Id, out ZoneDto[]? zone) && zone is not null && zone.Length > 0)
+                returnZones.AddRange(zone.Where(z => !returnZones.Any(zo => zo.Id == z.Id)));
+        }
+        return [.. returnZones];
+    }
+}

RobotNet.RobotManager/Services/Planner/AStar/AStarNode.cs

@@ -0,0 +1,28 @@
+namespace RobotNet.RobotManager.Services.Planner.AStar;
+
+#nullable disable
+
+public class AStarNode
+{
+    public Guid Id { get; set; }
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double Cost { get; set; }
+    public double Heuristic { get; set; }
+    public double TotalCost => Cost + Heuristic;
+    public string Name { get; set; }
+    public AStarNode Parent { get; set; }
+    public List<AStarNode> NegativeNodes { get; set; } = [];
+
+    public override bool Equals(object obj)
+    {
+        if (obj is AStarNode other)
+            return Id == other.Id;
+        return false;
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Id);
+    }
+}

RobotNet.RobotManager/Services/Planner/AStar/AStarPathPlanner.cs

@@ -0,0 +1,393 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.AStar;
+
+public class AStarPathPlanner(List<NodeDto> Nodes, List<EdgeDto> Edges)
+{
+    public static NodeDto? GetCurveNode(double t, EdgeDto edge, NodeDto startNode, NodeDto endNode)
+    {
+        if (edge.TrajectoryDegree == TrajectoryDegree.Two)
+        {
+            return new()
+            {
+                X = (1 - t) * (1 - t) * startNode.X + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * endNode.X,
+                Y = (1 - t) * (1 - t) * startNode.Y + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * endNode.Y
+            };
+        }
+        else if (edge.TrajectoryDegree == TrajectoryDegree.Three)
+        {
+            return new()
+            {
+                X = Math.Pow(1 - t, 3) * startNode.X + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * endNode.X,
+                Y = Math.Pow(1 - t, 3) * startNode.Y + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * endNode.Y,
+            };
+        }
+        return null;
+    }
+
+    public static double DistanceToCurveEdge(NodeDto nodeRef, EdgeDto edge, NodeDto startNode, NodeDto endNode)
+    {
+        double dMin = Math.Sqrt(Math.Pow(nodeRef.X - startNode.X, 2) + Math.Pow(nodeRef.Y - startNode.Y, 2));
+        var length = MapEditorHelper.GetEdgeLength(new()
+        {
+            X1 = startNode.X,
+            Y1 = startNode.Y,
+            X2 = endNode.X,
+            Y2 = endNode.Y,
+            ControlPoint1X = edge.ControlPoint1X,
+            ControlPoint1Y = edge.ControlPoint1Y,
+            ControlPoint2X = edge.ControlPoint2X,
+            ControlPoint2Y = edge.ControlPoint2Y,
+            TrajectoryDegree = edge.TrajectoryDegree,
+        });
+        double step = 0.1 / (length == 0 ? 0.1 : length);
+
+        for (double t = 0; t <= 1; t += step)
+        {
+            var nodeCurve = GetCurveNode(t, edge, startNode, endNode);
+            if (nodeCurve is null) continue;
+            double d = Math.Sqrt(Math.Pow(nodeRef.X - nodeCurve.X, 2) + Math.Pow(nodeRef.Y - nodeCurve.Y, 2));
+            if (d < dMin) dMin = d;
+        }
+
+        return dMin;
+    }
+
+    public static MessageResult<double> DistanceToEdge(NodeDto nodeRef, EdgeDto edge, NodeDto startNode, NodeDto endNode)
+    {
+
+        if (edge.TrajectoryDegree == TrajectoryDegree.One)
+        {
+            double time = 0;
+            var edgeLengthSquared = Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2);
+            if (edgeLengthSquared > 0)
+            {
+                time = Math.Max(0, Math.Min(1, ((nodeRef.X - startNode.X) * (endNode.X - startNode.X) + (nodeRef.Y - startNode.Y) * (endNode.Y - startNode.Y)) / edgeLengthSquared));
+            }
+
+            double nearestX = startNode.X + time * (endNode.X - startNode.X);
+            double nearestY = startNode.Y + time * (endNode.Y - startNode.Y);
+
+            return new(true) { Data = Math.Sqrt(Math.Pow(nodeRef.X - nearestX, 2) + Math.Pow(nodeRef.Y - nearestY, 2)) };
+        }
+        else
+        {
+            return new(true) { Data = DistanceToCurveEdge(nodeRef, edge, startNode, endNode) };
+        }
+    }
+
+    public EdgeDto? GetClosesEdge(NodeDto nodeRef, double limitDistance, CancellationToken? cancellationToken = null)
+    {
+        double minDistance = double.MaxValue;
+        EdgeDto? edgeResult = null;
+        foreach (var edge in Edges)
+        {
+            if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return null;
+            var startNode = Nodes.FirstOrDefault(node => node.Id == edge.StartNodeId);
+            var endNode = Nodes.FirstOrDefault(node => node.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) continue;
+
+            var getDistance = DistanceToEdge(nodeRef, edge, startNode, endNode);
+            if (getDistance.IsSuccess)
+            {
+                if (getDistance.Data < minDistance)
+                {
+                    minDistance = getDistance.Data;
+                    edgeResult = edge;
+                }
+            }
+        }
+        if (minDistance <= limitDistance) return edgeResult;
+        else return null;
+    }
+
+    private NodeDto? GetOnNode(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
+    {
+        if (cancellationToken?.IsCancellationRequested == true) return null;
+        KDTree KDTree = new(Nodes);
+        return KDTree.FindNearest(x, y, limitDistance);
+    }
+
+    private List<NodeDto> GetNegativeNode(Guid nodeId, CancellationToken? cancellationToken = null)
+    {
+        var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
+        if (node is null) return [];
+
+        var ListNodesNegative = new List<NodeDto>();
+        var ListPaths = Edges.Where(p => p.EndNodeId == nodeId || p.StartNodeId == nodeId);
+        foreach (var path in ListPaths)
+        {
+            if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return [];
+            if (path.StartNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
+                if (nodeAdd is not null) ListNodesNegative.Add(nodeAdd);
+                continue;
+            }
+            if (path.EndNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.StartNodeId);
+                if (nodeAdd is not null) ListNodesNegative.Add(nodeAdd);
+                continue;
+            }
+        }
+        return ListNodesNegative;
+    }
+
+    private double GetNegativeCost(AStarNode currenNode, AStarNode negativeNode)
+    {
+        var negativeEdges = Edges.Where(e => e.StartNodeId == currenNode.Id && e.EndNodeId == negativeNode.Id || e.StartNodeId == negativeNode.Id && e.EndNodeId == currenNode.Id).ToList();
+        double minDistance = double.MaxValue;
+        foreach (var edge in negativeEdges)
+        {
+            var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+            var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) return 0;
+            var distance = MapEditorHelper.GetEdgeLength(new()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y = edge.ControlPoint2Y,
+                TrajectoryDegree = edge.TrajectoryDegree,
+            });
+            if (distance < minDistance) minDistance = distance;
+        }
+        return minDistance != double.MaxValue ? minDistance : 0;
+    }
+
+    private List<AStarNode> GetNegativeAStarNode(AStarNode nodeCurrent, NodeDto endNode, CancellationToken? cancellationToken = null)
+    {
+        var possiblePointNegative = new List<AStarNode>();
+        foreach (var nodeNegative in nodeCurrent.NegativeNodes)
+        {
+            nodeNegative.Parent = nodeCurrent;
+            var ListNodesNegative = GetNegativeNode(nodeNegative.Id, cancellationToken);
+            foreach (var item in ListNodesNegative)
+            {
+                if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return [];
+                nodeNegative.NegativeNodes.Add(new AStarNode()
+                {
+                    Id = item.Id,
+                    X = item.X,
+                    Y = item.Y,
+                    Name = item.Name,
+                });
+            }
+            var cost = GetNegativeCost(nodeCurrent, nodeNegative);
+            nodeNegative.Cost = (cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2))) + nodeCurrent.Cost;
+            nodeNegative.Heuristic = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
+            possiblePointNegative.Add(nodeNegative);
+        }
+        return possiblePointNegative;
+    }
+
+    private List<AStarNode> Find(AStarNode startNode, NodeDto endNode, CancellationToken? cancellationToken = null)
+    {
+        try
+        {
+            var activeNodes = new PriorityQueue<AStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
+            var visitedNodes = new HashSet<AStarNode>();
+            List<AStarNode> Path = [];
+            activeNodes.Enqueue(startNode);
+
+            while (activeNodes.Count != 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
+            {
+                var checkNode = activeNodes.Dequeue();
+                if (checkNode.Id == endNode.Id)
+                {
+                    var node = checkNode;
+                    while (node != null)
+                    {
+                        if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+                        Path.Add(node);
+                        node = node.Parent;
+                    }
+                    return Path;
+                }
+
+                visitedNodes.Add(checkNode);
+
+                var ListNodeNegative = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
+                foreach (var node in ListNodeNegative)
+                {
+                    if (visitedNodes.TryGetValue(node, out AStarNode? value) && value is not null)
+                    {
+                        if (value.TotalCost > node.TotalCost)
+                        {
+                            visitedNodes.Remove(value);
+                            activeNodes.Enqueue(node);
+                        }
+                        continue;
+                    }
+
+                    var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id);
+                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
+                    {
+                        activeNodes.Items.Remove(activeNode);
+                        activeNodes.Enqueue(node);
+                    }
+                    else
+                    {
+                        activeNodes.Enqueue(node);
+                    }
+                }
+            }
+
+            return [];
+        }
+        catch
+        {
+            return [];
+        }
+    }
+
+    public MessageResult<List<NodeDto>> Planning(double x, double y, NodeDto goal, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+
+        AStarNode RobotNode = new()
+        {
+            Id = Guid.NewGuid(),
+            X = x,
+            Y = y,
+            Name = "RobotCurrentNode",
+        };
+        var closesNode = GetOnNode(x, y, maxDistanceToNode);
+        if (closesNode is not null)
+        {
+            if (closesNode.Id == goal.Id) return new(true, "Robot đang đứng tại điểm đích") { Data = [goal] };
+            RobotNode.Name = closesNode.Name;
+            RobotNode.Id = closesNode.Id;
+            RobotNode.X = closesNode.X;
+            RobotNode.Y = closesNode.Y;
+
+            foreach (var negativeNode in GetNegativeNode(RobotNode.Id, cancellationToken))
+            {
+                var cost = GetNegativeCost(RobotNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
+                RobotNode.NegativeNodes.Add(new()
+                {
+                    Id = negativeNode.Id,
+                    X = negativeNode.X,
+                    Y = negativeNode.Y,
+                    Name = negativeNode.Name,
+                    Cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(RobotNode.X - negativeNode.X, 2) + Math.Pow(RobotNode.Y - negativeNode.Y, 2)),
+                    Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
+                });
+            }
+        }
+        else
+        {
+            var closesEdge = GetClosesEdge(new() { X = x, Y = y }, maxDistanceToEdge, cancellationToken);
+            if (closesEdge is null)
+            {
+                return new(false, "Robot đang quá xa tuyến đường");
+            }
+
+            var startNodeForward = Nodes.FirstOrDefault(p => p.Id == closesEdge.StartNodeId);
+            var startNodeBackward = Nodes.FirstOrDefault(p => p.Id == closesEdge.EndNodeId);
+            if (startNodeForward is null || startNodeBackward is null)
+            {
+                return new(false, "Dữ liệu lỗi: điểm chặn của edge gần nhất không tồn tại");
+            }
+            if (startNodeForward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                Path.Add(startNodeBackward);
+                Path.Add(startNodeForward);
+                return new(true) { Data = Path };
+            }
+            if (startNodeBackward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                Path.Add(startNodeForward);
+                Path.Add(startNodeBackward);
+                return new(true) { Data = Path };
+            }
+            if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward)
+            {
+                RobotNode.NegativeNodes.Add(new()
+                {
+                    Id = startNodeForward.Id,
+                    X = startNodeForward.X,
+                    Y = startNodeForward.Y,
+                    Name = startNodeForward.Name,
+                    Cost = Math.Sqrt(Math.Pow(RobotNode.X - startNodeForward.X, 2) + Math.Pow(RobotNode.Y - startNodeForward.Y, 2)),
+                    Heuristic = Math.Abs(goal.X - startNodeForward.X) + Math.Abs(goal.Y - startNodeForward.Y),
+                });
+            }
+            if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward)
+            {
+                RobotNode.NegativeNodes.Add(new()
+                {
+                    Id = startNodeBackward.Id,
+                    X = startNodeBackward.X,
+                    Y = startNodeBackward.Y,
+                    Name = startNodeBackward.Name,
+                    Cost = Math.Sqrt(Math.Pow(RobotNode.X - startNodeBackward.X, 2) + Math.Pow(RobotNode.Y - startNodeBackward.Y, 2)),
+                    Heuristic = Math.Abs(goal.X - startNodeBackward.X) + Math.Abs(goal.Y - startNodeBackward.Y),
+                });
+            }
+        }
+
+        if (RobotNode.NegativeNodes.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+
+        var path = Find(RobotNode, goal, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            if (node.Name == "RobotCurrentNode")
+            {
+                Path.Add(new()
+                {
+                    Id = RobotNode.Id,
+                    Name = RobotNode.Name,
+                    X = RobotNode.X,
+                    Y = RobotNode.Y,
+                });
+                continue;
+            }
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = Path };
+    }
+
+    public MessageResult<NodeDto[]> Planning(NodeDto startNode, NodeDto goal, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+        var currentNode = new AStarNode
+        {
+            Id = startNode.Id,
+            X = startNode.X,
+            Y = startNode.Y,
+            NegativeNodes = [..GetNegativeNode(startNode.Id).Select(n => new AStarNode
+            {
+                Id = n.Id,
+                Name = n.Name,
+                X = n.X,
+                Y = n.Y,
+            })],
+        };
+        var path = Find(currentNode, goal, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = [.. Path] };
+    }
+}

RobotNet.RobotManager/Services/Planner/Differential/DifferentialPathPlanner.cs

@@ -0,0 +1,266 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.AStar;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.Differential;
+
+public class DifferentialPathPlanner : IPathPlanner
+{
+    private List<NodeDto> Nodes = [];
+    private List<EdgeDto> Edges = [];
+    private const double ReverseDirectionAngleDegree = 89;
+    private const double Ratio = 0.1;
+
+    private readonly PathPlanningOptions Options = new()
+    {
+        LimitDistanceToEdge = 1,
+        LimitDistanceToNode = 0.3,
+        ResolutionSplit = 0.1,
+    };
+
+    public void SetData(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        Nodes = [.. nodes];
+        Edges = [.. edges];
+    }
+
+    public void SetOptions(PathPlanningOptions options)
+    {
+        Options.LimitDistanceToNode = options.LimitDistanceToNode;
+        Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
+        Options.ResolutionSplit = options.ResolutionSplit;
+    }
+
+    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
+    {
+        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
+        if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
+        if (nodeplanning.Count > 2)
+        {
+            for (int i = 1; i < nodeplanning.Count - 1; i++)
+            {
+                NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
+                NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
+                var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
+                if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
+            }
+        }
+        if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
+        return RobotDirectionInNode;
+    }
+
+    public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
+
+        var RobotNearNode = new NodeDto()
+        {
+            X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
+            Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
+        };
+        var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
+        if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+        else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+    }
+
+    private EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
+    {
+        var EdgesPlanning = new List<EdgeDto>();
+        for (int i = 0; i < nodes.Length - 1; i++)
+        {
+            var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
+                                                  e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
+            if (edge is null)
+            {
+                if (i != 0) return [];
+                EdgesPlanning.Add(new EdgeDto()
+                {
+                    Id = Guid.NewGuid(),
+                    StartNodeId = nodes[i].Id,
+                    EndNodeId = nodes[i + 1].Id,
+                    DirectionAllowed = DirectionAllowed.Both,
+                    TrajectoryDegree = TrajectoryDegree.One,
+                });
+                continue;
+            }
+            bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three;
+            EdgesPlanning.Add(new()
+            {
+                Id = edge.Id,
+                StartNodeId = nodes[i].Id,
+                EndNodeId = nodes[i + 1].Id,
+                DirectionAllowed = edge.DirectionAllowed,
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+            });
+        }
+        return [.. EdgesPlanning];
+    }
+
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(x,
+                                              y,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]");
+        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([goal], []) };
+
+        var edgeplannings = GetEdgesPlanning([..Path.Data]);
+
+        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
+        var FinalDirection = GetRobotDirectionInPath(CurrenDirection, Path.Data, [..edgeplannings]);
+        foreach (var item in Path.Data)
+        {
+            item.Direction = MapCompute.GetNodeDirection(FinalDirection[Path.Data.IndexOf(item)]);
+        }
+
+        return new(true)
+        {
+            Data = ([.. Path.Data], [.. edgeplannings])
+        };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if(basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([goal], []) };
+
+        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(startNode, goal, cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
+        var FinalDirection = GetRobotDirectionInPath(CurrenDirection, [.. Path.Data], [.. edgeplannings]);
+        for (int i = 0; i < Path.Data.Length; i++)
+        {
+            Path.Data[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
+        }
+
+        return new(true)
+        {
+            Data = ([.. Path.Data], [.. edgeplannings])
+        };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+}

RobotNet.RobotManager/Services/Planner/Fokrlift/ForkliftPathPlanner.cs

@@ -0,0 +1,596 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.AStar;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.Fokrlift;
+
+public class ForkliftPathPlanner : IPathPlanner
+{
+    private List<NodeDto> Nodes = [];
+    private List<EdgeDto> Edges = [];
+    private const double ReverseDirectionAngleDegree = 89;
+    private const double RobotDirectionWithAngle = 90;
+    private const double Ratio = 0.1;
+
+    private readonly PathPlanningOptions Options = new()
+    {
+        LimitDistanceToEdge = 1,
+        LimitDistanceToNode = 0.3,
+        ResolutionSplit = 0.1,
+    };
+
+    public void SetData(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        Nodes = [.. nodes];
+        Edges = [.. edges];
+    }
+
+    public void SetOptions(PathPlanningOptions options)
+    {
+        Options.LimitDistanceToNode = options.LimitDistanceToNode;
+        Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
+        Options.ResolutionSplit = options.ResolutionSplit;
+    }
+
+    private static bool TStructureExisted(List<TStructure> TStructures, NodeDto node1, NodeDto node2, NodeDto node3)
+    {
+        var TStructureExistedStep1 = TStructures.Where(ts => ts.Node1 == node1 || ts.Node2 == node1 || ts.Node3 == node1).ToList();
+        if (TStructureExistedStep1.Count != 0)
+        {
+            var TStructureExistedStep2 = TStructureExistedStep1.Where(ts => ts.Node1 == node2 || ts.Node2 == node2 || ts.Node3 == node2).ToList();
+            if (TStructureExistedStep2.Count != 0)
+            {
+                var TStructureExistedStep3 = TStructureExistedStep2.Where(ts => ts.Node1 == node3 || ts.Node2 == node3 || ts.Node3 == node3).ToList();
+                if (TStructureExistedStep3.Count != 0) return true;
+            }
+        }
+        return false;
+    }
+
+    private List<TStructure> GetTStructure()
+    {
+        List<TStructure> TStructures = [];
+        foreach (var node in Nodes)
+        {
+            var inEdges = Edges.Where(edge => edge.StartNodeId == node.Id || edge.EndNodeId == node.Id).ToList();
+            if (inEdges.Count < 2) continue;
+            List<NodeDto> inNodes = [];
+            foreach (var edge in inEdges)
+            {
+                var endNode = Nodes.FirstOrDefault(n => n.Id == (node.Id == edge.EndNodeId ? edge.StartNodeId : edge.EndNodeId));
+                if (endNode is null) continue;
+                inNodes.Add(endNode);
+            }
+            for (int i = 0; i < inNodes.Count - 1; i++)
+            {
+                for (int j = i + 1; j < inNodes.Count; j++)
+                {
+                    if (TStructureExisted(TStructures, node, inNodes[i], inNodes[j])) continue;
+                    var edgeT = Edges.FirstOrDefault(e => (e.StartNodeId == inNodes[i].Id && e.EndNodeId == inNodes[j].Id) ||
+                                                          (e.EndNodeId == inNodes[i].Id && e.StartNodeId == inNodes[j].Id));
+                    var edge1 = inEdges.FirstOrDefault(edge => edge.StartNodeId == inNodes[i].Id || edge.EndNodeId == inNodes[i].Id);
+                    var edge2 = inEdges.FirstOrDefault(edge => edge.StartNodeId == inNodes[j].Id || edge.EndNodeId == inNodes[j].Id);
+                    if (edgeT is null || edge1 is null || edge2 is null) continue;
+                    if (edgeT.TrajectoryDegree == TrajectoryDegree.One &&
+                        edge1.TrajectoryDegree == TrajectoryDegree.One &&
+                        edge2.TrajectoryDegree == TrajectoryDegree.One) continue;
+
+                    TStructures.Add(new()
+                    {
+                        Node1 = node,
+                        Node2 = inNodes[i],
+                        Node3 = inNodes[j],
+                        Edge12 = edge1,
+                        Edge13 = edge2,
+                        Edge23 = edgeT,
+                    });
+                }
+            }
+        }
+        return TStructures;
+    }
+
+    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, NodeDto[] nodeplanning, EdgeDto[] edgePlanning, double reverseDirectionAngle)
+    {
+        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Length];
+        if (nodeplanning.Length > 0) RobotDirectionInNode[0] = currentDirection;
+        if (nodeplanning.Length > 2)
+        {
+            for (int i = 1; i < nodeplanning.Length - 1; i++)
+            {
+                NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
+                NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
+                var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
+                if (angle < reverseDirectionAngle) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
+            }
+        }
+        if (nodeplanning.Length > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
+        return RobotDirectionInNode;
+    }
+
+    public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto futureNode, EdgeDto edge, double robotTheta, double reverseDirectionAngle, bool inGoal)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, futureNode, edge, Ratio);
+
+        var RobotDirectionNearNode = new NodeDto()
+        {
+            X = currentNode.X + Math.Cos(robotTheta * Math.PI / 180),
+            Y = currentNode.Y + Math.Sin(robotTheta * Math.PI / 180),
+        };
+        var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotDirectionNearNode);
+        if (angle > reverseDirectionAngle) return inGoal ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+        else return inGoal ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+    }
+
+    private static double GetNodeAngle(NodeDto node, NodeDto lastNode, EdgeDto edge)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(node, lastNode, edge, Ratio);
+        return Math.Atan2(node.Y - NearNode.Y, node.X - NearNode.X) * 180 / Math.PI;
+    }
+
+    private static (bool IsSuccess, NodeDto? intraNode, TStructure? tstructure) IsReverse(NodeDto currentNode, NodeDto olderNode, NodeDto? futureNode, EdgeDto olderedge, EdgeDto? futureedge, double startAngle, List<TStructure> tstructures)
+    {
+        var tstructures1 = tstructures.Where(t => t.Node1.Id == currentNode.Id || t.Node2.Id == currentNode.Id || t.Node3.Id == currentNode.Id).ToList();
+        if (tstructures1 is null || tstructures1.Count < 1) return (false, null, null);
+        var tstructures2 = tstructures1.Where(t => t.Node1.Id == olderNode.Id || t.Node2.Id == olderNode.Id || t.Node3.Id == olderNode.Id).ToList();
+        if (tstructures2 is null || tstructures2.Count < 1) return (false, null, null);
+        foreach (var ts in tstructures2)
+        {
+            var midleReverse = ts.IsDriectionReverse(currentNode, olderNode);
+            var intraNode = ts.GetIntraNode(currentNode, olderNode);
+            if (intraNode is null) continue;
+
+            if (!ts.IsAccessDirection(olderNode, intraNode) || !ts.IsAccessDirection(intraNode, currentNode)) continue;
+
+            var CurrentDirection = GetRobotDirection(olderNode, currentNode, olderedge, startAngle, ReverseDirectionAngleDegree, false);
+            var intraEdge = ts.GetEdge(olderNode, intraNode);
+            if (intraEdge is null) continue;
+            var ReverseDirection = GetRobotDirection(olderNode, intraNode, intraEdge, startAngle, ReverseDirectionAngleDegree, false);
+            bool firstReverse = ReverseDirection != CurrentDirection;
+
+            bool endReverse = false;
+            if (futureNode is not null && futureedge is not null)
+            {
+                startAngle = GetNodeAngle(currentNode, olderNode, olderedge);
+                CurrentDirection = GetRobotDirection(currentNode, futureNode, futureedge, startAngle, ReverseDirectionAngleDegree, true);
+                intraEdge = ts.GetEdge(currentNode, intraNode);
+                if (intraEdge is null) continue;
+                startAngle = GetNodeAngle(currentNode, intraNode, intraEdge);
+                ReverseDirection = GetRobotDirection(currentNode, futureNode, futureedge, startAngle, ReverseDirectionAngleDegree, true);
+                endReverse = ReverseDirection != CurrentDirection;
+            }
+
+            if (!midleReverse)
+            {
+                if ((!firstReverse && !endReverse) || (firstReverse && endReverse)) continue;
+            }
+            else
+            {
+                if ((firstReverse && !endReverse) || (!firstReverse && endReverse)) continue;
+            }
+            return (true, intraNode, ts);
+        }
+        return (false, null, null);
+    }
+
+    private List<NodeDto> GetIntermediateNode(NodeDto startNode, NodeDto endNode)
+    {
+        var edge1s = Edges.Where(e => e.StartNodeId == startNode.Id || e.EndNodeId == startNode.Id).ToList();
+        var edge2s = Edges.Where(e => e.StartNodeId == endNode.Id || e.EndNodeId == endNode.Id).ToList();
+        if (edge1s is null || edge2s is null || edge1s.Count < 2 || edge2s.Count < 2) return [];
+        List<NodeDto> node1 = [];
+        List<NodeDto> IntermediateNode = [];
+        foreach (var edge1 in edge1s)
+        {
+            if (edge1.TrajectoryDegree != TrajectoryDegree.One) continue;
+            Guid interNodeId = Guid.Empty;
+            if (edge1.StartNodeId == startNode.Id && (edge1.DirectionAllowed == DirectionAllowed.Both || edge1.DirectionAllowed == DirectionAllowed.Forward)) interNodeId = edge1.EndNodeId;
+            else if (edge1.DirectionAllowed == DirectionAllowed.Both || edge1.DirectionAllowed == DirectionAllowed.Forward) interNodeId = edge1.StartNodeId;
+            if (interNodeId == Guid.Empty || interNodeId == endNode.Id) continue;
+            var interNode = Nodes.FirstOrDefault(n => n.Id == interNodeId);
+            if (interNode is null) continue;
+            //(double distance, double x, double y) = PathPlanning.DistanceToRangeSegment(interNode.X, interNode.Y, startNode.X, startNode.Y, endNode.X, endNode.Y);
+            //if (distance < 0.3 && x != startNode.X && x != endNode.X && y != startNode.Y && y != endNode.Y)
+            //{
+            //    node1.Add(interNode);
+            //}
+            node1.Add(interNode);
+        }
+        if (node1.Count == 0) return [];
+        foreach (var edge2 in edge2s)
+        {
+            if (edge2.TrajectoryDegree != TrajectoryDegree.One) continue;
+            Guid interNodeId = Guid.Empty;
+            if (edge2.StartNodeId == endNode.Id && (edge2.DirectionAllowed == DirectionAllowed.Both || edge2.DirectionAllowed == DirectionAllowed.Forward)) interNodeId = edge2.EndNodeId;
+            else if (edge2.DirectionAllowed == DirectionAllowed.Both || edge2.DirectionAllowed == DirectionAllowed.Forward) interNodeId = edge2.StartNodeId;
+            if (interNodeId == Guid.Empty || interNodeId == startNode.Id) continue;
+            var interNode = Nodes.FirstOrDefault(n => n.Id == interNodeId);
+            if (interNode is null) continue;
+            //(double distance, double x, double y) = PathPlanning.DistanceToRangeSegment(interNode.X, interNode.Y, startNode.X, startNode.Y, endNode.X, endNode.Y);
+            //if (distance < 0.3 && x != startNode.X && x != endNode.X && y != startNode.Y && y != endNode.Y)
+            //{
+            //    if (node1.Any(n => n.Id == interNode.Id) && !IntermediateNode.Any(n => n.Id == interNode.Id) && interNode.Id != startNode.Id)
+            //        IntermediateNode.Add(interNode);
+            //}
+            if (node1.Any(n => n.Id == interNode.Id) && !IntermediateNode.Any(n => n.Id == interNode.Id) && interNode.Id != startNode.Id)
+                IntermediateNode.Add(interNode);
+        }
+        return IntermediateNode;
+    }
+
+    private EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
+    {
+        var EdgesPlanning = new List<EdgeDto>();
+        for (int i = 0; i < nodes.Length - 1; i++)
+        {
+            var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
+                                                  e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
+            if (edge is null)
+            {
+                if (i != 0) return [];
+                EdgesPlanning.Add(new EdgeDto()
+                {
+                    Id = Guid.NewGuid(),
+                    StartNodeId = nodes[i].Id,
+                    EndNodeId = nodes[i + 1].Id,
+                    DirectionAllowed = DirectionAllowed.Both,
+                    TrajectoryDegree = TrajectoryDegree.One,
+                });
+                continue;
+            }
+            bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three; ;
+            EdgesPlanning.Add(new()
+            {
+                Id = edge.Id,
+                StartNodeId = nodes[i].Id,
+                EndNodeId = nodes[i + 1].Id,
+                DirectionAllowed = edge.DirectionAllowed,
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+            });
+        }
+        return [.. EdgesPlanning];
+    }
+
+    private (NodeDto[] NodesFilter, EdgeDto[] EdgesFilter) FilterPathPlanning(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length <= 1 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return ([], []);
+        List<NodeDto> nodeFilter = [nodes[0]];
+        for (int i = 1; i < nodes.Length - 1; i++)
+        {
+            var IntermediateNode = GetIntermediateNode(nodes[i - 1], nodes[i]);
+            if (IntermediateNode is null || IntermediateNode.Count == 0)
+            {
+                nodeFilter.Add(nodes[i]);
+                continue;
+            }
+            if (IntermediateNode.Any(n => n.Id == nodes[i + 1].Id))
+            {
+                nodeFilter.Add(nodes[i + 1]);
+                i++;
+            }
+            else nodeFilter.Add(nodes[i]);
+        }
+        nodeFilter.Add(nodes[^1]);
+        var edgeFilter = GetEdgesPlanning([.. nodeFilter]);
+        if (nodeFilter.Count - 1 != edgeFilter.Length) return ([], []);
+        return ([.. nodeFilter], [.. edgeFilter]);
+    }
+
+    private double GetLength(EdgeDto[] edges)
+    {
+        if (edges.Length == 0) return 999;
+        double distance = 0;
+        for (int i = 0; i < edges.Length; i++)
+        {
+            var edge = edges.FirstOrDefault(e => e.Id == edges[i].Id);
+            if (edge is null) return 999;
+            var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+            var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) return 999;
+            distance += MapEditorHelper.GetEdgeLength(new()
+            {
+                X1 = startNode.X,
+                X2 = endNode.X,
+                Y1 = startNode.Y,
+                Y2 = endNode.Y,
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y = edge.ControlPoint2Y,
+            });
+        }
+        return distance;
+    }
+
+    private static NodeDto[] CalculatorDirection(NodeDto[] nodes, EdgeDto[] edges, RobotDirection currentDirection)
+    {
+        var FinalDirection = GetRobotDirectionInPath(currentDirection, nodes, edges, ReverseDirectionAngleDegree);
+        NodeDto[] returnNodes = [.. nodes];
+        for (var i = 0; i < returnNodes.Length; i++)
+        {
+            returnNodes[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
+        }
+        return returnNodes;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(x,
+                                              y,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]");
+        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, RobotDirectionWithAngle, false);
+
+        return new(true)
+        {
+            Data = ([.. CalculatorDirection([.. Path.Data], [.. edgeplannings], CurrenDirection)], [.. edgeplannings])
+        };
+    }
+
+    private MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> CheckPathWithFinalDirection(NodeDto[] nodes, EdgeDto[] edges, double currentAngle, RobotDirection goalDirection = RobotDirection.NONE)
+    {
+        if ((nodes[^1].Direction == MapCompute.GetNodeDirection(goalDirection) && GetLength([.. edges]) < 10) || goalDirection == RobotDirection.NONE)
+            return new(true) { Data = FilterPathPlanning([.. nodes], [.. edges]) };
+
+        var edgeplannings = edges.ToList();
+        var nodeplannings = nodes.ToList();
+
+        var TStructures = GetTStructure();
+
+        Guid LastReverseDirectionId = Guid.Empty;
+        NodeDto LastNodeReverseDirection = new();
+        for (int i = 1; i < nodeplannings.Count; i++)
+        {
+            if (nodeplannings[i].Direction == Direction.FORWARD) continue;
+            NodeDto? futureNode = null;
+            EdgeDto? futureEdge = null;
+            if (i < nodeplannings.Count - 1)
+            {
+                futureNode = nodeplannings[i + 1];
+                futureEdge = edgeplannings[i];
+            }
+            double startAngle = currentAngle;
+            if (i >= 2) startAngle = GetNodeAngle(nodeplannings[i - 1], nodeplannings[i - 2], edgeplannings[i - 2]);
+            (var IsSuccess, var intraNode, var tstructure) = IsReverse(nodeplannings[i], nodeplannings[i - 1], futureNode, edgeplannings[i - 1], futureEdge, startAngle, TStructures);
+            if (!IsSuccess || intraNode is null || tstructure is null) continue;
+            var edge1 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 1].Id && e.EndNodeId == intraNode.Id) ||
+                                                   e.EndNodeId == nodeplannings[i - 1].Id && e.StartNodeId == intraNode.Id);
+            var edge2 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == intraNode.Id) ||
+                                                   e.EndNodeId == nodeplannings[i].Id && e.StartNodeId == intraNode.Id);
+            if (edge1 is null || edge2 is null) continue;
+            edgeplannings.RemoveAt(i - 1);
+            edgeplannings.Insert(i - 1, new()
+            {
+                Id = edge1.Id,
+                StartNodeId = nodeplannings[i - 1].Id,
+                EndNodeId = intraNode.Id,
+                TrajectoryDegree = edge1.TrajectoryDegree,
+                DirectionAllowed = edge1.DirectionAllowed,
+                ControlPoint1X = edge1.ControlPoint1X,
+                ControlPoint1Y = edge1.ControlPoint1Y,
+                ControlPoint2X = edge1.ControlPoint2X,
+                ControlPoint2Y = edge1.ControlPoint2Y
+            });
+            edgeplannings.Insert(i, new()
+            {
+                Id = edge2.Id,
+                StartNodeId = intraNode.Id,
+                EndNodeId = nodeplannings[i].Id,
+                TrajectoryDegree = edge2.TrajectoryDegree,
+                DirectionAllowed = edge2.DirectionAllowed,
+                ControlPoint1X = edge2.ControlPoint1X,
+                ControlPoint1Y = edge2.ControlPoint1Y,
+                ControlPoint2X = edge2.ControlPoint2X,
+                ControlPoint2Y = edge2.ControlPoint2Y
+            });
+            nodeplannings.Insert(i, intraNode);
+            var directionInPath = GetRobotDirectionInPath(MapCompute.GetRobotDirection(nodeplannings[0].Direction), [.. nodeplannings], [.. edgeplannings], ReverseDirectionAngleDegree);
+            foreach (var node in nodeplannings)
+            {
+                node.Direction = MapCompute.GetNodeDirection(directionInPath[nodeplannings.IndexOf(node)]);
+            }
+            LastReverseDirectionId = tstructure.Id;
+            LastNodeReverseDirection = nodeplannings[i + 1];
+            i++;
+        }
+
+        if (nodeplannings[^1].Direction == MapCompute.GetNodeDirection(goalDirection))
+            return new(true) { Data = FilterPathPlanning([.. nodeplannings], [.. edgeplannings]) };
+
+        for (int i = nodeplannings.Count - 1; i > 0; i--)
+        {
+            NodeDto? futureNode = null;
+            EdgeDto? futureEdge = null;
+            if (i < nodeplannings.Count - 1)
+            {
+                futureNode = nodeplannings[i + 1];
+                futureEdge = edgeplannings[i];
+            }
+            double startAngle = currentAngle;
+            if (i >= 2) startAngle = GetNodeAngle(nodeplannings[i - 1], nodeplannings[i - 2], edgeplannings[i - 2]);
+            (var IsSuccess, var intraNode, var tstructure) = IsReverse(nodeplannings[i], nodeplannings[i - 1], futureNode, edgeplannings[i - 1], futureEdge, startAngle, TStructures);
+            if (!IsSuccess || intraNode is null || tstructure is null) continue;
+
+            if (nodeplannings[i - 1].Id == LastNodeReverseDirection.Id)
+            {
+                var edge = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 2].Id && e.EndNodeId == nodeplannings[i].Id) ||
+                                                        (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == nodeplannings[i - 2].Id));
+                if (edge is null) continue;
+                edgeplannings.Insert(i - 2, new()
+                {
+                    Id = edge.Id,
+                    StartNodeId = nodeplannings[i - 2].Id,
+                    EndNodeId = nodeplannings[i].Id,
+                    TrajectoryDegree = edge.TrajectoryDegree,
+                    DirectionAllowed = edge.DirectionAllowed,
+                    ControlPoint1X = edge.ControlPoint1X,
+                    ControlPoint1Y = edge.ControlPoint1Y,
+                    ControlPoint2X = edge.ControlPoint2X,
+                    ControlPoint2Y = edge.ControlPoint2Y
+                });
+                edgeplannings.RemoveAt(i);
+                edgeplannings.RemoveAt(i - 1);
+                nodeplannings.RemoveAt(i - 1);
+            }
+            else if (tstructure.Id != LastReverseDirectionId || i < 2)
+            {
+                var edge1 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 1].Id && e.EndNodeId == intraNode.Id) ||
+                                                   e.EndNodeId == nodeplannings[i - 1].Id && e.StartNodeId == intraNode.Id);
+                var edge2 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == intraNode.Id) ||
+                                                       e.EndNodeId == nodeplannings[i].Id && e.StartNodeId == intraNode.Id);
+                if (edge1 is null || edge2 is null) continue;
+                edgeplannings.RemoveAt(i - 1);
+                edgeplannings.Insert(i - 1, new()
+                {
+                    Id = edge1.Id,
+                    StartNodeId = nodeplannings[i - 1].Id,
+                    EndNodeId = intraNode.Id,
+                    TrajectoryDegree = edge1.TrajectoryDegree,
+                    DirectionAllowed = edge1.DirectionAllowed,
+                    ControlPoint1X = edge1.ControlPoint1X,
+                    ControlPoint1Y = edge1.ControlPoint1Y,
+                    ControlPoint2X = edge1.ControlPoint2X,
+                    ControlPoint2Y = edge1.ControlPoint2Y,
+                });
+                edgeplannings.Insert(i, new()
+                {
+                    Id = edge2.Id,
+                    StartNodeId = intraNode.Id,
+                    EndNodeId = nodeplannings[i].Id,
+                    TrajectoryDegree = edge2.TrajectoryDegree,
+                    DirectionAllowed = edge2.DirectionAllowed,
+                    ControlPoint1X = edge2.ControlPoint1X,
+                    ControlPoint1Y = edge2.ControlPoint1Y,
+                    ControlPoint2X = edge2.ControlPoint2X,
+                    ControlPoint2Y = edge2.ControlPoint2Y,
+                });
+                nodeplannings.Insert(i, intraNode);
+            }
+            else
+            {
+                var edge = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 2].Id && e.EndNodeId == nodeplannings[i].Id) ||
+                                                        (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == nodeplannings[i - 2].Id));
+                if (edge is null) continue;
+                edgeplannings.Insert(i - 2, new()
+                {
+                    Id = edge.Id,
+                    StartNodeId = nodeplannings[i - 2].Id,
+                    EndNodeId = nodeplannings[i].Id,
+                    TrajectoryDegree = edge.TrajectoryDegree,
+                    DirectionAllowed = edge.DirectionAllowed,
+                    ControlPoint1X = edge.ControlPoint1X,
+                    ControlPoint1Y = edge.ControlPoint1Y,
+                    ControlPoint2X = edge.ControlPoint2X,
+                    ControlPoint2Y = edge.ControlPoint2Y,
+                });
+                edgeplannings.RemoveAt(i);
+                edgeplannings.RemoveAt(i - 1);
+                nodeplannings.RemoveAt(i - 1);
+            }
+            return new(true) { Data = FilterPathPlanning([.. CalculatorDirection([.. nodeplannings], [.. edgeplannings], MapCompute.GetRobotDirection(nodeplannings[0].Direction))], [.. edgeplannings]) };
+        }
+        return new(false, $"Đường đi đến đích không thỏa mãn điều kiện");
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, RobotDirectionWithAngle, true);
+
+        return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(startNode, goal, cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, RobotDirectionWithAngle, false);
+
+        return new(true)
+        {
+            Data = ([.. CalculatorDirection([.. Path.Data], [.. edgeplannings], CurrenDirection)], [.. edgeplannings])
+        };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, RobotDirectionWithAngle, true);
+
+        return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
+    }
+}

RobotNet.RobotManager/Services/Planner/Fokrlift/TStructure.cs

@@ -0,0 +1,135 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Planner.Fokrlift;
+public enum TStructureDirection
+{
+    NODE1_NODE2_NODE3,
+    NODE1_NODE3_NODE2,
+    NODE2_NODE1_NODE3,
+    NODE2_NODE3_NODE1,
+    NODE3_NODE2_NODE1,
+    NODE3_NODE1_NODE2,
+}
+
+public class TStructure
+{
+    public Guid Id { get; set; } = Guid.NewGuid();
+    public NodeDto Node1 { get; set; } = new();
+    public NodeDto Node2 { get; set; } = new();
+    public NodeDto Node3 { get; set; } = new();
+    public EdgeDto Edge12 { get; set; } = new();
+    public EdgeDto Edge13 { get; set; } = new();
+    public EdgeDto Edge23 { get; set; } = new();
+    private const double Ratio = 0.1;
+
+    public bool IsDriectionReverse(TStructureDirection direction)
+    {
+        NodeDto OriginNode = new();
+        NodeDto ToWardNode1 = new();
+        NodeDto ToWardNode2 = new();
+        EdgeDto ToWardEdge1 = new();
+        EdgeDto ToWardEdge2 = new();
+
+        switch (direction)
+        {
+            case TStructureDirection.NODE3_NODE2_NODE1:
+            case TStructureDirection.NODE1_NODE2_NODE3:
+                OriginNode = Node2;
+                ToWardNode1 = Node1;
+                ToWardNode2 = Node3;
+                ToWardEdge1 = Edge12;
+                ToWardEdge2 = Edge23;
+                break;
+            case TStructureDirection.NODE2_NODE3_NODE1:
+            case TStructureDirection.NODE1_NODE3_NODE2:
+                OriginNode = Node3;
+                ToWardNode1 = Node1;
+                ToWardNode2 = Node2;
+                ToWardEdge1 = Edge13;
+                ToWardEdge2 = Edge23;
+                break;
+            case TStructureDirection.NODE3_NODE1_NODE2:
+            case TStructureDirection.NODE2_NODE1_NODE3:
+                OriginNode = Node1;
+                ToWardNode1 = Node2;
+                ToWardNode2 = Node3;
+                ToWardEdge1 = Edge12;
+                ToWardEdge2 = Edge13;
+                break;
+        }
+
+        var NearToWardNode1 = MapEditorHelper.GetNearByNode(OriginNode, ToWardNode1, ToWardEdge1, Ratio);
+        var NearToWardNode3 = MapEditorHelper.GetNearByNode(OriginNode, ToWardNode2, ToWardEdge2, Ratio);
+        var angle = MapEditorHelper.GetAngle(OriginNode, NearToWardNode1, NearToWardNode3);
+        if (angle < 50) return true;
+        return false;
+    }
+
+    public bool IsDriectionReverse(NodeDto node1, NodeDto node2)
+    {
+        if (node1.Id == Node1.Id)
+        {
+            if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE3_NODE2);
+            else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE2_NODE3);
+        }
+        else if (node1.Id == Node2.Id)
+        {
+            if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE3_NODE1);
+            else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE1_NODE3);
+        }
+        else if (node1.Id == Node3.Id)
+        {
+            if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE2_NODE1);
+            else if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE1_NODE2);
+        }
+        return false;
+    }
+
+    public NodeDto? GetIntraNode(NodeDto node1, NodeDto node2)
+    {
+        if (node1.Id == Node1.Id)
+        {
+            if (node2.Id == Node2.Id) return Node3;
+            else if (node2.Id == Node3.Id) return Node2;
+        }
+        else if (node1.Id == Node2.Id)
+        {
+            if (node2.Id == Node1.Id) return Node3;
+            else if (node2.Id == Node3.Id) return Node1;
+        }
+        else if (node1.Id == Node3.Id)
+        {
+            if (node2.Id == Node1.Id) return Node2;
+            else if (node2.Id == Node2.Id) return Node1;
+        }
+        return null;
+    }
+
+    public EdgeDto? GetEdge(NodeDto node1, NodeDto node2)
+    {
+        if (Edge12.StartNodeId == node1.Id || Edge12.EndNodeId == node1.Id)
+        {
+            if (Edge12.StartNodeId == node2.Id || Edge12.EndNodeId == node2.Id) return Edge12;
+        }
+        if (Edge13.StartNodeId == node1.Id || Edge13.EndNodeId == node1.Id)
+        {
+            if (Edge13.StartNodeId == node2.Id || Edge13.EndNodeId == node2.Id) return Edge13;
+        }
+        if (Edge23.StartNodeId == node1.Id || Edge23.EndNodeId == node1.Id)
+        {
+            if (Edge23.StartNodeId == node2.Id || Edge23.EndNodeId == node2.Id) return Edge23;
+        }
+        return null;
+    }
+
+    public bool IsAccessDirection(NodeDto startNode, NodeDto endNode)
+    {
+        var edge = GetEdge(startNode, endNode);
+        if (edge is null) return false;
+        if (edge.StartNodeId == startNode.Id && (edge.DirectionAllowed == DirectionAllowed.Both || edge.DirectionAllowed == DirectionAllowed.Forward)) return true;
+        if (edge.EndNodeId == startNode.Id && (edge.DirectionAllowed == DirectionAllowed.Both || edge.DirectionAllowed == DirectionAllowed.Backward)) return true;
+        return false;
+    }
+}

RobotNet.RobotManager/Services/Planner/ForkliftV2/ForkLiftPathPlannerV2.cs

@@ -0,0 +1,310 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.AStar;
+using RobotNet.RobotManager.Services.Planner.Fokrlift;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.RobotShares.Models;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
+
+public class ForkLiftPathPlannerV2 : IPathPlanner
+{
+    private List<NodeDto> Nodes = [];
+    private List<EdgeDto> Edges = [];
+    private const double ReverseDirectionAngleDegree = 80;
+    private const double RobotDirectionWithAngle = 90;
+    private const double Ratio = 0.1;
+
+    private readonly PathPlanningOptions Options = new()
+    {
+        LimitDistanceToEdge = 1,
+        LimitDistanceToNode = 0.3,
+        ResolutionSplit = 0.1,
+    };
+
+    public void SetData(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        Nodes = [.. nodes];
+        Edges = [.. edges];
+    }
+
+    public void SetOptions(PathPlanningOptions options)
+    {
+        Options.LimitDistanceToNode = options.LimitDistanceToNode;
+        Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
+        Options.ResolutionSplit = options.ResolutionSplit;
+    }
+
+    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
+    {
+        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
+        if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
+        if (nodeplanning.Count > 2)
+        {
+            for (int i = 1; i < nodeplanning.Count - 1; i++)
+            {
+                NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
+                NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
+                var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
+                if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
+            }
+        }
+        if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
+        return RobotDirectionInNode;
+    }
+
+    public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
+
+        var RobotNearNode = new NodeDto()
+        {
+            X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
+            Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
+        };
+        var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
+        if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+        else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+    }
+
+    private static NodeDto[] CalculatorDirection(List<NodeDto> nodes, List<EdgeDto> edges, RobotDirection currentDirection)
+    {
+        var FinalDirection = GetRobotDirectionInPath(currentDirection, nodes, edges);
+        NodeDto[] returnNodes = [.. nodes];
+        foreach (var item in returnNodes)
+        {
+            item.Direction = MapCompute.GetNodeDirection(FinalDirection[nodes.IndexOf(item)]);
+        }
+        return returnNodes;
+    }
+
+    private List<EdgeDto> GetEdgesPlanning(List<NodeDto> nodes)
+    {
+        var EdgesPlanning = new List<EdgeDto>();
+        for (int i = 0; i < nodes.Count - 1; i++)
+        {
+            var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
+                                                  e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
+            if (edge is null)
+            {
+                if (i != 0) return [];
+                EdgesPlanning.Add(new EdgeDto()
+                {
+                    Id = Guid.NewGuid(),
+                    StartNodeId = nodes[i].Id,
+                    EndNodeId = nodes[i + 1].Id,
+                    DirectionAllowed = DirectionAllowed.Both,
+                    TrajectoryDegree = TrajectoryDegree.One,
+                });
+                continue;
+            }
+            bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three; ;
+            EdgesPlanning.Add(new()
+            {
+                Id = edge.Id,
+                StartNodeId = nodes[i].Id,
+                EndNodeId = nodes[i + 1].Id,
+                DirectionAllowed = edge.DirectionAllowed,
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+            });
+        }
+        return EdgesPlanning;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(x,
+                                              y,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]") { Data = ([goal], []) };
+        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+        return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(x,
+                                              y,
+                                              theta,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y, Theta = goal.Theta },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess|| Path.Data is null || Path.Data.Count < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithAngle(x,
+                                                    y,
+                                                    theta,
+                                                    goal.Id,
+                                                    cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], [])};
+
+            var edgeplannings = GetEdgesPlanning(Path.Data);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(x,
+                                              y,
+                                              theta,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              goalDirection,
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess || Path.Data is null || Path.Data.Count < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithFinalDirection(x,
+                                                                  y,
+                                                                  theta,
+                                                                  goal.Id,
+                                                                  goalDirection,
+                                                                  cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], []) };
+
+            var edgeplannings = GetEdgesPlanning(Path.Data);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(startNode,
+                                            goal,
+                                            cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
+        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        return new(true) { Data = ([.. CalculatorDirection([..Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(startNode,
+                                                            theta,
+                                                            goal,
+                                                            cancellationToken);
+        if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithAngle(startNodeId,
+                                                    theta,
+                                                    goal.Id,
+                                                    cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
+
+            var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(startNode,
+                                                                  theta,
+                                                                  goal,
+                                                                  goalDirection,
+                                                                  cancellationToken);
+        if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithFinalDirection(startNodeId,
+                                                                theta,
+                                                                goal.Id,
+                                                                goalDirection,
+                                                                cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
+
+            var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+}

RobotNet.RobotManager/Services/Planner/ForkliftV2/SSEAStarNode.cs

@@ -0,0 +1,31 @@
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
+
+#nullable disable
+
+public class SSEAStarNode
+{
+    public Guid Id { get; set; }
+    public double X { get; set; }
+    public double Y { get; set; }
+    public RobotDirection Direction { get; set; }
+    public double Cost { get; set; }
+    public double Heuristic { get; set; }
+    public double TotalCost => Cost + Heuristic;
+    public string Name { get; set; }
+    public SSEAStarNode Parent { get; set; }
+    public List<SSEAStarNode> NegativeNodes { get; set; } = [];
+
+    public override bool Equals(object obj)
+    {
+        if (obj is SSEAStarNode other)
+            return Id == other.Id && Direction == other.Direction;
+        return false;
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Id, Direction);
+    }
+}

RobotNet.RobotManager/Services/Planner/ForkliftV2/SSEAStarPathPlanner.cs

@@ -0,0 +1,621 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
+
+public class SSEAStarPathPlanner(List<NodeDto> Nodes, List<EdgeDto> Edges)
+{
+    private NodeDto? GetOnNode(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
+    {
+        if (cancellationToken?.IsCancellationRequested == true) return null;
+        KDTree KDTree = new(Nodes);
+        return KDTree.FindNearest(x, y, limitDistance);
+    }
+
+    //public EdgeDto? GetClosesEdge(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
+    //{
+    //    if (cancellationToken?.IsCancellationRequested == true) return null;
+    //    RTree RTree = new(Nodes, Edges);
+    //    return RTree.FindNearest(x, y, limitDistance);
+    //}
+
+    public EdgeDto? GetClosesEdge(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
+    {
+        double minDistance = double.MaxValue;
+        EdgeDto? edgeResult = null;
+        foreach (var edge in Edges)
+        {
+            if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return null;
+            var startNode = Nodes.FirstOrDefault(node => node.Id == edge.StartNodeId);
+            var endNode = Nodes.FirstOrDefault(node => node.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) continue;
+
+            var getDistance = MapCompute.DistanceToEdge(x, y, edge, startNode, endNode);
+            if (getDistance.IsSuccess)
+            {
+                if (getDistance.Data < minDistance)
+                {
+                    minDistance = getDistance.Data;
+                    edgeResult = edge;
+                }
+            }
+        }
+        if (minDistance <= limitDistance) return edgeResult;
+        else return null;
+    }
+
+    public List<NodeDto> GetNegativeNode(Guid nodeId, CancellationToken? cancellationToken = null)
+    {
+        var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
+        if (node is null) return [];
+
+        var listNodesNegative = new List<NodeDto>();
+        var listPaths = Edges.Where(p => p.EndNodeId == nodeId || p.StartNodeId == nodeId);
+        foreach (var path in listPaths)
+        {
+            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+            if (path.StartNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
+                if (nodeAdd != null) listNodesNegative.Add(nodeAdd);
+                continue;
+            }
+            if (path.EndNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.StartNodeId);
+                if (nodeAdd != null) listNodesNegative.Add(nodeAdd);
+                continue;
+            }
+        }
+        return listNodesNegative;
+    }
+
+    private double GetNegativeCost(SSEAStarNode currentNode, SSEAStarNode negativeNode)
+    {
+        var negativeEdges = Edges.Where(e => e.StartNodeId == currentNode.Id && e.EndNodeId == negativeNode.Id ||
+                                            e.StartNodeId == negativeNode.Id && e.EndNodeId == currentNode.Id).ToList();
+        double minDistance = double.MaxValue;
+        foreach (var edge in negativeEdges)
+        {
+            var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+            var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+            if (startNode is null || endNode is null) return 0;
+            var distance = MapEditorHelper.GetEdgeLength(new()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y = edge.ControlPoint2Y,
+                TrajectoryDegree = edge.TrajectoryDegree,
+            });
+            if (distance < minDistance) minDistance = distance;
+        }
+        return minDistance != double.MaxValue ? minDistance : 0;
+    }
+
+    private List<SSEAStarNode> GetNegativeAStarNode(SSEAStarNode nodeCurrent, NodeDto endNode, CancellationToken? cancellationToken = null)
+    {
+        var possiblePointNegative = new List<SSEAStarNode>();
+        if (nodeCurrent.Id == endNode.Id) return possiblePointNegative;
+
+        var listNodesNegative = GetNegativeNode(nodeCurrent.Id, cancellationToken);
+
+        foreach (var item in listNodesNegative)
+        {
+            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+            if (nodeCurrent.Parent is null) continue;
+            var nodeDtoCurrent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Id);
+            var nodeDtoNegative = Nodes.FirstOrDefault(n => n.Id == item.Id);
+            var nodeDtoParent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Parent.Id);
+            var negativeEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == item.Id || e.EndNodeId == nodeCurrent.Id && e.StartNodeId == item.Id);
+            var parentEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == nodeCurrent.Parent.Id || e.EndNodeId == nodeCurrent.Id && e.StartNodeId == nodeCurrent.Parent.Id);
+
+            if (nodeDtoCurrent is null || nodeDtoNegative is null || negativeEdge is null) continue;
+
+            var nearNodeNevgative = MapEditorHelper.GetNearByNode(nodeDtoCurrent, nodeDtoNegative, negativeEdge, 0.01);
+            var nearNodeParent = nodeDtoParent is not null && parentEdge is not null ? MapEditorHelper.GetNearByNode(nodeDtoCurrent, nodeDtoParent, parentEdge, 0.01) :
+                                 new()
+                                 {
+                                     Id = nodeCurrent.Parent.Id,
+                                     X = nodeCurrent.Parent.X,
+                                     Y = nodeCurrent.Parent.Y,
+                                     Name = nodeCurrent.Parent.Name
+                                 };
+
+            var angle = MapEditorHelper.GetAngle(nodeDtoCurrent, nearNodeNevgative, nearNodeParent);
+            RobotDirection direction = angle < 89 ? nodeCurrent.Direction == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD : nodeCurrent.Direction;
+
+            var nodeNegative = new SSEAStarNode
+            {
+                Id = item.Id,
+                X = item.X,
+                Y = item.Y,
+                Name = item.Name,
+                Direction = direction,
+                Parent = nodeCurrent
+            };
+
+            var cost = GetNegativeCost(nodeCurrent, nodeNegative);
+            cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2));
+            nodeNegative.Cost = cost + nodeCurrent.Cost + (direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0);
+            var distance = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
+            nodeNegative.Heuristic = distance + (direction == RobotDirection.BACKWARD ? distance : 0.0);
+            possiblePointNegative.Add(nodeNegative);
+        }
+        if (nodeCurrent.NegativeNodes is not null && nodeCurrent.NegativeNodes.Count > 0) possiblePointNegative.AddRange(nodeCurrent.NegativeNodes);
+        return possiblePointNegative;
+    }
+
+    public List<SSEAStarNode> Find(SSEAStarNode startNode, NodeDto endNode, RobotDirection goalDirection, CancellationToken? cancellationToken = null)
+    {
+        try
+        {
+            var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
+            var visitedNodes = new HashSet<SSEAStarNode>();
+            var path = new List<SSEAStarNode>();
+            var shortestPath = new HashSet<SSEAStarNode>();
+
+            activeNodes.Enqueue(startNode);
+
+            while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
+            {
+                var checkNode = activeNodes.Dequeue();
+                if (checkNode.Id == endNode.Id)
+                {
+                    if (checkNode.Direction == goalDirection)
+                    {
+                        var node = checkNode;
+                        while (node != null)
+                        {
+                            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+                            path.Add(node);
+                            node = node.Parent;
+                        }
+                        return path;
+                    }
+                    else
+                    {
+                        var node = checkNode;
+                        while (node != null)
+                        {
+                            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+                            shortestPath.Add(node);
+                            node = node.Parent;
+                        }
+                    }
+                }
+
+                visitedNodes.Add(checkNode);
+
+                var listNodeNegative = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
+                foreach (var node in listNodeNegative)
+                {
+                    if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
+                    {
+                        if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
+                        {
+                            visitedNodes.Remove(value);
+                            activeNodes.Enqueue(node);
+                        }
+                        continue;
+                    }
+
+                    var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Direction == node.Direction);
+                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
+                    {
+                        activeNodes.Items.Remove(activeNode);
+                        activeNodes.Enqueue(node);
+                    }
+                    else
+                    {
+                        activeNodes.Enqueue(node);
+                    }
+                }
+            }
+
+            return [];
+        }
+        catch
+        {
+            return [];
+        }
+    }
+
+    public List<SSEAStarNode> Find(SSEAStarNode startNode, NodeDto endNode, CancellationToken? cancellationToken = null)
+    {
+        try
+        {
+            var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
+            var visitedNodes = new HashSet<SSEAStarNode>();
+            var path = new List<SSEAStarNode>();
+            var shortestPath = new HashSet<SSEAStarNode>();
+
+            activeNodes.Enqueue(startNode);
+
+            while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
+            {
+                var checkNode = activeNodes.Dequeue();
+                if (checkNode.Id == endNode.Id)
+                {
+                    if (checkNode.Parent is not null)
+                    {
+                        var nodeParentDto = Nodes.FirstOrDefault(n => n.Id == checkNode.Parent.Id);
+                        var edge = Edges.FirstOrDefault(e => e.StartNodeId == checkNode.Id && e.EndNodeId == checkNode.Parent.Id || e.EndNodeId == checkNode.Id && e.StartNodeId == checkNode.Parent.Id);
+                        if (edge is not null && nodeParentDto is not null)
+                        {
+                            var nearParent = MapEditorHelper.GetNearByNode(endNode, nodeParentDto, edge, 0.01);
+                            var nearGoalNode = new NodeDto()
+                            {
+                                X = endNode.X + Math.Cos(endNode.Theta * Math.PI / 180),
+                                Y = endNode.Y + Math.Sin(endNode.Theta * Math.PI / 180),
+                            };
+
+                            var angle = MapEditorHelper.GetAngle(endNode, nearParent, nearGoalNode);
+                            RobotDirection goalDirection = angle < 89 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                            if (checkNode.Direction == goalDirection)
+                            {
+                                var returnNode = checkNode;
+                                while (returnNode != null)
+                                {
+                                    if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+                                    path.Add(returnNode);
+                                    returnNode = returnNode.Parent;
+                                }
+                                return path;
+                            }
+                        }
+                    }
+
+                    var node = checkNode;
+                    while (node != null)
+                    {
+                        if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+                        shortestPath.Add(node);
+                        node = node.Parent;
+                    }
+                }
+
+                visitedNodes.Add(checkNode);
+
+                var listNodeNegative = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
+                foreach (var node in listNodeNegative)
+                {
+                    if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
+                    {
+                        if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
+                        {
+                            visitedNodes.Remove(value);
+                            activeNodes.Enqueue(node);
+                        }
+                        continue;
+                    }
+
+                    var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Direction == node.Direction);
+                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
+                    {
+                        activeNodes.Items.Remove(activeNode);
+                        activeNodes.Enqueue(node);
+                    }
+                    else
+                    {
+                        activeNodes.Enqueue(node);
+                    }
+                }
+            }
+
+            return [];
+        }
+        catch
+        {
+            return [];
+        }
+    }
+
+    private SSEAStarNode GetClosesNode(NodeDto closesNode, NodeDto goal, double theta, CancellationToken? cancellationToken = null)
+    {
+        SSEAStarNode closesAStarNode = new()
+        {
+            Id = closesNode.Id,
+            X = closesNode.X,
+            Y = closesNode.Y,
+            Name = closesNode.Name,
+        };
+        foreach (var negativeNode in GetNegativeNode(closesAStarNode.Id, cancellationToken))
+        {
+            SSEAStarNode closesAStarNodeParent = new()
+            {
+                Id = closesNode.Id,
+                X = closesNode.X,
+                Y = closesNode.Y,
+                Name = closesNode.Name,
+            };
+            var RobotNearNode = new NodeDto()
+            {
+                X = closesAStarNode.X + Math.Cos(theta * Math.PI / 180),
+                Y = closesAStarNode.Y + Math.Sin(theta * Math.PI / 180),
+            };
+
+            var angle = MapEditorHelper.GetAngle(closesNode, negativeNode, RobotNearNode);
+            RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+
+            var cost = GetNegativeCost(closesAStarNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
+            cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(closesAStarNode.X - negativeNode.X, 2) + Math.Pow(closesAStarNode.Y - negativeNode.Y, 2));
+            cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
+            closesAStarNodeParent.Direction = direction;
+            closesAStarNode.NegativeNodes.Add(new()
+            {
+                Id = negativeNode.Id,
+                X = negativeNode.X,
+                Y = negativeNode.Y,
+                Name = negativeNode.Name,
+                Direction = direction,
+                Cost = cost,
+                Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
+                Parent = closesAStarNodeParent,
+            });
+        }
+        return closesAStarNode;
+    }
+
+    private static SSEAStarNode[] GetClosesEdge(EdgeDto closesEdge, NodeDto goal, NodeDto startNodeForward, NodeDto startNodeBackward, SSEAStarNode robotNode, double theta)
+    {
+        List<SSEAStarNode> negativeNodes = [];
+        if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward)
+        {
+            SSEAStarNode closesAStarNodeParent = new()
+            {
+                Id = robotNode.Id,
+                X = robotNode.X,
+                Y = robotNode.Y,
+                Name = robotNode.Name,
+            };
+            var RobotNearNode = new NodeDto()
+            {
+                X = robotNode.X + Math.Cos(theta * Math.PI / 180),
+                Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
+            };
+            var angle = MapEditorHelper.GetAngle(new() { X = robotNode.X, Y = robotNode.Y, Theta = theta }, startNodeForward, RobotNearNode);
+            RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+
+            double cost = Math.Sqrt(Math.Pow(robotNode.X - startNodeBackward.X, 2) + Math.Pow(robotNode.Y - startNodeBackward.Y, 2));
+            cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
+            closesAStarNodeParent.Direction = direction;
+            negativeNodes.Add(new()
+            {
+                Id = startNodeForward.Id,
+                X = startNodeForward.X,
+                Y = startNodeForward.Y,
+                Name = startNodeForward.Name,
+                Direction = direction,
+                Cost = cost,
+                Heuristic = Math.Abs(goal.X - startNodeForward.X) + Math.Abs(goal.Y - startNodeForward.Y),
+                Parent = closesAStarNodeParent,
+            });
+        }
+        if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward)
+        {
+            SSEAStarNode closesAStarNodeParent = new()
+            {
+                Id = robotNode.Id,
+                X = robotNode.X,
+                Y = robotNode.Y,
+                Name = robotNode.Name,
+            };
+            var RobotNearNode = new NodeDto()
+            {
+                X = robotNode.X + Math.Cos(theta * Math.PI / 180),
+                Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
+            };
+            var angle = MapEditorHelper.GetAngle(new() { X = robotNode.X, Y = robotNode.Y, Theta = theta }, startNodeBackward, RobotNearNode);
+            RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+
+            double cost = Math.Sqrt(Math.Pow(robotNode.X - startNodeBackward.X, 2) + Math.Pow(robotNode.Y - startNodeBackward.Y, 2));
+            cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
+            closesAStarNodeParent.Direction = direction;
+            negativeNodes.Add(new()
+            {
+                Id = startNodeBackward.Id,
+                X = startNodeBackward.X,
+                Y = startNodeBackward.Y,
+                Name = startNodeBackward.Name,
+                Direction = direction,
+                Cost = cost,
+                Heuristic = Math.Abs(goal.X - startNodeBackward.X) + Math.Abs(goal.Y - startNodeBackward.Y),
+                Parent = closesAStarNodeParent,
+            });
+        }
+        return [.. negativeNodes];
+    }
+
+    public MessageResult<List<NodeDto>> PlanningWithFinalDirection(double x, double y, double theta, NodeDto goal, RobotDirection goalDirection, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+
+        SSEAStarNode RobotNode = new()
+        {
+            Id = Guid.NewGuid(),
+            X = x,
+            Y = y,
+            Name = "RobotCurrentNode",
+        };
+        var closesNode = GetOnNode(x, y, maxDistanceToNode);
+        if (closesNode is not null)
+        {
+            if (closesNode.Id == goal.Id) return new(true, "Robot đang đứng tại điểm đích") { Data = [goal] };
+            RobotNode = GetClosesNode(closesNode, goal, theta);
+        }
+        else
+        {
+            var closesEdge = GetClosesEdge(x, y, maxDistanceToEdge, cancellationToken);
+            if (closesEdge is null)
+            {
+                return new(false, "Robot đang quá xa tuyến đường");
+            }
+
+            var startNodeForward = Nodes.FirstOrDefault(p => p.Id == closesEdge.StartNodeId);
+            var startNodeBackward = Nodes.FirstOrDefault(p => p.Id == closesEdge.EndNodeId);
+            if (startNodeForward is null || startNodeBackward is null)
+            {
+                return new(false, "Dữ liệu lỗi: điểm chặn của edge gần nhất không tồn tại");
+            }
+            if (startNodeForward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                Path.Add(startNodeBackward);
+                Path.Add(startNodeForward);
+                return new(true) { Data = Path };
+            }
+            if (startNodeBackward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                Path.Add(startNodeForward);
+                Path.Add(startNodeBackward);
+                return new(true) { Data = Path };
+            }
+            RobotNode.NegativeNodes.AddRange(GetClosesEdge(closesEdge, goal, startNodeForward, startNodeBackward, RobotNode, theta));
+        }
+
+        if (RobotNode.NegativeNodes.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+
+        var path = Find(RobotNode, goal, goalDirection, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            if (node.Id == path.First().Id)
+            {
+                Path.Add(new()
+                {
+                    Id = node.Id,
+                    Name = node.Name,
+                    X = node.X,
+                    Y = node.Y,
+                    Direction = MapCompute.GetNodeDirection(node.Direction),
+                });
+                continue;
+            }
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = Path };
+    }
+
+    public MessageResult<List<NodeDto>> PlanningWithGoalAngle(double x, double y, double theta, NodeDto goal, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+
+        SSEAStarNode RobotNode = new()
+        {
+            Id = Guid.NewGuid(),
+            X = x,
+            Y = y,
+            Name = "RobotCurrentNode",
+        };
+        var closesNode = GetOnNode(x, y, maxDistanceToNode);
+        if (closesNode is not null)
+        {
+            if (closesNode.Id == goal.Id) return new(true, "Robot đang đứng tại điểm đích") { Data = [goal] };
+            RobotNode = GetClosesNode(closesNode, goal, theta);
+        }
+        else
+        {
+            var closesEdge = GetClosesEdge(x, y, maxDistanceToEdge, cancellationToken);
+            if (closesEdge is null)
+            {
+                return new(false, "Robot đang quá xa tuyến đường");
+            }
+
+            var startNodeForward = Nodes.FirstOrDefault(p => p.Id == closesEdge.StartNodeId);
+            var startNodeBackward = Nodes.FirstOrDefault(p => p.Id == closesEdge.EndNodeId);
+            if (startNodeForward is null || startNodeBackward is null)
+            {
+                return new(false, "Dữ liệu lỗi: điểm chặn của edge gần nhất không tồn tại");
+            }
+            if (startNodeForward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                Path.Add(startNodeBackward);
+                Path.Add(startNodeForward);
+                return new(true) { Data = Path };
+            }
+            if (startNodeBackward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                Path.Add(startNodeForward);
+                Path.Add(startNodeBackward);
+                return new(true) { Data = Path };
+            }
+            RobotNode.NegativeNodes.AddRange(GetClosesEdge(closesEdge, goal, startNodeForward, startNodeBackward, RobotNode, theta));
+        }
+
+        if (RobotNode.NegativeNodes.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+
+        var path = Find(RobotNode, goal, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            if (node.Id == path.First().Id)
+            {
+                Path.Add(new()
+                {
+                    Id = node.Id,
+                    Name = node.Name,
+                    X = node.X,
+                    Y = node.Y,
+                    Direction = MapCompute.GetNodeDirection(node.Direction),
+                });
+                continue;
+            }
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = Path };
+    }
+
+    public MessageResult<NodeDto[]> PlanningWithFinalDirection(NodeDto startNode, double theta, NodeDto goal, RobotDirection goalDirection, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+        SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
+        var path = Find(RobotNode, goal, goalDirection, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = [.. Path] };
+    }
+
+    public MessageResult<NodeDto[]> PlanningWithGoalAngle(NodeDto startNode, double theta, NodeDto goal, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+        SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
+        var path = Find(RobotNode, goal, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = [.. Path] };
+    }
+}

RobotNet.RobotManager/Services/Planner/IPathPlanner.cs

@@ -0,0 +1,20 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner;
+
+public interface IPathPlanner
+{
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null);
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null);
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null);
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null);
+
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null);
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null);
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null);
+    MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null);
+    void SetData(NodeDto[] nodes, EdgeDto[] edges);
+    void SetOptions(PathPlanningOptions options);
+}

RobotNet.RobotManager/Services/Planner/IPathPlannerManager.cs

@@ -0,0 +1,8 @@
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Planner;
+
+public interface IPathPlannerManager
+{
+    IPathPlanner GetPathPlanningService(NavigationType type);
+}

RobotNet.RobotManager/Services/Planner/OmniDrive/OmniDrivePathPlanner.cs

@@ -0,0 +1,265 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.AStar;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.OmniDrive;
+
+public class OmniDrivePathPlanner : IPathPlanner
+{
+    private List<NodeDto> Nodes = [];
+    private List<EdgeDto> Edges = [];
+    private const double ReverseDirectionAngleDegree = 89;
+    private const double Ratio = 0.1;
+
+    private readonly PathPlanningOptions Options = new()
+    {
+        LimitDistanceToEdge = 1,
+        LimitDistanceToNode = 0.3,
+        ResolutionSplit = 0.1,
+    };
+
+    public void SetData(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        Nodes = [.. nodes];
+        Edges = [.. edges];
+    }
+
+    public void SetOptions(PathPlanningOptions options)
+    {
+        Options.LimitDistanceToNode = options.LimitDistanceToNode;
+        Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
+        Options.ResolutionSplit = options.ResolutionSplit;
+    }
+
+    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
+    {
+        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
+        if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
+        if (nodeplanning.Count > 2)
+        {
+            for (int i = 1; i < nodeplanning.Count - 1; i++)
+            {
+                NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
+                NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
+                var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
+                if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
+            }
+        }
+        if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
+        return RobotDirectionInNode;
+    }
+
+    public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
+
+        var RobotNearNode = new NodeDto()
+        {
+            X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
+            Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
+        };
+        var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
+        if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+        else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+    }
+
+    private EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
+    {
+        var EdgesPlanning = new List<EdgeDto>();
+        for (int i = 0; i < nodes.Length - 1; i++)
+        {
+            var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
+                                                  e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
+            if (edge is null)
+            {
+                if (i != 0) return [];
+                EdgesPlanning.Add(new EdgeDto()
+                {
+                    Id = Guid.NewGuid(),
+                    StartNodeId = nodes[i].Id,
+                    EndNodeId = nodes[i + 1].Id,
+                    DirectionAllowed = DirectionAllowed.Both,
+                    TrajectoryDegree = TrajectoryDegree.One,
+                });
+                continue;
+            }
+            bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three;
+            EdgesPlanning.Add(new()
+            {
+                Id = edge.Id,
+                StartNodeId = nodes[i].Id,
+                EndNodeId = nodes[i + 1].Id,
+                DirectionAllowed = edge.DirectionAllowed,
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+            });
+        }
+        return [.. EdgesPlanning];
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(x,
+                                              y,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]");
+        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
+        var FinalDirection = GetRobotDirectionInPath(CurrenDirection, Path.Data, [.. edgeplannings]);
+        foreach (var item in Path.Data)
+        {
+            item.Direction = MapCompute.GetNodeDirection(FinalDirection[Path.Data.IndexOf(item)]);
+        }
+
+        return new(true)
+        {
+            Data = ([.. Path.Data], [.. edgeplannings])
+        };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(startNode, goal, cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
+        var FinalDirection = GetRobotDirectionInPath(CurrenDirection, [.. Path.Data], [.. edgeplannings]);
+        for (int i = 0; i < Path.Data.Length; i++)
+        {
+            Path.Data[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
+        }
+
+        return new(true)
+        {
+            Data = ([.. Path.Data], [.. edgeplannings])
+        };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
+        if (!basicPath.IsSuccess) return basicPath;
+        if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
+        foreach (var node in basicPath.Data.Nodes)
+        {
+            node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
+        }
+
+        return basicPath;
+    }
+}

RobotNet.RobotManager/Services/Planner/PathPlannerManager.cs

@@ -0,0 +1,19 @@
+using RobotNet.RobotManager.Services.Planner.Differential;
+using RobotNet.RobotManager.Services.Planner.ForkliftV2;
+using RobotNet.RobotManager.Services.Planner.OmniDrive;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Planner;
+
+public class PathPlannerManager : IPathPlannerManager
+{
+    public IPathPlanner GetPathPlanningService(NavigationType type)
+    {
+        return type switch
+        {
+            NavigationType.Forklift => new ForkLiftPathPlannerV2(),
+            NavigationType.OmniDrive => new OmniDrivePathPlanner(),
+            _ => new DifferentialPathPlanner(),
+        };
+    }
+}

RobotNet.RobotManager/Services/Planner/PathPlanningOptions.cs

@@ -0,0 +1,8 @@
+namespace RobotNet.RobotManager.Services.Planner;
+
+public class PathPlanningOptions
+{
+    public double LimitDistanceToEdge { get; set; }
+    public double LimitDistanceToNode { get; set; }
+    public double ResolutionSplit { get; set; }
+}

RobotNet.RobotManager/Services/Planner/PriorityQueue.cs

@@ -0,0 +1,23 @@
+namespace RobotNet.RobotManager.Services.Planner;
+
+public class PriorityQueue<T>(Comparison<T> comparison)
+{
+    public List<T> Items => items;
+    private readonly List<T> items = [];
+
+    public void Enqueue(T item)
+    {
+        items.Add(item);
+        items.Sort(comparison);
+    }
+
+    public T Dequeue()
+    {
+        if (items.Count == 0) throw new InvalidOperationException("Queue is empty");
+        var item = items[0];
+        items.RemoveAt(0);
+        return item;
+    }
+
+    public int Count => items.Count;
+}

RobotNet.RobotManager/Services/Planner/Space/KDTree.cs

@@ -0,0 +1,61 @@
+using RobotNet.MapShares.Dtos;
+
+namespace RobotNet.RobotManager.Services.Planner.Space;
+
+public class KDTree(List<NodeDto> nodes)
+{
+    private readonly KDTreeNode? Root = BuildTree(nodes, 0);
+
+    private static KDTreeNode? BuildTree(List<NodeDto> nodes, int depth)
+    {
+        if (nodes.Count == 0) return null;
+        int axis = depth % 2;
+        nodes.Sort((a, b) => axis == 0 ? a.X.CompareTo(b.X) : a.Y.CompareTo(b.Y));
+        int median = nodes.Count / 2;
+
+        return new KDTreeNode
+        {
+            Node = nodes[median],
+            Axis = axis,
+            Left = BuildTree(nodes.GetRange(0, median), depth + 1),
+            Right = BuildTree(nodes.GetRange(median + 1, nodes.Count - median - 1), depth + 1)
+        };
+    }
+
+    public NodeDto? FindNearest(double x, double y, double limitDistance)
+    {
+        if (Root is null) return null;
+        var (node, dist) = Nearest(Root, x, y, null, double.MaxValue);
+        return dist <= limitDistance ? node : null;
+    }
+
+    private static (NodeDto?, double) Nearest(KDTreeNode? node, double x, double y, NodeDto? best, double bestDist)
+    {
+        if (node == null) return (best, bestDist);
+
+        double d = Math.Sqrt(Math.Pow(node.Node.X - x, 2) + Math.Pow(node.Node.Y - y, 2));
+        if (d < bestDist)
+        {
+            best = node.Node;
+            bestDist = d;
+        }
+
+        double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
+        KDTreeNode? nearSide = delta < 0 ? node.Left : node.Right;
+        KDTreeNode? farSide = delta < 0 ? node.Right : node.Left;
+
+        (best, bestDist) = Nearest(nearSide, x, y, best, bestDist);
+        if (Math.Abs(delta) < bestDist)
+            (best, bestDist) = Nearest(farSide, x, y, best, bestDist);
+
+        return (best, bestDist);
+    }
+}
+
+public class KDTreeNode
+{
+    public NodeDto Node { get; set; } = new();
+    public KDTreeNode? Left { get; set; }
+    public KDTreeNode? Right { get; set; }
+    public int Axis { get; set; } // 0 for X, 1 for Y
+}

RobotNet.RobotManager/Services/Planner/Space/MapCompute.cs

@@ -0,0 +1,86 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.Space;
+
+public class MapCompute
+{
+    public static double DistanceToCurveEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
+    {
+        double dMin = Math.Sqrt(Math.Pow(x - startNode.X, 2) + Math.Pow(y - startNode.Y, 2));
+        var length = MapEditorHelper.GetEdgeLength(new()
+        {
+            X1 = startNode.X,
+            Y1 = startNode.Y,
+            X2 = endNode.X,
+            Y2 = endNode.Y,
+            ControlPoint1X = edge.ControlPoint1X,
+            ControlPoint1Y = edge.ControlPoint1Y,
+            ControlPoint2X = edge.ControlPoint2X,
+            ControlPoint2Y = edge.ControlPoint2Y,
+            TrajectoryDegree = edge.TrajectoryDegree,
+        });
+        double step = 0.1 / (length == 0 ? 0.1 : length);
+
+        for (double t = 0; t <= 1; t += step)
+        {
+            (double curveX, double curveY) = MapEditorHelper.Curve(t, new()
+            {
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y= edge.ControlPoint2Y,
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+            });
+            double d = Math.Sqrt(Math.Pow(x - curveX, 2) + Math.Pow(y - curveY, 2));
+            if (d < dMin) dMin = d;
+        }
+
+        return dMin;
+    }
+
+    public static MessageResult<double> DistanceToEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
+    {
+        if (edge.TrajectoryDegree == TrajectoryDegree.One)
+        {
+            double time = 0;
+            var edgeLengthSquared = Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2);
+            if (edgeLengthSquared > 0)
+            {
+                time = Math.Max(0, Math.Min(1, ((x - startNode.X) * (endNode.X - startNode.X) + (y - startNode.Y) * (endNode.Y - startNode.Y)) / edgeLengthSquared));
+            }
+
+            double nearestX = startNode.X + time * (endNode.X - startNode.X);
+            double nearestY = startNode.Y + time * (endNode.Y - startNode.Y);
+
+            return new(true) { Data = Math.Sqrt(Math.Pow(x - nearestX, 2) + Math.Pow(y - nearestY, 2)) };
+        }
+        else
+        {
+            return new(true) { Data = DistanceToCurveEdge(x, y, edge, startNode, endNode) };
+        }
+    }
+
+    public static Direction GetNodeDirection(RobotDirection robotDirection) =>
+        robotDirection switch
+        {
+            RobotDirection.FORWARD => Direction.FORWARD,
+            RobotDirection.BACKWARD => Direction.BACKWARD,
+            _ => Direction.NONE
+        };
+
+    public static RobotDirection GetRobotDirection(Direction nodeDirection) =>
+        nodeDirection switch
+        {
+            Direction.FORWARD => RobotDirection.FORWARD,
+            Direction.BACKWARD => RobotDirection.BACKWARD,
+            _ => RobotDirection.NONE
+        };
+}

RobotNet.RobotManager/Services/Planner/Space/RTree.cs

@@ -0,0 +1,206 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Planner.Space;
+
+public class Rectangle(double minX, double minY, double maxX, double maxY)
+{
+    public double MinX { get; set; } = minX;
+    public double MinY { get; set; } = minY;
+    public double MaxX { get; set; } = maxX;
+    public double MaxY { get; set; } = maxY;
+
+    public double DistanceToPoint(double x, double y)
+    {
+        double dx = Math.Max(Math.Max(MinX - x, 0), x - MaxX);
+        double dy = Math.Max(Math.Max(MinY - y, 0), y - MaxY);
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    public bool Contains(double x, double y)
+    {
+        return x >= MinX && x <= MaxX && y >= MinY && y <= MaxY;
+    }
+}
+
+public class RTreeNode
+{
+    public Rectangle Bounds { get; set; } = new Rectangle(0, 0, 0, 0);
+    public List<RTreeNode> Children { get; set; } = [];
+    public List<(EdgeDto Edge, Rectangle Rect)> Entries { get; set; } = [];
+    public bool IsLeaf => Children.Count == 0;
+}
+
+public class RTree
+{
+    private readonly RTreeNode Root;
+    private const int MaxEntries = 4;
+    private readonly List<NodeDto> Nodes;
+
+    public RTree(List<NodeDto> nodes, List<EdgeDto> edges)
+    {
+        Nodes = nodes;
+        Root = new RTreeNode();
+        foreach (var edge in edges)
+        {
+            var rect = CalculateMBR(edge);
+            Insert(Root, (edge, rect));
+        }
+    }
+
+    private Rectangle CalculateMBR(EdgeDto edge)
+    {
+        var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+        var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+        if (startNode == null || endNode == null) return new Rectangle(0, 0, 0, 0);
+
+        double minX = Math.Min(startNode.X, endNode.X);
+        double maxX = Math.Max(startNode.X, endNode.X);
+        double minY = Math.Min(startNode.Y, endNode.Y);
+        double maxY = Math.Max(startNode.Y, endNode.Y);
+
+        // Mở rộng MBR nếu edge là đường cong
+        if (edge.TrajectoryDegree != TrajectoryDegree.One)
+        {
+            minX = Math.Min(minX, Math.Min(edge.ControlPoint1X, edge.ControlPoint2X));
+            maxX = Math.Max(maxX, Math.Max(edge.ControlPoint1X, edge.ControlPoint2X));
+            minY = Math.Min(minY, Math.Min(edge.ControlPoint1Y, edge.ControlPoint2Y));
+            maxY = Math.Max(maxY, Math.Max(edge.ControlPoint1Y, edge.ControlPoint2Y));
+        }
+
+        return new Rectangle(minX, minY, maxX, maxY);
+    }
+
+    private static void Insert(RTreeNode node, (EdgeDto Edge, Rectangle Rect) entry)
+    {
+        if (node.IsLeaf)
+        {
+            node.Entries.Add(entry);
+            if (node.Entries.Count > MaxEntries)
+            {
+                SplitNode(node);
+            }
+        }
+        else
+        {
+            var bestChild = ChooseBestChild(node, entry.Rect);
+            Insert(bestChild, entry);
+            AdjustBounds(bestChild);
+        }
+
+        node.Bounds.MinX = Math.Min(node.Bounds.MinX, entry.Rect.MinX);
+        node.Bounds.MinY = Math.Min(node.Bounds.MinY, entry.Rect.MinY);
+        node.Bounds.MaxX = Math.Max(node.Bounds.MaxX, entry.Rect.MaxX);
+        node.Bounds.MaxY = Math.Max(node.Bounds.MaxY, entry.Rect.MaxY);
+    }
+
+    private static RTreeNode ChooseBestChild(RTreeNode node, Rectangle rect)
+    {
+        RTreeNode best = node.Children[0];
+        double minEnlargement = CalculateEnlargement(best.Bounds, rect);
+        foreach (var child in node.Children.Skip(1))
+        {
+            double enlargement = CalculateEnlargement(child.Bounds, rect);
+            if (enlargement < minEnlargement)
+            {
+                minEnlargement = enlargement;
+                best = child;
+            }
+        }
+        return best;
+    }
+
+    private static double CalculateEnlargement(Rectangle bounds, Rectangle rect)
+    {
+        double newArea = (Math.Max(bounds.MaxX, rect.MaxX) - Math.Min(bounds.MinX, rect.MinX)) *
+                         (Math.Max(bounds.MaxY, rect.MaxY) - Math.Min(bounds.MinY, rect.MinY));
+        double oldArea = (bounds.MaxX - bounds.MinX) * (bounds.MaxY - bounds.MinY);
+        return newArea - oldArea;
+    }
+
+    private static void SplitNode(RTreeNode node)
+    {
+        var (group1, group2) = SplitEntries(node.Entries);
+        node.Children.Add(new RTreeNode { Entries = group1 });
+        node.Children.Add(new RTreeNode { Entries = group2 });
+        node.Entries.Clear();
+        foreach (var child in node.Children)
+        {
+            child.Bounds = CalculateBounds(child.Entries);
+        }
+    }
+
+    private static (List<(EdgeDto, Rectangle)>, List<(EdgeDto, Rectangle)>) SplitEntries(List<(EdgeDto Edge, Rectangle Rect)> entries)
+    {
+        entries.Sort((a, b) => a.Rect.MinX.CompareTo(b.Rect.MinX));
+        int mid = entries.Count / 2;
+        return (entries.GetRange(0, mid), entries.GetRange(mid, entries.Count - mid));
+    }
+
+    private static Rectangle CalculateBounds(List<(EdgeDto Edge, Rectangle Rect)> entries)
+    {
+        if (entries.Count == 0) return new(0, 0, 0, 0);
+        var first = entries[0].Rect;
+        double minX = first.MinX, minY = first.MinY, maxX = first.MaxX, maxY = first.MaxY;
+        foreach (var (Edge, Rect) in entries.Skip(1))
+        {
+            minX = Math.Min(minX, Rect.MinX);
+            minY = Math.Min(minY, Rect.MinY);
+            maxX = Math.Max(maxX, Rect.MaxX);
+            maxY = Math.Max(maxY, Rect.MaxY);
+        }
+        return new Rectangle(minX, minY, maxX, maxY);
+    }
+
+    private static void AdjustBounds(RTreeNode node)
+    {
+        if (node.IsLeaf)
+        {
+            node.Bounds = CalculateBounds(node.Entries);
+        }
+        else
+        {
+            node.Bounds = CalculateBounds(node.Children.SelectMany(c => c.Entries).ToList());
+        }
+    }
+
+    public EdgeDto? FindNearest(double x, double y, double limitDistance)
+    {
+        double minDistance = double.MaxValue;
+        EdgeDto? nearestEdge = null;
+        SearchNearest(Root, x, y, ref minDistance, ref nearestEdge);
+        return minDistance <= limitDistance ? nearestEdge : null;
+    }
+
+    private void SearchNearest(RTreeNode node, double x, double y, ref double minDistance, ref EdgeDto? nearestEdge)
+    {
+        if (node.IsLeaf)
+        {
+            foreach (var (edge, rect) in node.Entries)
+            {
+                var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+                var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+                if (startNode == null || endNode == null) continue;
+
+                var distanceResult = MapCompute.DistanceToEdge(x, y, edge, startNode, endNode);
+                if (distanceResult.IsSuccess && distanceResult.Data < minDistance)
+                {
+                    minDistance = distanceResult.Data;
+                    nearestEdge = edge;
+                }
+            }
+        }
+        else
+        {
+            var sortedChildren = node.Children.OrderBy(c => c.Bounds.DistanceToPoint(x, y));
+            foreach (var child in sortedChildren)
+            {
+                if (child.Bounds.DistanceToPoint(x, y) < minDistance)
+                {
+                    SearchNearest(child, x, y, ref minDistance, ref nearestEdge);
+                }
+            }
+        }
+    }
+}

RobotNet.RobotManager/Services/Robot/RobotController.cs

@@ -0,0 +1,750 @@
+using Microsoft.EntityFrameworkCore;
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+using RobotNet.RobotShares.VDA5050;
+using RobotNet.RobotShares.VDA5050.Factsheet;
+using RobotNet.RobotShares.VDA5050.FactsheetExtend;
+using RobotNet.RobotShares.VDA5050.Order;
+using RobotNet.RobotShares.VDA5050.State;
+using RobotNet.RobotShares.VDA5050.Type;
+using RobotNet.RobotShares.VDA5050.Visualization;
+using RobotNet.Shares;
+using System.Text.Json;
+using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;
+using NodePosition = RobotNet.RobotShares.VDA5050.Order.NodePosition;
+
+namespace RobotNet.RobotManager.Services.Robot;
+
+public class RobotController(string robotid,
+                             string Manufacturer,
+                             string Version,
+                             NavigationType NavigationType,
+                             IServiceProvider ServiceProvider,
+                             Func<string, string, bool> FuncPub) : IRobotController, IDisposable
+{
+    public string SerialNumber { get; } = robotid;
+    public bool IsOnline { get; set; }
+    public bool IsWorking => (RobotOrder is not null && RobotOrder.IsProcessing) || StateMsg.NodeStates.Length != 0 || StateMsg.EdgeStates.Length != 0;
+    public string State => GetState();
+    public string[] CurrentZones => RobotOrder is null ? [] : [.. RobotOrder.CurrentZones.Select(z => z.Name)];
+    public RobotOrderDto OrderState => GetOrderState();
+    public RobotActionDto[] ActionStates => GetActionsState();
+    public AutoResetEvent RobotUpdated { get; set; } = new(false);
+    public StateMsg StateMsg { get; set; } = new();
+    public VisualizationMsg VisualizationMsg { get; set; } = new();
+    public FactSheetMsg FactSheetMsg { get; set; } = new();
+    public FactsheetExtendMsg FactsheetExtendMsg { get; set; } = new();
+    public NavigationPathEdge[] BasePath => RobotOrder is null ? [] : RobotOrder.BasePath;
+    public NavigationPathEdge[] FullPath => RobotOrder is null ? [] : RobotOrder.FullPath;
+
+    private readonly LoggerController<RobotController> Logger = ServiceProvider.GetRequiredService<LoggerController<RobotController>>();
+    private readonly TimeSpan WaittingRobotFeedbackTime = TimeSpan.FromSeconds(10);
+    private RobotOrder? RobotOrder;
+
+    private CancellationTokenSource? CancelRandom;
+
+    public void Log(string message, LogLevel level = LogLevel.Information)
+    {
+        switch (level)
+        {
+            case LogLevel.Trace: Logger.Trace($"{SerialNumber} - {message}"); break;
+            case LogLevel.Error: Logger.Error($"{SerialNumber} - {message}"); break;
+            case LogLevel.Warning: Logger.Warning($"{SerialNumber} - {message}"); break;
+            case LogLevel.Critical: Logger.Critical($"{SerialNumber} - {message}"); break;
+            case LogLevel.Information: Logger.Info($"{SerialNumber} - {message}"); break;
+            case LogLevel.Debug: Logger.Debug($"{SerialNumber} - {message}"); break;
+            default: Logger.Debug($"{SerialNumber} - {message}"); break;
+        }
+    }
+
+    public async Task<MessageResult> CancelOrder()
+    {
+        try
+        {
+            if (StateMsg.NodeStates.Length != 0 || StateMsg.EdgeStates.Length != 0)
+            {
+                Action cancelOrderAction = new()
+                {
+                    ActionDescription = "Yêu cầu hủy nhiệm vụ hiện tại",
+                    ActionParameters = [new RobotShares.VDA5050.InstantAction.ActionParameter()
+                    {
+                        Key = "ORDER_ID",
+                        Value = StateMsg.OrderId,
+                    }],
+                    ActionType = ActionType.cancelOrder.ToString(),
+                    BlockingType = RobotShares.VDA5050.InstantAction.BlockingType.NONE.ToString(),
+                };
+                var pubAction = await InstantAction(cancelOrderAction, true);
+                if (!pubAction.IsSuccess) return new(false, pubAction.Message);
+            }
+            if (CancelRandom is not null && !CancelRandom.IsCancellationRequested) CancelRandom.Cancel();
+            if (RobotOrder is not null && RobotOrder.IsProcessing)
+            {
+                CancellationTokenSource cancellationToken = new();
+                cancellationToken.CancelAfter(TimeSpan.FromSeconds(10));
+                var waitCancel = await RobotOrder.Cancel(cancellationToken.Token);
+                if (!waitCancel) return new(false, "Robot không kết thúc order");
+            }
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{SerialNumber} Cancel Order logs: {ex.Message}");
+            return new(false, $"{SerialNumber} Cancel Order logs: {ex.Message}");
+        }
+    }
+
+    public async Task<MessageResult> CancelAction()
+    {
+        try
+        {
+            Action cancelOrderAction = new()
+            {
+                ActionDescription = "Yêu cầu hủy Action hiện tại",
+                ActionParameters = [new RobotShares.VDA5050.InstantAction.ActionParameter()
+                    {
+                        Key = "ORDER_ID",
+                        Value = SerialNumber,
+                    }],
+                ActionType = ActionType.cancelOrder.ToString(),
+                BlockingType = RobotShares.VDA5050.InstantAction.BlockingType.NONE.ToString(),
+            };
+            var pubAction = await InstantAction(cancelOrderAction, true);
+            return new(pubAction.IsSuccess, pubAction.Message);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{SerialNumber} Cancel Action logs: {ex.Message}");
+            return new(false, $"{SerialNumber} Cancel Action logs: {ex.Message}");
+        }
+    }
+
+    public void Initialize(double x, double y, double theta)
+    {
+        throw new NotImplementedException();
+    }
+
+    public async Task<MessageResult<string>> InstantAction(Action action, bool waittingFisished)
+    {
+        CancellationTokenSource CancellationToken = new();
+        CancellationToken.CancelAfter(WaittingRobotFeedbackTime);
+        bool IsFeedback = false;
+        int RepeatCounter = 0;
+        try
+        {
+            action.ActionId = Guid.NewGuid().ToString();
+            var instantActionsMsg = new RobotNet.RobotShares.VDA5050.InstantAction.InstantActionsMsg()
+            {
+                HeaderId = 1,
+                Manufacturer = Manufacturer,
+                Version = Version,
+                Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                SerialNumber = SerialNumber,
+                Actions = [action],
+            };
+            var pubInstantAction = FuncPub.Invoke(VDA5050Topic.InstantActions, JsonSerializer.Serialize(instantActionsMsg, JsonOptionExtends.Write));
+            if (!pubInstantAction)
+            {
+                string msg = $"Gửi Action xuống cho robot không thành công: {action.ActionType}";
+                Log(msg, LogLevel.Warning);
+                return new(false, msg);
+            }
+            while (!IsFeedback)
+            {
+                if (StateMsg is not null && StateMsg.ActionStates is not null)
+                {
+                    var actionstate = StateMsg.ActionStates.FirstOrDefault(ac => ac.ActionId == action.ActionId);
+                    if (actionstate is not null)
+                    {
+                        IsFeedback = true;
+                        break;
+                    }
+                }
+                if(CancellationToken.IsCancellationRequested)
+                {
+                    if (RepeatCounter++ == 0)
+                    {
+                        await Task.Delay(1000);
+                        CancellationToken = new();
+                        CancellationToken.CancelAfter(WaittingRobotFeedbackTime);
+                        pubInstantAction = FuncPub.Invoke(VDA5050Topic.InstantActions, JsonSerializer.Serialize(instantActionsMsg, JsonOptionExtends.Write));
+                        if (!pubInstantAction)
+                        {
+                            string msg = $"Gửi Action xuống cho robot không thành công: {action.ActionType}";
+                            Log(msg, LogLevel.Warning);
+                            return new(false, msg);
+                        }
+                    }
+                    else return new(false, $"Robot không đồng ý thực hiện action: {action.ActionType}");
+                }
+                await Task.Delay(500);
+            }
+            if (waittingFisished)
+            {
+                while (!CancellationToken.IsCancellationRequested)
+                {
+                    if (StateMsg is not null)
+                    {
+                        if (StateMsg.ActionStates is not null)
+                        {
+                            var actionCancelOrder = StateMsg.ActionStates.FirstOrDefault(a => a.ActionId == action.ActionId);
+                            if (actionCancelOrder is not null)
+                            {
+                                if (ActionFinish(actionCancelOrder.ActionStatus)) return new(true);
+                                if (ActionFailed(actionCancelOrder.ActionStatus))
+                                {
+                                    string msg = $"Robot trả về thực hiện action lỗi: {action.ActionType}";
+                                    Log(msg, LogLevel.Warning);
+                                    return new(false, msg);
+                                }
+                            }
+                        }
+                        if (GetErrorLevel() != ErrorLevel.NONE)
+                        {
+                            var errorStr = StateMsg.Errors.Select(error => error.ErrorType).ToArray();
+                            string msg = $"Robot xảy ra lỗi sau khi gọi thực hiện action {action.ActionType}";
+                            Log($"{msg} - {string.Join(",", errorStr)}", LogLevel.Warning);
+                            return new(false, msg);
+                        }
+                    }
+                    if (CancellationToken.IsCancellationRequested) return new(false, "Robot đang thực hiện Action nhưng đã quá thời gian timeout 10s");
+                    await Task.Delay(500);
+                }
+            }
+            if (CancellationToken.IsCancellationRequested) throw new Exception();
+            return new(true) { Data = action.ActionId };
+        }
+        catch (Exception ex)
+        {
+            string msg = $"Có lỗi xảy ra trong quá trình gửi action : {ex.Message}";
+            Log(msg, LogLevel.Warning);
+            return new(false, msg);
+        }
+    }
+
+    public MessageResult MoveStraight(double x, double y)
+    {
+        return new(false, "Chức năng này hiện không khả dụng");
+    }
+
+    public async Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<RobotShares.VDA5050.InstantAction.Action>>? actions = null, double? lastAngle = null)
+    {
+        try
+        {
+            if (IsWorking) return new(false, "Robot đang thực hiện nhiệm vụ");
+
+            using var scope = ServiceProvider.CreateScope();
+            var PathPlanner = scope.ServiceProvider.GetRequiredService<PathPlanner>();
+            var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            var MapManager = scope.ServiceProvider.GetRequiredService<MapManager>();
+            var TrafficManager = scope.ServiceProvider.GetRequiredService<TrafficManager>();
+            var TrafficACS = scope.ServiceProvider.GetRequiredService<TrafficACS>();
+            var orderLogger = scope.ServiceProvider.GetRequiredService<LoggerController<RobotOrder>>();
+
+            var robot = await robotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == SerialNumber) ??
+                        throw new Exception($"Không tìm thấy robot {SerialNumber} trong kho dữ liệu");
+            var robotModel = await robotDb.RobotModels.FirstOrDefaultAsync(r => r.Id == robot.ModelId) ??
+                        throw new Exception($"Không tìm thấy robot model {robot.ModelId} trong kho dữ liệu");
+
+            var path = await PathPlanner.Planning(VisualizationMsg.AgvPosition.X, VisualizationMsg.AgvPosition.Y, VisualizationMsg.AgvPosition.Theta, NavigationType, robot.MapId, goalName);
+            if (!path.IsSuccess) return new(false, path.Message);
+            if(path.IsSuccess && path.Data.Nodes.Length < 2)
+            {
+                RobotOrder?.CreateComledted();
+                return new(true, "");
+            }
+            if (path.Data.Nodes is null || path.Data.Edges is null || path.Data.Edges.Length == 0 || path.Data.Nodes.Length < 2)
+                return new(false, $"Đường dẫn tới đích {goalName} từ [{VisualizationMsg.AgvPosition.X} - {VisualizationMsg.AgvPosition.Y} - {VisualizationMsg.AgvPosition.Theta}] không tồn tại");
+
+            if (lastAngle != null)
+            {
+                path.Data.Nodes[^1].Theta = lastAngle.Value;
+            }
+
+            var mapData = await MapManager.GetMapData(robot.MapId);
+            if (!mapData.IsSuccess) return new(false, mapData.Message);
+            if (mapData is null || mapData.Data is null) return new(false, "Không thể tìm thấy bản đồ chứa robot.");
+            var nodeInZones = await PathPlanner.GetZones(robot.MapId, path.Data.Nodes);
+            if (!nodeInZones.IsSuccess) return new(false, mapData.Message);
+            var order = await Order([.. path.Data.Nodes], [.. path.Data.Edges], mapData.Data, actions, TrafficManager.Enable);
+            if (order.IsSuccess && order.Data is not null)
+            {
+                int counter = 0;
+                while (counter++ < 2)
+                {
+                    var createAgent = TrafficManager.CreateAgent(robot.MapId, this, new()
+                    {
+
+                        NavigationType = robotModel.NavigationType,
+                        Length = robotModel.Length,
+                        Width = robotModel.Width,
+                        NavigationPointX = robotModel.OriginX,
+                        NavigationPointY = robotModel.OriginY,
+                    },
+                    [..path.Data.Nodes.Select(n => new TrafficNodeDto()
+                    {
+                        Id = n.Id,
+                        Name = n.Name,
+                        X = n.X,
+                        Y = n.Y,
+                        Direction = MapCompute.GetRobotDirection(n.Direction),
+                    })], [..path.Data.Edges.Select(n => new TrafficEdgeDto()
+                    {
+                        Id = n.Id,
+                        StartNodeId = n.StartNodeId,
+                        EndNodeId = n.EndNodeId,
+                        TrajectoryDegree = n.TrajectoryDegree,
+                        ControlPoint1X = n.ControlPoint1X,
+                        ControlPoint1Y = n.ControlPoint1Y,
+                        ControlPoint2X = n.ControlPoint2X,
+                        ControlPoint2Y = n.ControlPoint2Y,
+                    })]);
+                    if (createAgent.IsSuccess) break;
+                    Logger.Warning($"{SerialNumber} - Không thể tạo traffic agent: {createAgent.Message}");
+                    if (counter > 1 && !createAgent.IsSuccess)
+                    {
+                        var cancel = await CancelOrder();
+                        if (!cancel.IsSuccess) Logger.Warning($"{SerialNumber} - Không thể hủy bỏ nhiệm vụ đã giao: {cancel.Message}");
+                        return new(false, $"Không thể tạo traffic agent: {createAgent.Message} - {(cancel.IsSuccess ? "Đã hủy order" : $"Không thể hủy order: {cancel.Message}")} ");
+                    }
+                }
+                RobotOrder = new RobotOrder([.. path.Data.Nodes], [.. path.Data.Edges], nodeInZones.Data ?? [], actions ?? new Dictionary<string, IEnumerable<Action>>(),
+                                            order.Data, this, NavigationType, TrafficManager, TrafficACS, MapManager, orderLogger, Order)
+                {
+                    CurrentZones = RobotOrder is null ? [] : RobotOrder.CurrentZones,
+                };
+
+                return new(true);
+            }
+            else return new(order.IsSuccess, order.Message);
+        }
+        catch (Exception ex)
+        {
+            string msg = $"Có lỗi xảy ra trong quá trình gửi order to Node {goalName} : {ex.Message}";
+            Log(msg, LogLevel.Warning);
+            return new(false, msg);
+        }
+    }
+
+    public MessageResult Rotate(double angle)
+    {
+        return new(false, "Chức năng này hiện không khả dụng");
+    }
+
+    private async Task<MessageResult> Order(OrderMsg orderMsg)
+    {
+        CancellationTokenSource CancellationExitToken = new();
+        CancellationExitToken.CancelAfter(WaittingRobotFeedbackTime);
+        try
+        {
+            var pubOrder = FuncPub.Invoke(VDA5050Topic.Order, JsonSerializer.Serialize(orderMsg, JsonOptionExtends.Write));
+            if (!pubOrder)
+            {
+                string msg = $"Gửi Order xuống cho robot không thành công: {orderMsg.OrderId}";
+                Log(msg, LogLevel.Warning);
+                return new(false, msg);
+            }
+            while (!CancellationExitToken.IsCancellationRequested)
+            {
+                if (StateMsg is not null)
+                {
+                    if (StateMsg.OrderId == orderMsg.OrderId && IsWorking && StateMsg.OrderUpdateId == orderMsg.OrderUpdateId) break;
+                }
+
+                await Task.Delay(500, CancellationExitToken.Token);
+            }
+            if (CancellationExitToken.IsCancellationRequested) throw new Exception();
+            return new(true);
+        }
+        catch (Exception ex)
+        {
+            string msg = $"Có lỗi xảy ra trong quá trình gửi order : {ex.Message}";
+            if (CancellationExitToken.IsCancellationRequested) msg = $"{SerialNumber} - Robot không đồng ý thực hiện order: {orderMsg.OrderId}";
+
+            Log(msg, LogLevel.Warning);
+            return new(false, msg);
+        }
+    }
+
+    private static List<Action> ConvertAction(string actions, ActionDto[] mapAction)
+    {
+        List<Action> Actions = [];
+        var ActionIds = !string.IsNullOrEmpty(actions) ? JsonSerializer.Deserialize<Guid[]>(actions) : [];
+        if (ActionIds is not null)
+        {
+            foreach (var actionid in ActionIds)
+            {
+                var actionDb = mapAction.FirstOrDefault(x => x.Id == actionid);
+                if (actionDb is not null)
+                {
+                    var vdaAction = JsonSerializer.Deserialize<Action>(actionDb.Content, JsonOptionExtends.Read);
+                    if (vdaAction is not null) Actions.Add(vdaAction);
+                }
+            }
+        }
+        return Actions;
+    }
+
+    private static Edge? ConvertToOrderEdge(EdgeDto edge, MapDataDto map)
+    {
+        var startNode = map.Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
+        var endNode = map.Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
+        var dataEdge = map.Edges.FirstOrDefault(e => e.Id == edge.Id);
+
+        if (startNode is null || endNode is null || dataEdge is null) return null;
+
+        List<ControlPoint> ControlPoints = [];
+        ControlPoints.Add(new()
+        {
+            X = startNode.X,
+            Y = startNode.Y,
+            Weight = 1,
+        });
+        if (dataEdge.TrajectoryDegree == TrajectoryDegree.Two)
+        {
+            ControlPoints.Add(new()
+            {
+                X = dataEdge.ControlPoint1X,
+                Y = dataEdge.ControlPoint1Y,
+                Weight = 1,
+            });
+        }
+        else if (dataEdge.TrajectoryDegree == TrajectoryDegree.Three)
+        {
+            ControlPoint controlPoint1 = new();
+            ControlPoint controlPoint2 = new();
+            if (dataEdge.StartNodeId == edge.StartNodeId)
+            {
+                controlPoint1.X = dataEdge.ControlPoint1X;
+                controlPoint1.Y = dataEdge.ControlPoint1Y;
+                controlPoint1.Weight = 1;
+                controlPoint2.X = dataEdge.ControlPoint2X;
+                controlPoint2.Y = dataEdge.ControlPoint2Y;
+                controlPoint2.Weight = 1;
+            }
+            else
+            {
+                controlPoint2.X = dataEdge.ControlPoint1X;
+                controlPoint2.Y = dataEdge.ControlPoint1Y;
+                controlPoint2.Weight = 1;
+                controlPoint1.X = dataEdge.ControlPoint2X;
+                controlPoint1.Y = dataEdge.ControlPoint2Y;
+                controlPoint1.Weight = 1;
+            }
+            ControlPoints.Add(controlPoint1);
+            ControlPoints.Add(controlPoint2);
+        }
+        ControlPoints.Add(new()
+        {
+            X = endNode.X,
+            Y = endNode.Y,
+            Weight = 1,
+        });
+
+        var result = new Edge()
+        {
+            EdgeId = edge.Id.ToString(),
+            EdgeDescription = $"{startNode.Name} - {endNode.Name}",
+            EndNodeId = edge.EndNodeId.ToString(),
+            StartNodeId = edge.StartNodeId.ToString(),
+            SequenceId = 1,
+            Direction = dataEdge.DirectionAllowed.ToString(),
+            Orientation = 0,
+            Released = true,
+            Length = MapEditorHelper.GetEdgeLength(new()
+            {
+                X1 = startNode.X,
+                X2 = endNode.X,
+                Y1 = startNode.Y,
+                Y2 = endNode.Y,
+                TrajectoryDegree = dataEdge.TrajectoryDegree,
+                ControlPoint1X = dataEdge.ControlPoint1X,
+                ControlPoint1Y = dataEdge.ControlPoint1Y,
+                ControlPoint2X = dataEdge.ControlPoint2X,
+                ControlPoint2Y = dataEdge.ControlPoint2Y,
+            }),
+            MaxHeight = dataEdge.MaxHeight,
+            MaxRotationSpeed = dataEdge.MaxRotationSpeed,
+            OrientationType = "",
+            MaxSpeed = dataEdge.MaxSpeed,
+            MinHeight = dataEdge.MinHeight,
+            RotationAllowed = dataEdge.RotationAllowed,
+            Actions = [.. ConvertAction(dataEdge.Actions, [.. map.Actions])],
+            Trajectory = new()
+            {
+                Degree = dataEdge.TrajectoryDegree == TrajectoryDegree.One ? 1 : dataEdge.TrajectoryDegree == TrajectoryDegree.Two ? 2 : 3,
+                KnotVector = dataEdge.TrajectoryDegree == TrajectoryDegree.One ? [0, 0, 1, 1] : dataEdge.TrajectoryDegree == TrajectoryDegree.Two ? [0, 0, 0, 1, 1, 1] : [0, 0, 0, 0, 1, 1, 1, 1],
+                ControlPoints = [.. ControlPoints],
+            },
+        };
+        return result;
+    }
+
+    public static void CreateOrderMsg(ref OrderMsg orderMsgTemplate, NodeDto[] nodes, EdgeDto[] edges, MapDataDto map, IDictionary<string, IEnumerable<Action>> actions, NavigationType navigationType, bool trafficEnable)
+    {
+        if (nodes.Length <= 1 || edges.Length < 1) throw new ArgumentException("Dữ liệu đường đi không hợp lệ");
+        List<Node> OrderNodes = [];
+        List<Edge> OrderEdges = [];
+        for (int i = 0; i < nodes.Length; i++)
+        {
+            var nodeDb = map.Nodes.FirstOrDefault(n => n.Id == nodes[i].Id);
+            if (nodeDb is null)
+            {
+                if (i != 0) throw new ArgumentException("Đường đi tồn tại node không hợp lệ");
+
+                var angleForward = Math.Atan2(nodes[1].Y - nodes[0].Y, nodes[1].X - nodes[0].X);
+                var angleBackward = Math.Atan2(nodes[0].Y - nodes[1].Y, nodes[0].X - nodes[1].X);
+                OrderNodes.Add(new()
+                {
+                    NodeId = nodes[i].Id.ToString(),
+                    Released = true,
+                    SequenceId = i,
+                    NodeDescription = nodes[i].Name ?? "",
+                    NodePosition = new NodePosition()
+                    {
+                        X = nodes[i].X,
+                        Y = nodes[i].Y,
+                        Theta = navigationType == NavigationType.OmniDrive ? 10 : nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward,
+                        MapDescription = map.Name,
+                        MapId = map.Id.ToString(),
+                        AllowedDeviationTheta = nodes[i].AllowedDeviationTheta,
+                        AllowedDeviationXY = nodes[i].AllowedDeviationXy,
+                    },
+                    Actions = actions.TryGetValue(nodes[i].Name, out IEnumerable<Action>? anoCustomActions) ? [.. anoCustomActions] : [],
+                });
+                continue;
+            }
+
+            double angle;
+            if (i == 0)
+            {
+                var nearNode = MapEditorHelper.GetNearByNode(nodes[0], nodes[1], edges[0], 0.01);
+                var angleForward = Math.Atan2(nearNode.Y - nodes[0].Y, nearNode.X - nodes[0].X);
+                var angleBackward = Math.Atan2(nodes[0].Y - nearNode.Y, nodes[0].X - nearNode.X);
+                angle = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
+            }
+            else
+            {
+                var nearNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i - 1], edges[i - 1], 0.01);
+                var angleForward = Math.Atan2(nodeDb.Y - nearNode.Y, nodeDb.X - nearNode.X);
+                var angleBackward = Math.Atan2(nearNode.Y - nodeDb.Y, nearNode.X - nodeDb.X);
+
+                if (nodes[i].Direction == nodes[i - 1].Direction) angle = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
+                else angle = nodes[i - 1].Direction == Direction.FORWARD ? angleForward : angleBackward;
+            }
+
+            List<Action> nodeActions = ConvertAction(nodeDb.Actions, [.. map.Actions]);
+            if (actions.TryGetValue(nodes[i].Name, out IEnumerable<Action>? customActions) && customActions is not null)
+            {
+                nodeActions.AddRange(customActions);
+            }
+            OrderNodes.Add(new()
+            {
+                NodeId = nodes[i].Id.ToString(),
+                Released = i == 0 || !trafficEnable,
+                SequenceId = i,
+                NodeDescription = nodes[i].Name ?? "",
+                NodePosition = new NodePosition()
+                {
+                    X = nodeDb.X,
+                    Y = nodeDb.Y,
+                    Theta = i == (nodes.Length - 1) ? (nodeDb.Theta * Math.PI / 180) : (navigationType == NavigationType.OmniDrive ? 10 : angle),
+                    MapDescription = map.Name,
+                    MapId = map.Id.ToString(),
+                    AllowedDeviationTheta = nodeDb.AllowedDeviationTheta,
+                    AllowedDeviationXY = nodeDb.AllowedDeviationXy,
+                },
+                Actions = [.. nodeActions],
+            });
+        }
+        foreach (var edge in edges)
+        {
+            var orderEdge = ConvertToOrderEdge(edge, map);
+            if (orderEdge is null)
+            {
+                if (edge == edges.First())
+                {
+                    orderEdge = new Edge()
+                    {
+                        EdgeId = edge.Id.ToString(),
+                        StartNodeId = edge.StartNodeId.ToString(),
+                        EndNodeId = edge.EndNodeId.ToString(),
+                        EdgeDescription = $"{edge.StartNodeId} - {edge.EndNodeId}",
+                        Direction = edge.DirectionAllowed.ToString(),
+                        Orientation = 0,
+                        Released = false,
+                        Length = 0,
+                        MaxRotationSpeed = 0.5,
+                        OrientationType = "",
+                        MaxSpeed = 0.5,
+                        MinHeight = 0,
+                        RotationAllowed = true,
+                        Actions = [.. ConvertAction(edge.Actions, [.. map.Actions])],
+                        Trajectory = new()
+                        {
+                            Degree = 1,
+                            KnotVector = [0, 0, 1, 1],
+                            ControlPoints = [new()
+                                {
+                                    X = nodes[0].X,
+                                    Y = nodes[0].Y,
+                                    Weight = 1,
+                                },
+                                new()
+                                {
+                                    X = nodes[1].X,
+                                    Y = nodes[1].Y,
+                                    Weight = 1,
+                                }],
+                        },
+                    };
+                }
+                else throw new ArgumentException("Đường đi tồn tại edge không hợp lệ");
+            }
+            orderEdge.SequenceId = Array.IndexOf(edges, edge);
+            orderEdge.Released = !trafficEnable;
+            OrderEdges.Add(orderEdge);
+        }
+        orderMsgTemplate.Nodes = [.. OrderNodes];
+        orderMsgTemplate.Edges = [.. OrderEdges];
+    }
+
+    private async Task<MessageResult<OrderMsg?>> Order(List<NodeDto> nodes, List<EdgeDto> edges, MapDataDto map, IDictionary<string, IEnumerable<Action>>? actions, bool trafficManager)
+    {
+        try
+        {
+            var OrderMsg = new OrderMsg()
+            {
+                HeaderId = 1,
+                Manufacturer = Manufacturer,
+                Version = Version,
+                Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+                SerialNumber = SerialNumber,
+                OrderId = Guid.NewGuid().ToString(),
+                OrderUpdateId = 1,
+                ZoneSetId = map.Name ?? "",
+            };
+            CreateOrderMsg(ref OrderMsg, [.. nodes], [.. edges], map, actions ?? new Dictionary<string, IEnumerable<Action>>(), NavigationType, trafficManager);
+            var pubOrder = await Order(OrderMsg);
+            if (pubOrder.IsSuccess) return new(true) { Data = OrderMsg };
+            return new(false, pubOrder.Message);
+        }
+        catch (Exception ex)
+        {
+            string msg = "Có lỗi xảy ra trong quá trình tạo Order";
+            Log($"{msg}: {ex.Message}", LogLevel.Warning);
+            return new(true, msg);
+        }
+    }
+
+    private string GetState()
+    {
+        if (StateMsg.Information is not null)
+        {
+            var RobotGeneral = StateMsg.Information.FirstOrDefault(info => info.InfoType == InformationType.robot_general.ToString());
+            if (RobotGeneral is not null)
+            {
+                var references = RobotGeneral.InfoReferences.FirstOrDefault(key => key.ReferenceKey == InformationReferencesKey.robot_state.ToString());
+                if (references is not null) return references.ReferenceValue;
+            }
+        }
+        return "";
+    }
+
+    private RobotOrderDto GetOrderState()
+    {
+        return new()
+        {
+            OrderId = StateMsg.OrderId,
+            IsCompleted = (RobotOrder is not null && RobotOrder.IsCompleted) || RobotOrder is null,
+            IsError = RobotOrder is not null && RobotOrder.IsError,
+            IsProcessing = RobotOrder is not null && RobotOrder.IsProcessing,
+            IsCanceled = RobotOrder is not null && RobotOrder.IsCanceled,
+            Errors = RobotOrder is not null ? RobotOrder.Errors : [],
+        };
+    }
+
+    private RobotActionDto[] GetActionsState()
+    {
+        List<RobotActionDto> Actions = [];
+        foreach (var action in StateMsg.ActionStates)
+        {
+            Actions.Add(new RobotActionDto()
+            {
+                ActionId = action.ActionId,
+                Action = action,
+                IsError = ActionFailed(action.ActionStatus),
+                IsCompleted = ActionFinish(action.ActionStatus),
+                IsProcessing = ActionRunning(action.ActionStatus),
+                Errors = [.. GetErrorString()],
+            });
+        }
+        return [.. Actions];
+    }
+
+    private ErrorLevel GetErrorLevel()
+    {
+        if (StateMsg.Errors is not null)
+        {
+            if (StateMsg.Errors.Any(error => error.ErrorLevel == ErrorLevel.FATAL.ToString())) return ErrorLevel.FATAL;
+            if (StateMsg.Errors.Any(error => error.ErrorLevel == ErrorLevel.WARNING.ToString())) return ErrorLevel.WARNING;
+        }
+        return ErrorLevel.NONE;
+    }
+
+    private static bool ActionFinish(string status) => status == ActionStatus.FINISHED.ToString();
+
+    private static bool ActionFailed(string status) => status == ActionStatus.FAILED.ToString();
+
+    private static bool ActionRunning(string status) => status == ActionStatus.RUNNING.ToString();
+
+    private IEnumerable<string> GetErrorString()
+    {
+        if (StateMsg.Errors is not null && StateMsg.Errors.Length > 0)
+        {
+            foreach (var error in StateMsg.Errors)
+            {
+                yield return $"{error.ErrorType} - {error.ErrorDescription}";
+            }
+        }
+        yield break;
+    }
+
+    public Task<MessageResult> MoveRandom(List<string> nodes)
+    {
+        try
+        {
+            CancelRandom = new CancellationTokenSource();
+            var random = new Random();
+            var randomTask = Task.Run(async () =>
+            {
+                while (!CancelRandom.IsCancellationRequested)
+                {
+                    var index = random.Next(nodes.Count);
+                    await MoveToNode(nodes[index]);
+                    while (!CancelRandom.IsCancellationRequested)
+                    {
+                        if (!IsWorking) break;
+                        await Task.Delay(1000);
+                    }
+                    await Task.Delay(2000);
+                }
+            }, cancellationToken: CancelRandom.Token);
+        }
+        catch { }
+        return Task.FromResult<MessageResult>(new(true));
+    }
+
+    public void Dispose()
+    {
+        if (IsOnline) IsOnline = false;
+        GC.SuppressFinalize(this);
+    }
+}

RobotNet.RobotManager/Services/Robot/RobotOrder.cs

@@ -0,0 +1,593 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+using RobotNet.RobotShares.VDA5050.Order;
+using RobotNet.RobotShares.VDA5050.State;
+using RobotNet.Shares;
+using SixLabors.ImageSharp;
+using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;
+
+namespace RobotNet.RobotManager.Services.Robot;
+
+public class RobotOrder : IRobotOrder, IDisposable
+{
+    public bool IsError { get; private set; }
+    public bool IsCompleted { get; private set; }
+    public bool IsProcessing => !IsDisposed;
+    public bool IsCanceled { get; private set; }
+    public string[] Errors => [.. GetError()];
+    public Guid MapId { get; private set; }
+    public TrafficSolutionState TrafficSolutionState { get; private set; } = TrafficSolutionState.None;
+    public NavigationPathEdge[] FullPath => GetFullPath();
+    public NavigationPathEdge[] BasePath => GetBasePath();
+    public List<ZoneDto> CurrentZones { get; set; } = [];
+
+    private const int intervalTime = 50;
+    private readonly WatchTimerAsync<RobotOrder> Timer;
+    private int TimerCounter = 0;
+
+    private OrderMsg OrderMsg;
+    private readonly LoggerController<RobotOrder> Logger;
+    private readonly TrafficManager TrafficManager;
+    private readonly TrafficACS TrafficACS;
+    private readonly MapManager MapManager;
+    private readonly Func<OrderMsg, Task<MessageResult>> PubOrder;
+    private readonly List<NodeDto> Nodes;
+    private readonly List<EdgeDto> Edges;
+    private readonly Dictionary<Guid, ZoneDto[]> Zones;
+    private readonly IDictionary<string, IEnumerable<Action>> Actions;
+    private readonly IRobotController RobotController;
+    private readonly NavigationType NavigationType;
+    private Guid TrafficManagerGoalId = Guid.Empty;
+    private Guid TrafficACSGoalId = Guid.Empty;
+
+    private bool IsDisposed = false;
+    private bool IsWaittingCancel = false;
+    private Guid CurrentBaseId = Guid.Empty;
+    private Guid LastNodeId = Guid.Empty;
+
+    public RobotOrder(NodeDto[] nodes,
+                      EdgeDto[] edges,
+                      Dictionary<Guid, ZoneDto[]> zones,
+                      IDictionary<string, IEnumerable<Action>> actions,
+                      OrderMsg orderMsg,
+                      IRobotController robotController,
+                      NavigationType navigationType,
+                      TrafficManager traffiManager,
+                      TrafficACS trafficACS,
+                      MapManager mapManager,
+                      LoggerController<RobotOrder> logger,
+                      Func<OrderMsg, Task<MessageResult>> pubOrder)
+    {
+        if (nodes.Length < 2)
+        {
+            IsError = true;
+            IsDisposed = true;
+            throw new ArgumentException("Đường dẫn không hợp lệ. Số lượng nodes nhỏ hơn 2");
+        }
+        if (nodes.Length < 1)
+        {
+            IsError = true;
+            IsDisposed = true;
+            throw new ArgumentException("Đường dẫn không hợp lệ. Số lượng edges nhỏ hơn 1");
+        }
+        Nodes = [.. nodes];
+        Edges = [.. edges];
+        Zones = zones ?? [];
+        MapId = Nodes[1].MapId;
+        Actions = actions;
+        OrderMsg = orderMsg;
+        RobotController = robotController;
+        NavigationType = navigationType;
+        TrafficManager = traffiManager;
+        TrafficACS = trafficACS;
+        MapManager = mapManager;
+        Logger = logger;
+        PubOrder = pubOrder;
+        Timer = new(intervalTime, TimerHandler, logger);
+        Timer.Start();
+    }
+
+    private async Task<bool> PublishOrder(Guid goalId)
+    {
+        OrderMsg.OrderUpdateId++;
+        OrderMsg.HeaderId++;
+        OrderMsg.Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
+        var pubOrder = await PubOrder.Invoke(OrderMsg);
+        if (!pubOrder.IsSuccess)
+        {
+            OrderMsg.OrderUpdateId--;
+            OrderMsg.HeaderId--;
+            return false;
+        }
+        CurrentBaseId = goalId;
+        return true;
+    }
+
+    private void UpdatePath(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        try
+        {
+            var map = Task.Run(async () => await MapManager.GetMapData(MapId));
+            map.Wait();
+            if (map.Result is null) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: không thể lấy dữ liệu bản đồ {MapId}");
+            else if (!map.Result.IsSuccess) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: {map.Result.Message}");
+            else if (map.Result.Data is not null)
+            {
+                Robot.RobotController.CreateOrderMsg(ref OrderMsg, nodes, edges, map.Result.Data, Actions, NavigationType, TrafficManager.Enable);
+                Nodes.Clear();
+                Nodes.AddRange(nodes);
+                Edges.Clear();
+                Edges.AddRange(edges);
+            }
+            else Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: dữ liệu bản đồ {MapId} rỗng");
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: {ex}");
+        }
+    }
+
+    private bool UpdateGoal(Guid goalId)
+    {
+        var currentNodeOrderIndex = Array.FindIndex(OrderMsg.Nodes, n => n.NodeId == LastNodeId.ToString());
+        if (currentNodeOrderIndex != -1)
+        {
+            OrderMsg.Nodes = [.. OrderMsg.Nodes.Skip(currentNodeOrderIndex)];
+            OrderMsg.Edges = [.. OrderMsg.Edges.Skip(currentNodeOrderIndex)];
+        }
+
+        var goalOrder = OrderMsg.Nodes.FirstOrDefault(n => n.NodeId == goalId.ToString());
+        if (goalOrder is not null && CurrentBaseId.ToString() != goalOrder.NodeId)
+        {
+            for (int i = 0; i <= Array.IndexOf(OrderMsg.Nodes, goalOrder); i++)
+            {
+                OrderMsg.Nodes[i].Released = true;
+            }
+            for (int i = 0; i < Array.IndexOf(OrderMsg.Nodes, goalOrder); i++)
+            {
+                OrderMsg.Edges[i].Released = true;
+            }
+            return true;
+        }
+        return false;
+    }
+
+    private void HandleGiveWayState(TrafficSolution solution)
+    {
+        if (solution.GivewayNodes[^1].Id != solution.ReleaseNode.Id) return;
+        if (solution.GivewayEdges.Count == 0) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí tránh đường lỗi: Không có edge mới nhận được");
+        else if (solution.GivewayNodes.Count <= 1) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí tránh đường lỗi: nodes mới nhận được có số lượng {solution.Nodes.Length}");
+        else if (CurrentBaseId != solution.ReleaseNode.Id)
+        {
+            List<NodeDto> nodes = [..solution.GivewayNodes.Select(n => new NodeDto()
+            {
+                Id = n.Id,
+                Name = n.Name,
+                X = n.X,
+                Y = n.Y,
+                Direction = MapCompute.GetNodeDirection(n.Direction),
+            })];
+            List<EdgeDto> edges = [..solution.GivewayEdges.Select(e => new EdgeDto()
+            {
+                Id = e.Id,
+                ControlPoint1X = e.ControlPoint1X,
+                ControlPoint2X = e.ControlPoint2X,
+                ControlPoint1Y = e.ControlPoint1Y,
+                ControlPoint2Y = e.ControlPoint2Y,
+                TrajectoryDegree = e.TrajectoryDegree,
+                StartNodeId = e.StartNodeId,
+                EndNodeId = e.EndNodeId,
+            })];
+            nodes.Add(nodes[^2]);
+            edges.Add(new()
+            {
+                Id = edges[^1].Id,
+                StartNodeId = nodes[^2].Id,
+                EndNodeId = nodes[^1].Id,
+                ControlPoint1X = edges[^1].ControlPoint2X,
+                ControlPoint1Y = edges[^1].ControlPoint2Y,
+                ControlPoint2X = edges[^1].ControlPoint1X,
+                ControlPoint2Y = edges[^1].ControlPoint1Y,
+                TrajectoryDegree = edges[^1].TrajectoryDegree,
+            });
+
+            UpdatePath([.. nodes], [.. edges]);
+        }
+    }
+
+    private void HandleRefreshPath(TrafficSolution solution)
+    {
+        if (solution.Edges.Length == 0) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí làm mới đường lỗi: Không có edge mới nhận được");
+        else if (solution.Nodes.Length <= 1) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí làm mới đường lỗi: nodes mới nhận được có số lượng {solution.Nodes.Length}");
+        else if (solution.Nodes[^1].Id != Nodes[^1].Id) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí làm mới đường lỗi: tuyến đường mới không kết thúc tại đích cũ");
+        else
+        {
+            List<NodeDto> nodes = [..solution.Nodes.Select(n => new NodeDto()
+            {
+                Id = n.Id,
+                Name = n.Name,
+                X = n.X,
+                Y = n.Y,
+                Direction = MapCompute.GetNodeDirection(n.Direction),
+            })];
+            List<EdgeDto> edges = [..solution.Edges.Select(e => new EdgeDto()
+            {
+                Id = e.Id,
+                StartNodeId = e.StartNodeId,
+                EndNodeId = e.EndNodeId,
+                ControlPoint1X = e.ControlPoint1X,
+                ControlPoint1Y = e.ControlPoint1Y,
+                ControlPoint2X = e.ControlPoint2X,
+                ControlPoint2Y = e.ControlPoint2Y,
+                TrajectoryDegree = e.TrajectoryDegree,
+            })];
+
+            UpdatePath([.. nodes], [.. edges]);
+        }
+    }
+
+    private async Task<Guid> RequestInACS(Dictionary<Guid, ZoneDto[]> newZones)
+    {
+        Guid trafficACSrelaseNodeId = Guid.Empty;
+        foreach (var zone in newZones)
+        {
+            if (zone.Value.Length == 0) trafficACSrelaseNodeId = zone.Key;
+            else
+            {
+                bool requestSuccess = true;
+                foreach (var zoneACS in zone.Value)
+                {
+                    if (string.IsNullOrEmpty(zoneACS.Name)) continue;
+                    if (CurrentZones.Any(z => z.Id == zoneACS.Id)) continue;
+
+                    var getTrafficACS = await TrafficACS.RequestIn(RobotController.SerialNumber, zoneACS.Name);
+                    if (getTrafficACS.IsSuccess && getTrafficACS.Data) CurrentZones.Add(zoneACS);
+                    else
+                    {
+                        requestSuccess = false;
+                        break;
+                    }
+                }
+                if (requestSuccess) trafficACSrelaseNodeId = zone.Key;
+                else break;
+            }
+        }
+        return trafficACSrelaseNodeId;
+    }
+
+    private async Task UpdateTraffic()
+    {
+        var trafficManagerGoalIndex = Nodes.FindIndex(n => n.Id == TrafficManagerGoalId);
+        var trafficACSGoalIndex = Nodes.FindIndex(n => n.Id == TrafficACSGoalId);
+        if (trafficManagerGoalIndex != -1 && trafficACSGoalIndex != -1)
+        {
+            var goalIndex = Math.Min(trafficManagerGoalIndex, trafficACSGoalIndex);
+            NodeDto goal = Nodes[goalIndex];
+            var updatemsg = UpdateGoal(Nodes[goalIndex].Id);
+            if (updatemsg && Nodes[goalIndex].Id != CurrentBaseId)
+            {
+                var publish = await PublishOrder(Nodes[goalIndex].Id);
+                if (!publish) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order publish xảy ra lỗi");
+            }
+        }
+    }
+
+    private async Task GoOutTrafficACS(Guid lastNodeId)
+    {
+        if (CurrentBaseId == Guid.Empty) return;
+        var goalIndex = Nodes.FindIndex(n => n.Id == CurrentBaseId);
+        if (goalIndex == -1) return;
+        var inNodeIndex = Nodes.FindIndex(n => n.Id == lastNodeId);
+        inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
+        if (goalIndex <= inNodeIndex) return;
+        var baseNodes = Nodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
+        var baseZones = PathPlanner.GetZones([.. baseNodes], Zones);
+        var outZones = CurrentZones.Where(z => !baseZones.Any(bz => bz.Id == z.Id)).ToList();
+        foreach (var zoneACS in outZones)
+        {
+            if (string.IsNullOrEmpty(zoneACS.Name)) continue;
+            var outTrafficACS = await TrafficACS.RequestOut(RobotController.SerialNumber, zoneACS.Name);
+            if (outTrafficACS.IsSuccess && outTrafficACS.Data) CurrentZones.RemoveAll(z => z.Id == zoneACS.Id);
+        }
+    }
+
+    private Guid GetLastNodeId()
+    {
+        Guid lastNodeId = Guid.Empty;
+        double minDeviationDistance = TrafficACS.DeviationDistance;
+        foreach (var node in Nodes)
+        {
+            var distance = Math.Sqrt(Math.Pow(RobotController.VisualizationMsg.AgvPosition.X - node.X, 2) + Math.Pow(RobotController.VisualizationMsg.AgvPosition.Y - node.Y, 2));
+            if (distance < minDeviationDistance)
+            {
+                minDeviationDistance = distance;
+                lastNodeId = node.Id;
+            }
+        }
+        return lastNodeId;
+    }
+
+    private async Task TimerHandler()
+    {
+        try
+        {
+            var lastNodeId = GetLastNodeId();
+            if (lastNodeId != Guid.Empty && LastNodeId != lastNodeId) LastNodeId = lastNodeId;
+
+            if (TimerCounter++ > 10)
+            {
+                TimerCounter = 0;
+                if (IsOrderClear())
+                {
+                    if (RobotController.StateMsg.LastNodeId == Nodes[^1].Id.ToString() && GetActionFinished())
+                    {
+                        IsCompleted = true;
+                        Logger.Info($"{RobotController.StateMsg.SerialNumber} - Robot Order đã hoàn thành tới đích {Nodes[^1].Name}");
+                    }
+                    else if (RobotController.StateMsg.LastNodeId != Nodes[^1].Id.ToString() || GetActionFailed())
+                    {
+                        Logger.Error($"{RobotController.StateMsg.SerialNumber} - Robot Order kết thúc lỗi. {string.Join(", ", GetError())}");
+                        IsError = true;
+                    }
+                }
+                if (!RobotController.IsOnline)
+                {
+                    Logger.Error($"{RobotController.StateMsg.SerialNumber} - Robot Order kết thúc: robot mất kết nối");
+                    IsError = true;
+                }
+                if (IsCompleted || IsError || (IsWaittingCancel && IsOrderClear()))
+                {
+                    Dispose();
+                    return;
+                }
+                if (!IsWaittingCancel)
+                {
+                    await GoOutTrafficACS(LastNodeId);
+
+                    if (!TrafficManager.Enable)
+                    {
+                        if (TrafficManagerGoalId != Nodes[^1].Id) TrafficManagerGoalId = Nodes[^1].Id;
+                    }
+                    else
+                    {
+                        TrafficManager.UpdateInNode(RobotController.SerialNumber, LastNodeId);
+                        var trafficSolution = TrafficManager.GetTrafficNode(RobotController.SerialNumber);
+                        switch (trafficSolution.State)
+                        {
+                            case TrafficSolutionState.GiveWay:
+                                HandleGiveWayState(trafficSolution);
+                                break;
+                            case TrafficSolutionState.RefreshPath:
+                                HandleRefreshPath(trafficSolution);
+                                break;
+                            case TrafficSolutionState.Complete:
+                            case TrafficSolutionState.Waitting:
+                            default:
+                                break;
+                        }
+                        TrafficSolutionState = trafficSolution.State;
+                        var goal = Nodes.FirstOrDefault(n => n.Id == trafficSolution.ReleaseNode.Id);
+                        if (goal is not null && goal.Id != TrafficManagerGoalId) TrafficManagerGoalId = goal.Id;
+                    }
+
+                    if (!TrafficACS.Enable)
+                    {
+                        if (TrafficACSGoalId != Nodes[^1].Id) TrafficACSGoalId = Nodes[^1].Id;
+                    }
+                    else
+                    {
+                        var newZones = TrafficACS.GetZones(LastNodeId, [.. Nodes], Zones);
+                        var trafficACSrelaseNodeId = await RequestInACS(newZones);
+                        if (trafficACSrelaseNodeId != Guid.Empty && trafficACSrelaseNodeId != TrafficACSGoalId) TrafficACSGoalId = trafficACSrelaseNodeId;
+                    }
+
+                    await UpdateTraffic();
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"{RobotController.StateMsg.SerialNumber} - Robot Order Handler xảy ra lỗi: {ex}");
+        }
+    }
+
+    private IEnumerable<string> GetError()
+    {
+        if (RobotController.StateMsg.Errors is not null && RobotController.StateMsg.Errors.Length > 0)
+        {
+            foreach (var error in RobotController.StateMsg.Errors)
+            {
+                yield return $"{error.ErrorType} - {error.ErrorDescription}";
+            }
+        }
+        yield break;
+    }
+
+    private bool GetActionFinished()
+    {
+        if (RobotController.StateMsg.ActionStates is not null && RobotController.StateMsg.ActionStates.Length > 0)
+        {
+            var finalActions = OrderMsg.Nodes[^1].Actions;
+            foreach (var actionState in RobotController.StateMsg.ActionStates)
+            {
+                if (finalActions.Any(a => a.ActionId == actionState.ActionId) && actionState.ActionStatus != ActionStatus.FINISHED.ToString()) return false;
+            }
+        }
+        return true;
+    }
+
+    private bool GetActionFailed()
+    {
+        if (RobotController.StateMsg.ActionStates is not null && RobotController.StateMsg.ActionStates.Length > 0)
+        {
+            var finalActions = OrderMsg.Nodes[^1].Actions;
+            foreach (var actionState in RobotController.StateMsg.ActionStates)
+            {
+                if (finalActions.Any(a => a.ActionId == actionState.ActionId) && actionState.ActionStatus == ActionStatus.FAILED.ToString()) return true;
+            }
+        }
+        return false;
+    }
+
+    public async Task<bool> Cancel(CancellationToken cancellation)
+    {
+        try
+        {
+            IsWaittingCancel = true;
+            while (!cancellation.IsCancellationRequested)
+            {
+                if (IsDisposed)
+                {
+                    IsCanceled = true;
+                    return true;
+                }
+                if (Timer.Disposed) Dispose();
+                try { await Task.Delay(500, cancellation); } catch { }
+            }
+            IsWaittingCancel = false;
+            return false;
+        }
+        catch
+        {
+            IsWaittingCancel = false;
+            return false;
+        }
+    }
+
+    private NavigationPathEdge[] SplitChecking(NodeDto lastNode, NodeDto nearLastNode, EdgeDto edge)
+    {
+        List<NavigationPathEdge> pathEdges = [];
+        var splitStartPath = PathPlanner.PathSplit([new NodeDto
+                        {
+                            Id = lastNode.Id,
+                            X = lastNode.X,
+                            Y = lastNode.Y,
+                        }, new NodeDto
+                        {
+                            Id = nearLastNode.Id,
+                            X = nearLastNode.X,
+                            Y = nearLastNode.Y,
+                        }], [edge], 0.1);
+        if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
+        {
+            int index = 0;
+            double minDistance = double.MaxValue;
+            for (int i = 0; i < splitStartPath.Data.Length; i++)
+            {
+                var distance = Math.Sqrt(Math.Pow(RobotController.VisualizationMsg.AgvPosition.X - splitStartPath.Data[i].X, 2) +
+                                         Math.Pow(RobotController.VisualizationMsg.AgvPosition.Y - splitStartPath.Data[i].Y, 2));
+                if (distance < minDistance)
+                {
+                    minDistance = distance;
+                    index = i;
+                }
+            }
+            for (int i = index; i < splitStartPath.Data.Length - 1; i++)
+            {
+                pathEdges.Add(new()
+                {
+                    StartX = splitStartPath.Data[i].X,
+                    StartY = splitStartPath.Data[i].Y,
+                    EndX = splitStartPath.Data[i + 1].X,
+                    EndY = splitStartPath.Data[i + 1].Y,
+                    Degree = 1,
+                });
+            }
+        }
+        return [.. pathEdges];
+    }
+
+    private NavigationPathEdge[] GetFullPath()
+    {
+        List<NavigationPathEdge> pathEdges = [];
+        if (RobotController.StateMsg.NodeStates is not null && RobotController.StateMsg.NodeStates.Length > 0)
+        {
+            var lastNodeIndex = Nodes.FindIndex(n => n.Id == LastNodeId);
+            lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0;
+            if (lastNodeIndex < Nodes.Count - 1) pathEdges = [.. SplitChecking(Nodes[lastNodeIndex], Nodes[lastNodeIndex + 1], Edges[lastNodeIndex])];
+            if (lastNodeIndex < Nodes.Count - 2)
+            {
+                var nodes = Nodes.GetRange(lastNodeIndex + 1, Nodes.Count - lastNodeIndex - 1);
+                var edges = Edges.GetRange(lastNodeIndex + 1, nodes.Count - 1);
+                for (int i = 0; i < nodes.Count - 1; i++)
+                {
+                    pathEdges.Add(new()
+                    {
+                        StartX = nodes[i].X,
+                        StartY = nodes[i].Y,
+                        EndX = nodes[i + 1].X,
+                        EndY = nodes[i + 1].Y,
+                        ControlPoint1X = edges[i].ControlPoint1X,
+                        ControlPoint1Y = edges[i].ControlPoint1Y,
+                        ControlPoint2X = edges[i].ControlPoint2X,
+                        ControlPoint2Y = edges[i].ControlPoint2Y,
+                        Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
+                    });
+                }
+            }
+        }
+        return [.. pathEdges];
+    }
+
+    private NavigationPathEdge[] GetBasePath()
+    {
+        List<NavigationPathEdge> pathEdges = [];
+        if (RobotController.StateMsg.NodeStates is not null && RobotController.StateMsg.NodeStates.Length > 0)
+        {
+            var lastNodeIndex = Nodes.FindIndex(n => n.Id == LastNodeId);
+            lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0;
+            var baseNodeIndex = Nodes.FindIndex(n => n.Id == CurrentBaseId);
+            baseNodeIndex = baseNodeIndex != -1 ? baseNodeIndex : Nodes.Count - 1;
+
+            if (lastNodeIndex < baseNodeIndex)
+            {
+                if (lastNodeIndex < Nodes.Count - 1) pathEdges = [.. SplitChecking(Nodes[lastNodeIndex], Nodes[lastNodeIndex + 1], Edges[lastNodeIndex])];
+                if (lastNodeIndex < Nodes.Count - 2 && lastNodeIndex < baseNodeIndex - 1)
+                {
+                    var nodes = Nodes.GetRange(lastNodeIndex + 1, baseNodeIndex - lastNodeIndex);
+                    if (nodes.Count > 1)
+                    {
+                        var edges = Edges.GetRange(lastNodeIndex + 1, nodes.Count - 1);
+                        for (int i = 0; i < nodes.Count - 1; i++)
+                        {
+                            pathEdges.Add(new()
+                            {
+                                StartX = nodes[i].X,
+                                StartY = nodes[i].Y,
+                                EndX = nodes[i + 1].X,
+                                EndY = nodes[i + 1].Y,
+                                ControlPoint1X = edges[i].ControlPoint1X,
+                                ControlPoint1Y = edges[i].ControlPoint1Y,
+                                ControlPoint2X = edges[i].ControlPoint2X,
+                                ControlPoint2Y = edges[i].ControlPoint2Y,
+                                Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
+                            });
+                        }
+                    }
+                }
+            }
+        }
+        return [.. pathEdges];
+    }
+
+    private bool IsOrderClear() => RobotController.StateMsg.NodeStates.Length == 0 && RobotController.StateMsg.EdgeStates.Length == 0;
+
+    public void CreateComledted()
+    {
+        IsCompleted = true;
+        IsCanceled = false;
+        IsError = false;
+    }
+
+    public void Dispose()
+    {
+        IsDisposed = true;
+        Timer.Dispose();
+        TrafficManager.DeleteAgent(RobotController.SerialNumber);
+        GC.SuppressFinalize(this);
+    }
+}

RobotNet.RobotManager/Services/RobotEditorStorageRepository.cs

@@ -0,0 +1,129 @@
+using Minio;
+using Minio.DataModel.Args;
+using Minio.Exceptions;
+
+namespace RobotNet.RobotManager.Services;
+
+public class RobotEditorStorageRepository
+{
+    private class MinioOption
+    {
+        public bool UsingLocal { get; set; }
+        public string? Endpoint { get; set; }
+        public string? User { get; set; }
+        public string? Password { get; set; }
+        public string? Bucket { get; set; }
+    }
+    private readonly IMinioClient MinioClient;
+    private readonly MinioOption MinioOptions = new();
+    public RobotEditorStorageRepository(IConfiguration configuration)
+    {
+        configuration.Bind("MinIO", MinioOptions);
+        MinioClient = new MinioClient()
+                         .WithEndpoint(MinioOptions.Endpoint)
+                         .WithCredentials(MinioOptions.User, MinioOptions.Password)
+                         .WithSSL(false)
+                         .Build();
+    }
+
+
+    public async Task<(bool, string)> UploadAsync(string path, string objectName, Stream data, long size, string contentType, CancellationToken cancellationToken)
+    {
+        try
+        {
+            if (!MinioOptions.UsingLocal)
+            {
+                var beArgs = new BucketExistsArgs().WithBucket(MinioOptions.Bucket);
+                bool found = await MinioClient.BucketExistsAsync(beArgs, cancellationToken).ConfigureAwait(false);
+                if (!found)
+                {
+                    var mbArgs = new MakeBucketArgs()
+                                    .WithBucket(MinioOptions.Bucket);
+                    await MinioClient.MakeBucketAsync(mbArgs, cancellationToken).ConfigureAwait(false);
+                }
+                var getListBucketsTask = await MinioClient.ListBucketsAsync(cancellationToken);
+                var putObjectArgs = new PutObjectArgs()
+                        .WithBucket(MinioOptions.Bucket)
+                        .WithObject($"{path}/{objectName}")
+                        .WithObjectSize(size)
+                        .WithStreamData(data)
+                        .WithContentType(contentType);
+                await MinioClient.PutObjectAsync(putObjectArgs, cancellationToken).ConfigureAwait(false);
+                return (true, "");
+            }
+
+            var mapImageFolder = string.IsNullOrEmpty(MinioOptions.Bucket) ? "MapImages" : MinioOptions.Bucket;
+            if (!Directory.Exists(mapImageFolder)) Directory.CreateDirectory(mapImageFolder);
+            var pathLocal = Path.Combine(mapImageFolder, $"{objectName}.png");
+            if (File.Exists($"{pathLocal}.bk")) File.Delete($"{pathLocal}.bk");
+            if (File.Exists(pathLocal)) File.Move(pathLocal, $"{pathLocal}.bk");
+            using (Stream fileStream = new FileStream(pathLocal, FileMode.Create))
+            {
+                await data.CopyToAsync(fileStream, cancellationToken);
+            }
+            return (true, "");
+        }
+        catch (MinioException ex)
+        {
+            return (false, ex.Message);
+        }
+    }
+
+    public (bool usingLocal, string url) GetUrl(string path, string objectName)
+    {
+        try
+        {
+            if (!MinioOptions.UsingLocal)
+            {
+                var presignedGetObjectArgs = new PresignedGetObjectArgs()
+                        .WithBucket(MinioOptions.Bucket)
+                        .WithObject($"{path}/{objectName}")
+                        .WithExpiry(60 * 60 * 24);
+                var url = MinioClient.PresignedGetObjectAsync(presignedGetObjectArgs);
+                url.Wait();
+                return (false, url.Result);
+            }
+            var mapImageFolder = string.IsNullOrEmpty(MinioOptions.Bucket) ? "MapImages" : MinioOptions.Bucket;
+            if (Directory.Exists(mapImageFolder))
+            {
+                var pathLocal = Path.Combine(mapImageFolder, $"{objectName}.png");
+                if (File.Exists(pathLocal))
+                {
+                    return (true, pathLocal);
+                }
+            }
+            return (true, "");
+        }
+        catch (MinioException e)
+        {
+            return (false, e.Message);
+        }
+    }
+
+    public async Task<(bool, string)> DeleteAsync(string path, string objectName, CancellationToken cancellationToken)
+    {
+        try
+        {
+            if (!MinioOptions.UsingLocal)
+            {
+                var removeObjectArgs = new RemoveObjectArgs()
+                        .WithBucket(MinioOptions.Bucket)
+                        .WithObject($"{path}/{objectName}");
+                await MinioClient.RemoveObjectAsync(removeObjectArgs, cancellationToken).ConfigureAwait(false);
+                return (true, "");
+            }
+            var mapImageFolder = string.IsNullOrEmpty(MinioOptions.Bucket) ? "MapImages" : MinioOptions.Bucket;
+            if (Directory.Exists(mapImageFolder))
+            {
+                var pathLocal = Path.Combine(mapImageFolder, $"{objectName}.png");
+                if (File.Exists(pathLocal)) File.Delete(pathLocal);
+                if (File.Exists($"{pathLocal}.bk")) File.Delete($"{pathLocal}.bk");
+            }
+            return (true, "");
+        }
+        catch (MinioException ex)
+        {
+            return (false, ex.Message);
+        }
+    }
+}

RobotNet.RobotManager/Services/RobotManager.cs

@@ -0,0 +1,562 @@
+using MQTTnet;
+using MQTTnet.Protocol;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services.Robot;
+using RobotNet.RobotManager.Services.Simulation;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.OpenACS;
+using RobotNet.RobotShares.VDA5050;
+using RobotNet.RobotShares.VDA5050.Connection;
+using RobotNet.RobotShares.VDA5050.Factsheet;
+using RobotNet.RobotShares.VDA5050.FactsheetExtend;
+using RobotNet.RobotShares.VDA5050.State;
+using RobotNet.RobotShares.VDA5050.Visualization;
+using RobotNet.Script.Expressions;
+using RobotNet.Shares;
+using System.Linq.Expressions;
+using System.Text;
+using System.Text.Json;
+
+namespace RobotNet.RobotManager.Services;
+
+public class RobotManager : BackgroundService
+{
+    public IEnumerable<string> RobotSerialNumbers => RobotControllers.Keys;
+
+    private readonly VDA5050Setting VDA5050Setting = new();
+    private Dictionary<string, IRobotController> RobotControllers { get; } = [];
+    private readonly IServiceProvider ServiceProvider;
+    private IMqttClient? MQTTClient;
+    private readonly LoggerController<RobotManager> Logger;
+
+    public RobotManager(IConfiguration configuration, IServiceProvider serviceProvider, LoggerController<RobotManager> logger)
+    {
+        configuration.Bind("VDA5050Setting", VDA5050Setting);
+        ServiceProvider = serviceProvider;
+        Logger = logger;
+    }
+
+    public IRobotController? this[string robotid] => RobotControllers.TryGetValue(robotid, out IRobotController? value) ? value : null;
+
+    private RobotController? AddRobotController(string robotId)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        var ApplicationDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+
+        var robotdb = ApplicationDb.Robots.FirstOrDefault(robot => robot.RobotId == robotId);
+        if (robotdb is null) return null;
+        var robotModel = ApplicationDb.RobotModels.FirstOrDefault(model => model.Id == robotdb.ModelId);
+        if (robotModel is null) return null;
+
+        var robotController = new RobotController(robotId, VDA5050Setting.Manufacturer, VDA5050Setting.Version, robotModel.NavigationType, ServiceProvider, PublishMQTT);
+        RobotControllers.Add(robotId, robotController);
+        return robotController;
+    }
+
+    public void AddRobotSimulation(IEnumerable<string> robotIds)
+    {
+        using var scope = ServiceProvider.CreateScope();
+        var ApplicationDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+        var Traffic = scope.ServiceProvider.GetRequiredService<TrafficManager>();
+
+        foreach (var robotId in robotIds)
+        {
+            if (RobotControllers.TryGetValue(robotId, out _)) continue;
+            var robotdb = ApplicationDb.Robots.FirstOrDefault(robot => robot.RobotId == robotId);
+            if (robotdb is null) return;
+            var robotModel = ApplicationDb.RobotModels.FirstOrDefault(model => model.Id == robotdb.ModelId);
+            if (robotModel is null) return;
+
+            var robotController = new RobotSimulation(robotId, robotModel, ServiceProvider);
+            RobotControllers.Add(robotId, robotController);
+        }
+    }
+
+    public bool DeleteRobot(string robotId)
+    {
+        try
+        {
+            if (RobotControllers.TryGetValue(robotId, out var robotcontroller))
+            {
+                RobotControllers.Remove(robotId);
+            }
+            return true;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Hệ thống xảy ra lỗi khi xóa bỏ robot: {ex.Message}");
+            return false;
+        }
+    }
+
+    public IEnumerable<RobotInfomationDto> GetRobotInfo(Guid mapId)
+    {
+        try
+        {
+            using var scope = ServiceProvider.CreateScope();
+            var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            List<RobotInfomationDto> RobotInfos = [];
+            foreach (var robotcontroller in RobotControllers.Values.ToList())
+            {
+                var robotDb = RobotDb.Robots.FirstOrDefault(r => robotcontroller.SerialNumber == r.RobotId);
+                if (robotDb is null || robotDb.MapId != mapId) continue;
+                RobotInfos.Add(new()
+                {
+                    RobotId = robotcontroller.SerialNumber,
+                    Name = robotDb?.Name,
+                    MapId = robotDb is null ? Guid.Empty : robotDb.MapId,
+                    Battery = new()
+                    {
+                        BatteryHealth = robotcontroller.StateMsg.BatteryState.BatteryHealth,
+                        BatteryCharge = robotcontroller.StateMsg.BatteryState.BatteryCharge,
+                        BatteryVoltage = robotcontroller.StateMsg.BatteryState.BatteryVoltage,
+                        Charging = robotcontroller.StateMsg.BatteryState.Charging,
+                    },
+                    Errors = [],
+                    Infomations = [],
+                    Navigation = new()
+                    {
+                        NavigationState = robotcontroller.State,
+                        RobotPath = robotcontroller.FullPath,
+                        RobotBasePath = robotcontroller.BasePath,
+                    },
+                    AgvPosition = new()
+                    {
+                        X = robotcontroller.VisualizationMsg.AgvPosition.X,
+                        Y = robotcontroller.VisualizationMsg.AgvPosition.Y,
+                        Theta = robotcontroller.VisualizationMsg.AgvPosition.Theta,
+                        PositionInitialized = robotcontroller.VisualizationMsg.AgvPosition.PositionInitialized,
+                        LocalizationScore = robotcontroller.VisualizationMsg.AgvPosition.LocalizationScore,
+                        DeviationRange = robotcontroller.VisualizationMsg.AgvPosition.DeviationRange,
+                    },
+                    AgvVelocity = new()
+                    {
+                        Vx = robotcontroller.VisualizationMsg.Velocity.Vx,
+                        Vy = robotcontroller.VisualizationMsg.Velocity.Vy,
+                        Omega = robotcontroller.VisualizationMsg.Velocity.Omega,
+                    },
+                    Loads = robotcontroller.StateMsg.Loads,
+                });
+
+            }
+            return RobotInfos;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Get Robot Info Error: - {ex}");
+            return [];
+        }
+    }
+
+    public RobotACSLockedDto[] GetRobotACSLocked()
+    {
+        try
+        {
+            List<RobotACSLockedDto> robotACSLockedDtos = [];
+            foreach(var robot in RobotControllers.Values)
+            {
+                robotACSLockedDtos.Add(new()
+                {
+                    RobotId = robot.SerialNumber,
+                    ZoneIds = robot.CurrentZones,
+                });
+            }
+            return [..robotACSLockedDtos];
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Get Robot ACS Locked Error: - {ex}");
+            return [];
+        }
+    }
+
+    private static Script.Expressions.RobotState ConvertIRobotControllerToRobotState(IRobotController robotController)
+    {
+        bool isReady = robotController.IsOnline && !robotController.OrderState.IsProcessing && robotController.StateMsg.Errors.Length == 0;
+        if (robotController.ActionStates.Length > 0)
+        {
+            isReady = isReady && robotController.ActionStates.All(a => !a.IsProcessing);
+        }
+        return new RobotState(isReady, 
+            robotController.StateMsg.BatteryState.BatteryVoltage, 
+            robotController.StateMsg.Loads.Length != 0, 
+            robotController.StateMsg.BatteryState.Charging, 
+            robotController.StateMsg.AgvPosition.X,
+            robotController.StateMsg.AgvPosition.Y,
+            robotController.StateMsg.AgvPosition.Theta);
+    }
+
+    /// <summary>
+    /// Tìm kiếm robot theo tên model, tên bản đồ
+    /// </summary>
+    /// <param name="robotModelName"></param>
+    /// <param name="mapName"></param>
+    /// <returns></returns>
+    public async Task<MessageResult<string[]>> SearchRobot(string? robotModelName, string? mapName, Func<Script.Expressions.RobotState, bool> funcSearch)
+    {
+        List<string> robotIds = [];
+        try
+        {
+            using var scope = ServiceProvider.CreateScope();
+            var RobotDbContext = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            var MapManager = scope.ServiceProvider.GetRequiredService<MapManager>();
+            var robotModel = RobotDbContext.RobotModels.FirstOrDefault(m => m.ModelName == robotModelName);
+            if (robotModel is null) return new(false, $"Robot model name {robotModelName} không tồn tại");
+            var map = await MapManager.GetMapInfo(mapName ?? "");
+            if (!map.IsSuccess && !string.IsNullOrEmpty(mapName)) return new(false, map.Message);
+            if (map is null || map.Data is null) return new(false, $"Map name {mapName} không tồn tại");
+
+            foreach (var robot in RobotControllers.Values)
+            {
+                if (!robot.IsOnline) continue;
+                var robotDb = RobotDbContext.Robots.FirstOrDefault(r => r.RobotId == robot.SerialNumber);
+                if (robotDb is null) continue;
+                var robotDbModel = RobotDbContext.RobotModels.FirstOrDefault(m => m.Id == robotDb.ModelId);
+                if (robotDbModel is null) continue;
+                var robotDbMap = await MapManager.GetMapInfo(robotDb.MapId);
+                if (robotDbMap is null || !robotDbMap.IsSuccess) continue;
+                if (robotDbMap.Data is null) continue;
+
+                bool isRobotModelMatch = string.IsNullOrEmpty(robotModelName) || robotDbModel.Id == robotModel.Id;
+                bool isMapMatch = string.IsNullOrEmpty(mapName) || (map is not null && robotDbMap.Data.Id == map.Data.Id);
+                bool isExpressionMatch = funcSearch(ConvertIRobotControllerToRobotState(robot));
+
+                if (isRobotModelMatch && isMapMatch && isExpressionMatch) robotIds.Add(robot.SerialNumber);
+            }
+            return new(true, robotIds.Count > 0 ? "Tìm thấy robot" : "Không tìm thấy robot nào thỏa mãn điều kiện")
+            {
+                Data = [.. robotIds],
+            };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Find Robot Error: {ex}");
+            return new(false, "Hệ thống có lỗi xảy ra");
+        }
+    }
+
+    public RobotVDA5050StateDto? GetRobotVDA5050State(string robotId)
+    {
+        try
+        {
+            using var scope = ServiceProvider.CreateScope();
+            var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            if (RobotControllers.TryGetValue(robotId, out IRobotController? robotController) && robotController is not null)
+            {
+                var robot = robotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
+                if (robot is null) return null;
+
+                return new()
+                {
+                    RobotId = robotId,
+                    Name = robot.Name,
+                    MapId = robot.MapId,
+                    Online = robotController.IsOnline,
+                    State = robotController.StateMsg,
+                    OrderState = robotController.OrderState,
+                    IsWorking = robotController.IsWorking,
+                    Visualization = robotController.VisualizationMsg,
+                };
+            }
+            return null;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Get VDA State Error: {robotId} - {ex}");
+            return null;
+        }
+    }
+
+    public IEnumerable<RobotOnlineStateDto> GetRobotOnlineState()
+    {
+        try
+        {
+            using var scope = ServiceProvider.CreateScope();
+            var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            var robots = robotDb.Robots.ToList();
+            return [.. robots.Select(robot =>
+            {
+                var robotOnline = RobotControllers.TryGetValue(robot.RobotId, out IRobotController? robotController);
+                return new RobotOnlineStateDto()
+                {
+                    RobotId = robot.RobotId,
+                    State = !robotOnline || robotController is null ? "OFFLINE" : robotController.State,
+                    IsOnline = robotOnline && robotController is not null && robotController.IsOnline,
+                    Battery = !robotOnline || robotController is null ? 0 : robotController.StateMsg.BatteryState.BatteryHealth,
+                };
+            })];
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Get Robot Online State Error: {ex}");
+            return [];
+        }
+    }
+
+    private bool PublishMQTT(string topic, string data)
+    {
+        var repeat = VDA5050Setting.Repeat;
+        while (repeat-- > 0)
+        {
+            try
+            {
+                var applicationMessage = new MqttApplicationMessageBuilder()
+                                .WithTopic(topic)
+                                .WithPayload(data)
+                                .WithQualityOfServiceLevel(MqttQualityOfServiceLevel.AtLeastOnce)
+                                .Build();
+                if (MQTTClient is null) return false;
+                var publish = MQTTClient.PublishAsync(applicationMessage, CancellationToken.None);
+                publish.Wait();
+                if (!publish.Result.IsSuccess) continue;
+                return publish.Result.IsSuccess;
+            }
+            catch
+            {
+                return false;
+            }
+        }
+        return false;
+    }
+
+    private void ConnectionChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<ConnectionMsg>(data);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
+            IRobotController robotController;
+            if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
+            {
+                value.IsOnline = msg.ConnectionState == ConnectionState.ONLINE.ToString();
+                robotController = value;
+            }
+            else
+            {
+                var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
+                if (addRobtoControllerTask is null) return;
+                robotController = addRobtoControllerTask;
+                RobotControllers[msg.SerialNumber].IsOnline = msg.ConnectionState == ConnectionState.ONLINE.ToString();
+            }
+            robotController.RobotUpdated.Set();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Robot Manager ConnectionChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    private void StateChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<StateMsg>(data, JsonOptionExtends.Write);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
+
+            if (msg.AgvPosition is null) return;
+            msg.AgvPosition.Theta = msg.AgvPosition.Theta * 180 / Math.PI;
+            IRobotController robotController;
+            if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
+            {
+                value.StateMsg = msg;
+                robotController = value;
+            }
+            else
+            {
+                var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
+                if (addRobtoControllerTask is null) return;
+                robotController = addRobtoControllerTask;
+                RobotControllers[msg.SerialNumber].StateMsg = msg;
+            }
+            robotController.IsOnline = true;
+            robotController.RobotUpdated.Set();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Robot Manager StateChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    private void VisualizationChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<VisualizationMsg>(data, JsonOptionExtends.Write);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
+            if (msg.AgvPosition is null) return;
+            msg.AgvPosition.Theta = msg.AgvPosition.Theta * 180 / Math.PI;
+            IRobotController robotController;
+            if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
+            {
+                value.VisualizationMsg = msg;
+                robotController = value;
+            }
+            else
+            {
+                var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
+                if (addRobtoControllerTask is null) return;
+                robotController = addRobtoControllerTask;
+                RobotControllers[msg.SerialNumber].VisualizationMsg = msg;
+            }
+            robotController.IsOnline = true;
+            robotController.RobotUpdated.Set();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Robot Manager VisualizationChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    private void FactsheetChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<FactSheetMsg>(data, JsonOptionExtends.Write);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
+            IRobotController robotController;
+            if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
+            {
+                value.FactSheetMsg = msg;
+                robotController = value;
+            }
+            else
+            {
+                var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
+                if (addRobtoControllerTask is null) return;
+                robotController = addRobtoControllerTask;
+                RobotControllers[msg.SerialNumber].FactSheetMsg = msg;
+            }
+            robotController.IsOnline = true;
+            robotController.RobotUpdated.Set();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Robot Manager FactsheetChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    private void FactsheetExtendChanged(string data)
+    {
+        try
+        {
+            var msg = JsonSerializer.Deserialize<FactsheetExtendMsg>(data, JsonOptionExtends.Write);
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber)) return;
+            IRobotController robotController;
+            if (RobotControllers.TryGetValue(msg.SerialNumber, out IRobotController? value))
+            {
+                value.FactsheetExtendMsg = msg;
+                robotController = value;
+            }
+            else
+            {
+                var addRobtoControllerTask = AddRobotController(msg.SerialNumber);
+                if (addRobtoControllerTask is null) return;
+                robotController = addRobtoControllerTask;
+                RobotControllers[msg.SerialNumber].FactsheetExtendMsg = msg;
+            }
+            robotController.IsOnline = true;
+            robotController.RobotUpdated.Set();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"Robot Manager FactsheetExtendChanged xảy ra lỗi: {ex.Message} - {ex.StackTrace}");
+        }
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            try
+            {
+                var mqttFactory = new MqttClientFactory();
+
+                MQTTClient = mqttFactory.CreateMqttClient();
+                var mqttClientOptions = new MqttClientOptionsBuilder()
+                                        .WithTcpServer(VDA5050Setting.HostServer, VDA5050Setting.Port)
+                                        .WithClientId("FleetManager")
+                                        .WithCredentials(VDA5050Setting.UserName, VDA5050Setting.Password)
+                                        .Build();
+
+                if ((await MQTTClient.ConnectAsync(mqttClientOptions, stoppingToken)).ResultCode != MqttClientConnectResultCode.Success)
+                {
+                    await Task.Delay(1000, stoppingToken);
+                    continue;
+                }
+
+                Logger.Info("Fleet Manager kết nối tới broker thành công");
+                var mqttSubscribeOptions = mqttFactory.CreateSubscribeOptionsBuilder()
+                    .WithTopicFilter(f =>
+                    {
+                        f.WithTopic(VDA5050Topic.Connection);
+                    })
+                    .WithTopicFilter(f =>
+                    {
+                        f.WithTopic(VDA5050Topic.Visualization);
+                    })
+                    .WithTopicFilter(f =>
+                    {
+                        f.WithTopic(VDA5050Topic.State);
+                    })
+                    .WithTopicFilter(f =>
+                    {
+                        f.WithTopic(VDA5050Topic.Factsheet);
+                    })
+                    .WithTopicFilter(f =>
+                    {
+                        f.WithTopic(VDA5050Topic.FactsheetExtend);
+                    })
+                    .Build();
+
+                var response = await MQTTClient.SubscribeAsync(mqttSubscribeOptions, stoppingToken);
+                Logger.Info("Fleet Manager Subscribe thành công");
+                MQTTClient.ApplicationMessageReceivedAsync += delegate (MqttApplicationMessageReceivedEventArgs args)
+                {
+                    var stringData = Encoding.Default.GetString(args.ApplicationMessage.Payload);
+                    switch (args.ApplicationMessage.Topic)
+                    {
+                        case VDA5050Topic.Connection: ConnectionChanged(stringData); break;
+                        case VDA5050Topic.Visualization: VisualizationChanged(stringData); break;
+                        case VDA5050Topic.State: StateChanged(stringData); break;
+                        case VDA5050Topic.Factsheet: FactsheetChanged(stringData); break;
+                        case VDA5050Topic.FactsheetExtend: FactsheetExtendChanged(stringData); break;
+                        default: break;
+                    }
+                    return Task.CompletedTask;
+                };
+                Logger.Info("Fleet Manager Subscribe event thành công");
+                break;
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Kết nối tới broker xảy ra lỗi: {ex.Message}");
+                await Task.Delay(3000, stoppingToken);
+            }
+        }
+
+        Logger.Info("Fleet Manager bắt đầu kiểm tra kết nối tới robot");
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            try
+            {
+                foreach (var robotController in RobotControllers)
+                {
+                    if (!robotController.Value.RobotUpdated.WaitOne(TimeSpan.FromSeconds(1)) && robotController.Value.IsOnline)
+                    {
+                        robotController.Value.Dispose();
+                        continue;
+                    }
+                    robotController.Value.RobotUpdated.Reset();
+                }
+                await Task.Delay(TimeSpan.FromSeconds(VDA5050Setting.CheckingRobotMsgTimout), stoppingToken);
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Kiểm tra kết nối tới robot xảy ra lỗi: {ex.Message}");
+                await Task.Delay(3000, stoppingToken);
+            }
+        }
+    }
+}

RobotNet.RobotManager/Services/RobotPublisher.cs

@@ -0,0 +1,105 @@
+using Microsoft.AspNetCore.SignalR;
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.HubClients;
+using RobotNet.RobotManager.Hubs;
+
+namespace RobotNet.RobotManager.Services;
+
+public class RobotPublisher(RobotManager RobotManager, IHubContext<RobotHub> RobotHub, MapHubClient MapHub, LoggerController<RobotPublisher> Logger) : IHostedService
+{
+    public readonly Dictionary<Guid, string> MapActive = [];
+    public readonly Dictionary<string, string> RobotDetailActive = [];
+
+    private int PublishCounter = 0;
+    private const int intervalTime = 200;
+
+    private WatchTimerAsync<RobotPublisher>? Timer;
+
+    private readonly Dictionary<Guid, ElementDto[]> ElementsState = [];
+
+    private async Task TimerHandler()
+    {
+        try
+        {
+            foreach (var mapActive in MapActive)
+            {
+                var robotinfos = RobotManager.GetRobotInfo(mapActive.Key);
+                if (robotinfos is not null && robotinfos.Any())
+                {
+                    await RobotHub.Clients.Client(mapActive.Value).SendAsync("UpdateChanged", robotinfos);
+                }
+            }
+
+            PublishCounter++;
+            if (PublishCounter >= 5)
+            {
+                await RobotHub.Clients.All.SendAsync("IsOnlineChanged", RobotManager.GetRobotOnlineState());
+                foreach (var robotActive in RobotDetailActive)
+                {
+                    var robotinfos = RobotManager.GetRobotVDA5050State(robotActive.Key);
+                    if (robotinfos is not null)
+                    {
+                        await RobotHub.Clients.Client(robotActive.Value).SendAsync("VDA5050InfoChanged", robotinfos);
+                    }
+                }
+                foreach (var mapActive in MapActive)
+                {
+                    var elementsState = await MapHub.GetElementsState(mapActive.Key);
+                    if (!elementsState.IsSuccess) Logger.Warning($"Robot Publisher Error: Lấy dữ liệu element xảy ra lỗi: {elementsState.Message}");
+                    else if (elementsState.Data?.Length > 0)
+                    {
+                        ElementsState.TryGetValue(mapActive.Key, out ElementDto[]? elementsOlder);
+                        List<ElementDto> updateElementsState = [];
+                        foreach (var element in elementsState.Data)
+                        {
+                            if (elementsOlder is null || !elementsOlder.Any(e => e.Id == element.Id) ||
+                                elementsOlder.Any(e => e.Id == element.Id && (e.IsOpen != element.IsOpen || e.OffsetX != element.OffsetX || e.OffsetY != element.OffsetY)))
+                            {
+                                updateElementsState.Add(element);
+                            }
+                        }
+                        if (updateElementsState is not null && updateElementsState.Count != 0)
+                        {
+                            ElementsState[mapActive.Key] = elementsState.Data;
+                            await RobotHub.Clients.Client(mapActive.Value).SendAsync("ElementsStateChanged", updateElementsState);
+                        }
+                    }
+                }
+                PublishCounter = 0;
+            }
+
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Robot Publisher Error: {ex}");
+        }
+    }
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        await Task.Yield();
+
+        while (!cancellationToken.IsCancellationRequested)
+        {
+            try
+            {
+                Timer = new(intervalTime, TimerHandler, Logger);
+                Timer.Start();
+                await MapHub.StartAsync();
+                break;
+            }
+
+            catch (Exception ex)
+            {
+                Logger.Warning($"Robot Publisher Start: Khởi tạo kết nối với MapManager có lỗi xảy ra: {ex.Message}");
+                await Task.Delay(2000, cancellationToken);
+            }
+        }
+    }
+
+    public Task StopAsync(CancellationToken cancellationToken)
+    {
+        Timer?.Dispose();
+        return Task.CompletedTask;
+    }
+}

RobotNet.RobotManager/Services/Simulation/Algorithm/FuzzyLogic.cs

@@ -0,0 +1,252 @@
+namespace RobotNet.RobotManager.Services.Simulation.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;
+    }
+}

RobotNet.RobotManager/Services/Simulation/Algorithm/MathExtension.cs

@@ -0,0 +1,8 @@
+namespace RobotNet.RobotManager.Services.Simulation.Algorithm;
+
+public class MathExtension
+{
+    public static double ToRad => Math.PI / 180;
+    public static double ToDegree => 180 / Math.PI;
+    public static double CheckLimit(double value, double Max, double Min) => value < Max ? value > Min ? value : Min : Max;
+}

RobotNet.RobotManager/Services/Simulation/Algorithm/PID.cs

@@ -0,0 +1,42 @@
+namespace RobotNet.RobotManager.Services.Simulation.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;
+
+    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 LimitMax, double LimitMin, double TimeSample)
+    {
+        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 = MathExtension.CheckLimit(Out, LimitMax, LimitMin);
+        return Out;
+    }
+}

RobotNet.RobotManager/Services/Simulation/Algorithm/PurePursuit.cs

@@ -0,0 +1,159 @@
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Simulation.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;
+    }
+
+}

RobotNet.RobotManager/Services/Simulation/DifferentialNavigationService.cs

@@ -0,0 +1,169 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Simulation.Algorithm;
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class DifferentialNavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider) : NavigationService(visualization, model, ServiceProvider)
+{
+    public override MessageResult Rotate(double angle)
+    {
+        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
+        TargetAngle = MathExtension.CheckLimit(angle, 180, -180);
+        Action = NavigationAction.Rotate;
+        NavStart();
+        CheckingStart();
+        return new(true);
+    }
+
+    public override MessageResult MoveStraight(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(path);
+        double Angle = Math.Atan2(path[1].Y - path[0].Y, path[1].X - path[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        UpdateGoal(NavigationPath[^1]);
+        FinalGoal = NavigationPath[^1];
+        return new(true);
+    }
+
+    public override MessageResult Move(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        return new(true);
+    }
+
+    public override MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        var pathSplit = PathPlanner.PathSplit(nodes, edges);
+        if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
+        if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
+        {
+            Id = n.Id,
+            X = n.X,
+            Y = n.Y,
+            Theta = n.Theta,
+            Direction = MapCompute.GetRobotDirection(n.Direction),
+            Actions = n.Actions,
+        })];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        InNode = CheckingNodes[0];
+        FinalGoal = CheckingNodes[^1];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+
+        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+        Rotate(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
+        return new(true);
+    }
+
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            if (Action == NavigationAction.Rotate)
+            {
+                if (RotatePID is not null)
+                {
+                    NavigationState = NavigationStateType.Rotating;
+                    double Error = Visualization.Theta - TargetAngle;
+                    if (Error > 180) Error -= 360;
+                    else if (Error < -180) Error += 360;
+                    if (Math.Abs(Error) < 1)
+                    {
+                        if (NavigationPath is not null) Action = NavigationAction.Move;
+                        else Dispose();
+                    }
+                    else
+                    {
+                        var SpeedCal = RotatePID.PID_step(Error * MathExtension.ToRad, AngularVelocity, -AngularVelocity, SampleTimeMilliseconds / 1000.0);
+                        VelocityController.SetSpeed(SpeedCal, SpeedCal);
+                    }
+                }
+            }
+            else if (Action == NavigationAction.Move)
+            {
+                if (NavigationPath is not null && NavigationGoal is not null)
+                {
+                    if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null && FinalGoal is not null)
+                    {
+                        var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - FinalGoal.X, 2) + Math.Pow(Visualization.Y - FinalGoal.Y, 2));
+                        var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - NavigationGoal.X, 2) + Math.Pow(Visualization.Y - NavigationGoal.Y, 2));
+                        var deviation = NavigationGoal.Id == FinalGoal.Id ? 0.02 : 0.1;
+                        if (DistanceToCheckingNode > deviation)
+                        {
+                            NavigationState = NavigationStateType.Moving;
+                            double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, RobotModel.MaxVelocity, 0, SampleTimeMilliseconds / 1000.0);
+                            double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
+                            AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
+                            (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, SampleTimeMilliseconds / 1000.0);
+
+                            //AngularVelocityLeft /= RobotModel.RadiusWheel;
+                            //AngularVelocityRight = AngularVelocityRight / RobotModel.RadiusWheel * -1; 
+                            if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
+                            {
+                                AngularVelocityLeft /= RobotModel.RadiusWheel;
+                                AngularVelocityRight = AngularVelocityRight / RobotModel.RadiusWheel * -1;
+                            }
+                            else
+                            {
+                                AngularVelocityLeft = AngularVelocityLeft / RobotModel.RadiusWheel * -1;
+                                AngularVelocityRight /= RobotModel.RadiusWheel;
+                            }
+                            VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight);
+                            if (MovePurePursuit.OnNodeIndex < NavigationPath.Count)
+                            {
+                                var inNode = NavigationPath[MovePurePursuit.OnNodeIndex];
+                                if (CheckingNodes.Any(n => n.Id == inNode.Id)) InNode = inNode;
+                                else
+                                {
+                                    var inNodeIndex = CheckingNodes.IndexOf(InNode ?? new());
+                                    inNodeIndex = inNodeIndex < 0 ? 0 : inNodeIndex;
+                                    for (int i = inNodeIndex; i < CheckingNodes.Count; i++)
+                                    {
+                                        if (Math.Sqrt(Math.Pow(CheckingNodes[i].X - inNode.X, 2) + Math.Pow(CheckingNodes[i].Y - inNode.Y, 2)) < 0.2)
+                                        {
+                                            InNode = CheckingNodes[i];
+                                            break;
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                        else if (DistanceToGoal < 0.02) Dispose();
+                        else NavigationState = NavigationStateType.Waitting;
+                    }
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{RobotModel.RobotId} Nav ex: {ex.Message}");
+        }
+    }
+
+}

RobotNet.RobotManager/Services/Simulation/ForkliftNavigationSevice.cs

@@ -0,0 +1,140 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Simulation.Algorithm;
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class ForkliftNavigationSevice(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider) : NavigationService(visualization, model, ServiceProvider)
+{
+    public override MessageResult Rotate(double angle)
+    {
+        return new(false, "Robot không có chức năng này");
+    }
+
+    public override MessageResult MoveStraight(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.15).WithPath(path);
+
+        UpdateGoal(NavigationPath[^1]);
+        FinalGoal = NavigationPath[^1];
+        NavStart();
+        return new(true);
+    }
+
+
+    public override MessageResult Move(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.2).WithPath(path);
+
+        NavStart();
+        CheckingStart();
+        return new(true);
+    }
+
+    public override MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        var pathSplit = PathPlanner.PathSplit(nodes, edges);
+        if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
+        if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
+        {
+            Id = n.Id,
+            X = n.X,
+            Y = n.Y,
+            Theta = n.Theta,
+            Direction = MapCompute.GetRobotDirection(n.Direction),
+            Actions = n.Actions,
+        })];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        InNode = CheckingNodes[0];
+        FinalGoal = CheckingNodes[^1];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+
+        NavStart();
+        CheckingStart();
+        return new(true);
+    }
+
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            if (NavigationPath is not null && NavigationGoal is not null)
+            {
+                if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null && FinalGoal is not null)
+                {
+                    var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - FinalGoal.X, 2) + Math.Pow(Visualization.Y - FinalGoal.Y, 2));
+                    var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - NavigationGoal.X, 2) + Math.Pow(Visualization.Y - NavigationGoal.Y, 2));
+                    var deviation = NavigationGoal.Id == FinalGoal.Id ? 0.02 : 0.1;
+                    if (DistanceToCheckingNode > deviation)
+                    {
+                        NavigationState = NavigationStateType.Moving;
+                        double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, RobotModel.MaxVelocity, 0, SampleTimeMilliseconds / 1000.0);
+                        double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
+
+                        AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
+                        (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, SampleTimeMilliseconds / 1000.0);
+
+                        if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
+                        {
+                            AngularVelocityLeft /= RobotModel.RadiusWheel;
+                            AngularVelocityRight = AngularVelocityRight / RobotModel.RadiusWheel * -1;
+                        }
+                        else
+                        {
+                            AngularVelocityLeft = AngularVelocityLeft / RobotModel.RadiusWheel * -1;
+                            AngularVelocityRight /= RobotModel.RadiusWheel;
+                        }
+                        VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight);
+                        if (MovePurePursuit.OnNodeIndex < NavigationPath.Count)
+                        {
+                            var inNode = NavigationPath[MovePurePursuit.OnNodeIndex];
+                            if (CheckingNodes.Any(n => n.Id == inNode.Id)) InNode = inNode;
+                            else
+                            {
+                                var inNodeIndex = CheckingNodes.IndexOf(InNode ?? new());
+                                inNodeIndex = inNodeIndex < 0 ? 0 : inNodeIndex;
+                                for (int i = inNodeIndex; i < CheckingNodes.Count; i++)
+                                {
+                                    if (Math.Sqrt(Math.Pow(CheckingNodes[i].X - inNode.X, 2) + Math.Pow(CheckingNodes[i].Y - inNode.Y, 2)) < 0.2)
+                                    {
+                                        InNode = CheckingNodes[i];
+                                        break;
+                                    }
+                                }
+                            }
+                        }
+                    }
+                    else if (DistanceToGoal < 0.02) Dispose();
+                    else NavigationState = NavigationStateType.Waitting;
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{RobotModel.RobotId} Nav ex: {ex.Message}");
+        }
+    }
+}

RobotNet.RobotManager/Services/Simulation/GridDifferentialNavigationService.cs

@@ -0,0 +1,172 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Simulation.Algorithm;
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class GridDifferentialNavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider) : NavigationService(visualization, model, ServiceProvider)
+{
+    public override MessageResult Rotate(double angle)
+    {
+        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
+        TargetAngle = MathExtension.CheckLimit(angle, 180, -180);
+        Action = NavigationAction.Rotate;
+        NavStart();
+        CheckingStart();
+        return new(true);
+    }
+
+    public override MessageResult MoveStraight(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(path);
+        double Angle = Math.Atan2(path[1].Y - path[0].Y, path[1].X - path[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        UpdateGoal(NavigationPath[^1]);
+        FinalGoal = NavigationPath[^1];
+        return new(true);
+    }
+
+    public override MessageResult Move(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        return new(true);
+    }
+
+    public override MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        var pathNodes = PathPlanner.CalculatorDirection(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
+        var pathSplit = PathPlanner.PathSplit(pathNodes, edges);
+        if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
+        if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");
+
+        var getZones = PathPlanner.GetZones(MapId, nodes);
+        getZones.Wait();
+        Zones = getZones.Result.Data ?? [];
+
+        NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
+        {
+            Id = n.Id,
+            X = n.X,
+            Y = n.Y,
+            Theta = n.Theta,
+            Direction = MapCompute.GetRobotDirection(n.Direction),
+            Actions = n.Actions,
+        })];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        InNode = CheckingNodes[0];
+        FinalGoal = CheckingNodes[^1];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+
+        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+        Rotate(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
+        return new(true);
+    }
+
+    protected override void NavigationHandler()
+    {
+        try
+        {
+            if (Action == NavigationAction.Rotate)
+            {
+                if (RotatePID is not null)
+                {
+                    NavigationState = NavigationStateType.Rotating;
+                    double Error = Visualization.Theta - TargetAngle;
+                    if (Error > 180) Error -= 360;
+                    else if (Error < -180) Error += 360;
+                    if (Math.Abs(Error) < 1)
+                    {
+                        if (NavigationPath is not null) Action = NavigationAction.Move;
+                        else Dispose();
+                    }
+                    else
+                    {
+                        var SpeedCal = RotatePID.PID_step(Error * MathExtension.ToRad, AngularVelocity, -AngularVelocity, SampleTimeMilliseconds / 1000.0);
+                        VelocityController.SetSpeed(SpeedCal, SpeedCal);
+                    }
+                }
+            }
+            else if (Action == NavigationAction.Move)
+            {
+                if (NavigationPath is not null && NavigationGoal is not null)
+                {
+                    if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null && FinalGoal is not null)
+                    {
+                        var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - FinalGoal.X, 2) + Math.Pow(Visualization.Y - FinalGoal.Y, 2));
+                        var DistanceToNavigationGoal = Math.Sqrt(Math.Pow(Visualization.X - NavigationGoal.X, 2) + Math.Pow(Visualization.Y - NavigationGoal.Y, 2));
+                        var deviation = NavigationGoal.Id == FinalGoal.Id ? 0.02 : 0.05;
+                        if (DistanceToNavigationGoal > deviation)
+                        {
+                            NavigationState = NavigationStateType.Moving;
+                            double SpeedTarget = MovePID.PID_step(DistanceToNavigationGoal, RobotModel.MaxVelocity, 0, SampleTimeMilliseconds / 1000.0);
+                            double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
+                            AngularVel *= NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
+                            (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, SampleTimeMilliseconds / 1000.0);
+
+                            if (NavigationPath[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
+                            {
+                                AngularVelocityLeft /= RobotModel.RadiusWheel;
+                                AngularVelocityRight = AngularVelocityRight / RobotModel.RadiusWheel * -1;
+                            }
+                            else
+                            {
+                                AngularVelocityLeft = AngularVelocityLeft / RobotModel.RadiusWheel * -1;
+                                AngularVelocityRight /= RobotModel.RadiusWheel;
+                            }
+                            VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight);
+                            if (MovePurePursuit.OnNodeIndex < NavigationPath.Count)
+                            {
+                                var inNode = NavigationPath[MovePurePursuit.OnNodeIndex];
+                                if (CheckingNodes.Any(n => n.Id == inNode.Id)) InNode = inNode;
+                                else
+                                {
+                                    var inNodeIndex = CheckingNodes.IndexOf(InNode ?? new());
+                                    inNodeIndex = inNodeIndex < 0 ? 0 : inNodeIndex;
+                                    for (int i = inNodeIndex; i < CheckingNodes.Count; i++)
+                                    {
+                                        if (Math.Sqrt(Math.Pow(CheckingNodes[i].X - inNode.X, 2) + Math.Pow(CheckingNodes[i].Y - inNode.Y, 2)) < 0.2)
+                                        {
+                                            InNode = CheckingNodes[i];
+                                            break;
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                        else if (DistanceToGoal < 0.02) Dispose();
+                        else NavigationState = NavigationStateType.Waitting;
+                    }
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{RobotModel.RobotId} Nav ex: {ex.Message}");
+        }
+    }
+}

RobotNet.RobotManager/Services/Simulation/INavigationService.cs

@@ -0,0 +1,18 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public interface INavigationService : IRobotOrder, IDisposable
+{
+    Guid MapId { get; set; }
+    NavigationNode? InNode { get; }
+    List<ZoneDto> CurrentZones { get; set; }
+    NavigationStateType NavigationState { get; }
+    MessageResult Rotate(double angle);
+    MessageResult Move(NavigationNode[] path);
+    MessageResult Move(NodeDto[] nodes, EdgeDto[] edges);
+    MessageResult MoveStraight(NavigationNode[] path);
+    MessageResult Cancel();
+}

RobotNet.RobotManager/Services/Simulation/Models/NavigationAction.cs

@@ -0,0 +1,9 @@
+namespace RobotNet.RobotManager.Services.Simulation.Models;
+
+public enum NavigationAction
+{
+    Rotate,
+    Move,
+    Start,
+    Stop,
+}

RobotNet.RobotManager/Services/Simulation/Models/NavigationNode.cs

@@ -0,0 +1,40 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Simulation.Models;
+
+public class NavigationNode
+{
+    public Guid Id { get; set; }
+    public double X { get; set; }
+    public double Y { get; set; }
+    public double Theta { get; set; }
+    public RobotDirection Direction { get; set; }
+    public string Actions { get; set; } = string.Empty;
+
+    public override bool Equals(object? obj)
+    {
+        if (obj is NavigationNode other)
+            return Id == other.Id;
+        return false;
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Id);
+    }
+
+    public NodeDto ToNodeDto()
+    {
+        return new NodeDto
+        {
+            Id = Id,
+            X = X,
+            Y = Y,
+            Theta = Theta,
+            Direction = MapCompute.GetNodeDirection(Direction),
+            Actions = Actions
+        };
+    }
+}

RobotNet.RobotManager/Services/Simulation/Models/NavigationStateType.cs

@@ -0,0 +1,12 @@
+namespace RobotNet.RobotManager.Services.Simulation.Models;
+
+public enum NavigationStateType
+{
+    None,
+    Ready,
+    Moving,
+    Stop,
+    Error,
+    Rotating,
+    Waitting,
+}

RobotNet.RobotManager/Services/Simulation/Models/RobotSimulationModel.cs

@@ -0,0 +1,16 @@
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Simulation.Models;
+
+public class RobotSimulationModel
+{
+    public string RobotId { get; set; } = string.Empty;
+    public double RadiusWheel { get; set; }
+    public double Width { get; set; }
+    public double Length { get; set; }
+    public double MaxVelocity { get; set; }
+    public double MaxAngularVelocity { get; set; }
+    public double Acceleration { get; set; }
+    public double Deceleration { get; set; }
+    public NavigationType NavigationType { get; set; }
+}

RobotNet.RobotManager/Services/Simulation/NavigationManager.cs

@@ -0,0 +1,15 @@
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class NavigationManager
+{
+    public static INavigationService GetNavigation(VisualizationService Visualization, RobotSimulationModel RobotModel, IServiceProvider ServiceProvider)
+    {
+        if (RobotModel.NavigationType == NavigationType.Forklift) return new ForkliftNavigationSevice(Visualization, RobotModel, ServiceProvider);
+        else if(RobotModel.NavigationType == NavigationType.GridDifferential) return new GridDifferentialNavigationService(Visualization, RobotModel, ServiceProvider);
+        return new DifferentialNavigationService(Visualization, RobotModel, ServiceProvider);
+    }
+}

RobotNet.RobotManager/Services/Simulation/NavigationService.cs

@@ -0,0 +1,655 @@
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.OpenACS;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Simulation.Algorithm;
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public abstract class NavigationService : INavigationService, IDisposable
+{
+    protected readonly VisualizationService Visualization;
+    protected readonly VelocityController VelocityController;
+    protected readonly RobotSimulationModel RobotModel;
+    protected readonly TrafficManager TrafficManager;
+    protected readonly TrafficACS TrafficACS;
+    protected readonly PathPlanner PathPlanner;
+    protected readonly MapManager MapManager;
+    protected readonly LoggerController<NavigationService> Logger;
+
+    private WatchTimer<NavigationService>? navTimer;
+    protected const int SampleTimeMilliseconds = 50;
+
+    private WatchTimerAsync<NavigationService>? checkingTimer;
+    private const int CheckingTimeMilliseconds = 1000;
+
+    protected double TargetAngle = 0;
+    protected PID? RotatePID;
+    protected readonly double AngularVelocity;
+
+    protected PID? MovePID;
+    protected FuzzyLogic? MoveFuzzy;
+    protected PurePursuit? MovePurePursuit;
+
+    private Guid TrafficManagerGoalId = Guid.Empty;
+    private Guid TrafficACSGoalId = Guid.Empty;
+    protected NavigationNode? NavigationGoal;
+    protected NavigationNode? FinalGoal;
+    protected List<NavigationNode>? NavigationPath;
+    protected List<NavigationNode> CheckingNodes = [];
+    private List<NavigationNode> RefreshPath = [];
+    private List<NavigationNode> BaseNodes = [];
+    private bool IsWaittingStop;
+    private double RotateAngle;
+    protected Dictionary<Guid, ZoneDto[]> Zones = [];
+
+    protected NavigationAction Action = NavigationAction.Stop;
+
+    public NavigationNode? InNode { get; set; } = null;
+    public NavigationStateType NavigationState { get; set; } = NavigationStateType.Ready;
+    public TrafficSolutionState TrafficSolutionState { get; private set; } = TrafficSolutionState.None;
+    public bool IsError { get; private set; }
+    public bool IsCompleted { get; private set; } = true;
+    public bool IsProcessing { get; private set; }
+    public bool IsCanceled { get; private set; }
+    public string[] Errors => [];
+    public NavigationPathEdge[] FullPath => GetFullPath();
+    public NavigationPathEdge[] BasePath => GetBasePath();
+    public Guid MapId { get; set; } = Guid.Empty;
+    public List<ZoneDto> CurrentZones { get; set; } = [];
+
+    public NavigationService(VisualizationService visualization, RobotSimulationModel model, IServiceProvider ServiceProvider)
+    {
+        Visualization = visualization;
+        VelocityController = new VelocityController(Visualization).WithDeceleration(model.Deceleration).WithAcceleration(model.Acceleration).WithSampleTime(SampleTimeMilliseconds / 1000.0);
+        RobotModel = model;
+        AngularVelocity = MathExtension.CheckLimit(RobotModel.MaxAngularVelocity * RobotModel.Width / RobotModel.RadiusWheel, 2 * Math.PI, 0);
+        TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
+        TrafficACS = ServiceProvider.GetRequiredService<TrafficACS>();
+        PathPlanner = ServiceProvider.GetRequiredService<PathPlanner>();
+        MapManager = ServiceProvider.GetRequiredService<MapManager>();
+        Logger = ServiceProvider.GetRequiredService<LoggerController<NavigationService>>();
+    }
+
+    public virtual MessageResult Rotate(double angle)
+    {
+        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
+        TargetAngle = MathExtension.CheckLimit(angle, 180, -180);
+        Action = NavigationAction.Rotate;
+        NavStart();
+        CheckingStart();
+        return new(true);
+    }
+
+    public virtual MessageResult MoveStraight(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [NavigationPath[0], NavigationPath[^1]];
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath(path);
+        double Angle = Math.Atan2(path[1].Y - path[0].Y, path[1].X - path[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        return new(true);
+    }
+
+    public virtual MessageResult Move(NavigationNode[] path)
+    {
+        if (path.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        NavigationPath = [.. path];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        if (CheckingNodes.Count < 2) return new(false, "Đường dẫn traffic không hợp lệ");
+        InNode = CheckingNodes[0];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
+        Rotate(Angle * 180 / Math.PI);
+        return new(true);
+    }
+
+    public virtual MessageResult Move(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        if (nodes.Length < 2) return new(false, "Đường dẫn không hợp lệ");
+        var pathNodes = PathPlanner.CalculatorDirection(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
+        var pathSplit = PathPlanner.PathSplit(pathNodes, edges);
+        if (!pathSplit.IsSuccess) return new(false, pathSplit.Message);
+        if (pathSplit.Data is null) return new(false, "Đường dẫn không hợp lệ");
+
+        var getZones = PathPlanner.GetZones(MapId, nodes);
+        getZones.Wait();
+        Zones = getZones.Result.Data ?? [];
+
+        NavigationPath = [..pathSplit.Data.Select(n => new NavigationNode()
+        {
+            Id = n.Id,
+            X = n.X,
+            Y = n.Y,
+            Theta = n.Theta,
+            Direction = MapCompute.GetRobotDirection(n.Direction),
+            Actions = n.Actions,
+        })];
+        CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+        InNode = CheckingNodes[0];
+        FinalGoal = CheckingNodes[^1];
+
+        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
+        MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
+        MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
+
+        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+        Rotate(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
+        return new(true);
+    }
+
+    public MessageResult Cancel()
+    {
+        Dispose();
+        VelocityController.Stop();
+        NavigationState = NavigationStateType.Ready;
+        IsCanceled = true;
+        return new(true);
+    }
+
+    protected virtual void NavigationHandler()
+    {
+    }
+
+    private void HandleRefreshPath(TrafficSolution trafficSolution)
+    {
+        var edgesDto = trafficSolution.Edges.Select(e => new EdgeDto
+        {
+            Id = e.Id,
+            StartNodeId = e.StartNodeId,
+            EndNodeId = e.EndNodeId,
+            TrajectoryDegree = e.TrajectoryDegree,
+            ControlPoint1X = e.ControlPoint1X,
+            ControlPoint1Y = e.ControlPoint1Y,
+            ControlPoint2X = e.ControlPoint2X,
+            ControlPoint2Y = e.ControlPoint2Y,
+        });
+        List<NodeDto> nodesDto = [..trafficSolution.Nodes.Select(n => new NodeDto
+                    {
+                        Id = n.Id,
+                        X = n.X,
+                        Y = n.Y,
+                        Name = n.Name,
+                        Direction = MapCompute.GetNodeDirection(n.Direction),
+                    })];
+        var splitPath = PathPlanner.PathSplit([.. nodesDto], [.. edgesDto]);
+
+        if (splitPath.IsSuccess)
+        {
+            if (splitPath.Data != null)
+            {
+                RefreshPath = [..splitPath.Data.Select(n => new NavigationNode()
+                            {
+                                Id = n.Id,
+                                X = n.X,
+                                Y = n.Y,
+                                Theta = n.Theta,
+                                Direction = MapCompute.GetRobotDirection(n.Direction),
+                                Actions = n.Actions,
+                            })];
+            }
+        }
+    }
+
+    private void HandleCompleteState(TrafficSolution trafficSolution)
+    {
+        if (trafficSolution.State == TrafficSolutionState.Complete && RefreshPath.Count > 0)
+        {
+            NavigationPath = RefreshPath;
+            CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+            if (MovePurePursuit is not null)
+            {
+                MovePurePursuit.UpdatePath([.. NavigationPath]);
+                MovePurePursuit.OnNodeIndex = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y).index;
+            }
+            RefreshPath = [];
+
+            double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+            double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+            TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
+            Action = NavigationAction.Rotate;
+        }
+    }
+
+    private void HandleGivewayState(TrafficSolution trafficSolution)
+    {
+        if (trafficSolution.GivewayNodes[^1].Id == trafficSolution.ReleaseNode.Id && NavigationGoal?.Id != trafficSolution.ReleaseNode.Id)
+        {
+            var splitPath = PathPlanner.PathSplit([..trafficSolution.GivewayNodes.Select(n => new NodeDto
+                        {
+                            Id = n.Id,
+                            X = n.X,
+                            Y = n.Y,
+                            Name = n.Name,
+                            Direction = MapCompute.GetNodeDirection(n.Direction),
+                        })], [.. trafficSolution.GivewayEdges.Select(e => new EdgeDto
+                        {
+                            Id = e.Id,
+                            StartNodeId = e.StartNodeId,
+                            EndNodeId = e.EndNodeId,
+                            TrajectoryDegree = e.TrajectoryDegree,
+                            ControlPoint1X = e.ControlPoint1X,
+                            ControlPoint1Y = e.ControlPoint1Y,
+                            ControlPoint2X = e.ControlPoint2X,
+                            ControlPoint2Y = e.ControlPoint2Y,
+                        })]);
+            if (splitPath.IsSuccess && splitPath.Data != null && MovePurePursuit != null)
+            {
+                NavigationPath = [..splitPath.Data.Select(n => new NavigationNode()
+                                {
+                                    Id = n.Id,
+                                    X = n.X,
+                                    Y = n.Y,
+                                    Theta = n.Theta,
+                                    Direction = MapCompute.GetRobotDirection(n.Direction),
+                                    Actions = n.Actions,
+                                })];
+                CheckingNodes = [.. NavigationPath.Where(n => n.Actions == "CheckingNode")];
+                MovePurePursuit.UpdatePath([.. NavigationPath]);
+                MovePurePursuit.OnNodeIndex = 0;
+
+                double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+                double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+                TargetAngle = MathExtension.CheckLimit(CheckingNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward, 180, -180);
+                Action = NavigationAction.Rotate;
+            }
+        }
+    }
+
+    private async Task<Guid> RequestInACS(Dictionary<Guid, ZoneDto[]> newZones)
+    {
+        Guid trafficACSrelaseNodeId = Guid.Empty;
+        foreach (var zone in newZones)
+        {
+            if (zone.Value.Length == 0) trafficACSrelaseNodeId = zone.Key;
+            else
+            {
+                bool requestSuccess = true;
+                foreach (var zoneACS in zone.Value)
+                {
+                    if (string.IsNullOrEmpty(zoneACS.Name)) continue;
+                    if (CurrentZones.Any(z => z.Id == zoneACS.Id)) continue;
+
+                    var getInTrafficACS = await TrafficACS.RequestIn(RobotModel.RobotId, zoneACS.Name);
+                    if (getInTrafficACS.IsSuccess && getInTrafficACS.Data) CurrentZones.Add(zoneACS);
+                    else
+                    {
+                        requestSuccess = false;
+                        break;
+                    }
+                }
+                if (requestSuccess) trafficACSrelaseNodeId = zone.Key;
+                else break;
+            }
+        }
+        return trafficACSrelaseNodeId;
+    }
+
+    private void UpdateTraffic()
+    {
+        var trafficManagerGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficManagerGoalId);
+        var trafficACSGoalIndex = CheckingNodes.FindIndex(n => n.Id == TrafficACSGoalId);
+        if (trafficManagerGoalIndex != -1 && trafficACSGoalIndex != -1)
+        {
+            var goalIndex = Math.Min(trafficManagerGoalIndex, trafficACSGoalIndex);
+            NavigationNode goal = CheckingNodes[goalIndex];
+            var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == (InNode is null ? Guid.Empty : InNode.Id));
+            inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
+            if (BaseNodes.Count == 0 || BaseNodes[^1].Id != goal.Id) BaseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
+
+            if (IsWaittingStop)
+            {
+                if (NavigationState == NavigationStateType.Waitting)
+                {
+                    TargetAngle = MathExtension.CheckLimit(RotateAngle, 180, -180);
+                    Action = NavigationAction.Rotate;
+                    IsWaittingStop = false;
+                }
+            }
+            else
+            {
+                for (int i = inNodeIndex + 1; i <= goalIndex; i++)
+                {
+                    if (Math.Abs(CheckingNodes[i].Theta - CheckingNodes[i - 1].Theta) > 30)
+                    {
+                        goal = CheckingNodes[i];
+                        IsWaittingStop = true;
+                        RotateAngle = CheckingNodes[i].Theta;
+                        break;
+                    }
+                }
+                UpdateGoal(goal);
+            }
+        }
+    }
+
+    private async Task GoOutTrafficACS()
+    {
+        if (BaseNodes is null || BaseNodes.Count == 0 || InNode is null) return;
+        var goalIndex = CheckingNodes.FindIndex(n => n.Id == BaseNodes[^1].Id);
+        if (goalIndex == -1) return;
+        var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
+        inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0;
+        if (goalIndex <= inNodeIndex) return;
+        var baseNodes = CheckingNodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex);
+        var baseZones = PathPlanner.GetZones([..baseNodes.Select(n => n.ToNodeDto())], Zones);
+        var outZones = CurrentZones.Where(z => !baseZones.Any(bz => bz.Id == z.Id)).ToList();
+        foreach (var zoneACS in outZones)
+        {
+            if (string.IsNullOrEmpty(zoneACS.Name)) continue;
+            var outTrafficACS = await TrafficACS.RequestOut(RobotModel.RobotId, zoneACS.Name);
+            if (outTrafficACS.IsSuccess && outTrafficACS.Data) CurrentZones.RemoveAll(z => z.Id == zoneACS.Id);
+        }
+    }
+
+    protected virtual async Task CheckingHandler()
+    {
+        try
+        {
+            if (NavigationPath is null || InNode is null) throw new Exception("Đường đi không tồn tại");
+
+            await GoOutTrafficACS();
+
+            if (!TrafficManager.Enable)
+            {
+                if (TrafficManagerGoalId != CheckingNodes[^1].Id) TrafficManagerGoalId = CheckingNodes[^1].Id;
+            }
+            else
+            {
+                TrafficManager.UpdateInNode(RobotModel.RobotId, InNode.Id);
+                var trafficSolution = TrafficManager.GetTrafficNode(RobotModel.RobotId);
+                if (trafficSolution.State != TrafficSolutionState.RefreshPath && TrafficSolutionState == TrafficSolutionState.RefreshPath && trafficSolution.Edges.Length > 0 && trafficSolution.Nodes.Length > 1)
+                {
+                    HandleRefreshPath(trafficSolution);
+                }
+                if (trafficSolution.State == TrafficSolutionState.Complete || trafficSolution.State == TrafficSolutionState.Waitting)
+                {
+                    HandleCompleteState(trafficSolution);
+                }
+                else if (trafficSolution.State == TrafficSolutionState.GiveWay && trafficSolution.GivewayNodes.Count > 1 && trafficSolution.GivewayEdges.Count > 0)
+                {
+                    HandleGivewayState(trafficSolution);
+                }
+                var goal = CheckingNodes.FirstOrDefault(n => n.Id == trafficSolution.ReleaseNode.Id);
+                if (goal is not null && goal.Id != TrafficManagerGoalId) TrafficManagerGoalId = goal.Id;
+                TrafficSolutionState = trafficSolution.State;
+            }
+
+            if (!TrafficACS.Enable)
+            {
+                if (TrafficACSGoalId != CheckingNodes[^1].Id) TrafficACSGoalId = CheckingNodes[^1].Id;
+            }
+            else
+            {
+                var newZones = TrafficACS.GetZones(InNode?.Id ?? Guid.Empty, [.. CheckingNodes.Select(n => n.ToNodeDto())], Zones);
+                var trafficACSrelaseNodeId = await RequestInACS(newZones);
+                if (trafficACSrelaseNodeId != Guid.Empty && trafficACSrelaseNodeId != TrafficACSGoalId) TrafficACSGoalId = trafficACSrelaseNodeId;
+            }
+            UpdateTraffic();
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{RobotModel.RobotId} Checking ex: {ex.Message}");
+        }
+    }
+
+    private NavigationPathEdge[] GetBasePath()
+    {
+        if (InNode is not null && BaseNodes is not null && BaseNodes.Count > 1)
+        {
+            var inNodeIndex = BaseNodes.FindIndex(n => n.Id == InNode.Id);
+            if (inNodeIndex != -1 && inNodeIndex < BaseNodes.Count - 1)
+            {
+                var nodes = BaseNodes.GetRange(inNodeIndex, BaseNodes.Count - inNodeIndex);
+                var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
+                {
+                    Id = n.Id,
+                })]);
+                if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
+                {
+                    List<NavigationPathEdge> pathEdges = [];
+                    var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
+                    if (edge is null)
+                    {
+                        pathEdges.Add(new()
+                        {
+                            StartX = Visualization.X,
+                            StartY = Visualization.Y,
+                            EndX = nodes[1].X,
+                            EndY = nodes[1].Y,
+                            Degree = 1,
+                        });
+                    }
+                    else
+                    {
+                        var splitStartPath = PathPlanner.PathSplit([new NodeDto
+                        {
+                            Id = nodes[0].Id,
+                            X = nodes[0].X,
+                            Y = nodes[0].Y,
+                        }, new NodeDto
+                        {
+                            Id = nodes[1].Id,
+                            X = nodes[1].X,
+                            Y = nodes[1].Y,
+                            Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
+                        }], [edge]);
+                        if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
+                        {
+                            int index = 0;
+                            double minDistance = double.MaxValue;
+                            for (int i = 0; i < splitStartPath.Data.Length; i++)
+                            {
+                                var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
+                                if (distance < minDistance)
+                                {
+                                    minDistance = distance;
+                                    index = i;
+                                }
+                            }
+                            for (int i = index; i < splitStartPath.Data.Length - 1; i++)
+                            {
+                                pathEdges.Add(new()
+                                {
+                                    StartX = splitStartPath.Data[i].X,
+                                    StartY = splitStartPath.Data[i].Y,
+                                    EndX = splitStartPath.Data[i + 1].X,
+                                    EndY = splitStartPath.Data[i + 1].Y,
+                                    Degree = 1,
+                                });
+                            }
+                        }
+                    }
+                    for (int i = 1; i < nodes.Count - 1; i++)
+                    {
+                        pathEdges.Add(new()
+                        {
+                            StartX = nodes[i].X,
+                            StartY = nodes[i].Y,
+                            EndX = nodes[i + 1].X,
+                            EndY = nodes[i + 1].Y,
+                            ControlPoint1X = edges[i].ControlPoint1X,
+                            ControlPoint1Y = edges[i].ControlPoint1Y,
+                            ControlPoint2X = edges[i].ControlPoint2X,
+                            ControlPoint2Y = edges[i].ControlPoint2Y,
+                            Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
+                        });
+                    }
+                    return [.. pathEdges];
+                }
+            }
+        }
+        return [];
+    }
+
+    private NavigationPathEdge[] GetFullPath()
+    {
+        if (InNode is not null && CheckingNodes is not null && CheckingNodes.Count > 1)
+        {
+            var inNodeIndex = CheckingNodes.FindIndex(n => n.Id == InNode.Id);
+            if (inNodeIndex != -1 && inNodeIndex < CheckingNodes.Count - 1)
+            {
+                var nodes = CheckingNodes.GetRange(inNodeIndex, CheckingNodes.Count - inNodeIndex);
+                var edges = MapManager.GetEdges(MapId, [..nodes.Select(n => new NodeDto
+                {
+                    Id = n.Id,
+                })]);
+                if (edges.Length == nodes.Count - 1 && nodes.Count > 1)
+                {
+                    List<NavigationPathEdge> pathEdges = [];
+                    var edge = MapManager.GetEdge(nodes[0].Id, nodes[1].Id, MapId);
+                    if (edge is null)
+                    {
+                        pathEdges.Add(new()
+                        {
+                            StartX = Visualization.X,
+                            StartY = Visualization.Y,
+                            EndX = nodes[1].X,
+                            EndY = nodes[1].Y,
+                            Degree = 1,
+                        });
+                    }
+                    else
+                    {
+                        var splitStartPath = PathPlanner.PathSplit([new NodeDto
+                        {
+                            Id = nodes[0].Id,
+                            X = nodes[0].X,
+                            Y = nodes[0].Y,
+                        }, new NodeDto
+                        {
+                            Id = nodes[1].Id,
+                            X = nodes[1].X,
+                            Y = nodes[1].Y,
+                            Direction = MapCompute.GetNodeDirection(nodes[0].Direction),
+                        }], [edge]);
+                        if (splitStartPath.IsSuccess && splitStartPath.Data is not null)
+                        {
+                            int index = 0;
+                            double minDistance = double.MaxValue;
+                            for (int i = 0; i < splitStartPath.Data.Length; i++)
+                            {
+                                var distance = Math.Sqrt(Math.Pow(Visualization.X - splitStartPath.Data[i].X, 2) + Math.Pow(Visualization.Y - splitStartPath.Data[i].Y, 2));
+                                if (distance < minDistance)
+                                {
+                                    minDistance = distance;
+                                    index = i;
+                                }
+                            }
+                            for (int i = index; i < splitStartPath.Data.Length - 1; i++)
+                            {
+                                pathEdges.Add(new()
+                                {
+                                    StartX = splitStartPath.Data[i].X,
+                                    StartY = splitStartPath.Data[i].Y,
+                                    EndX = splitStartPath.Data[i + 1].X,
+                                    EndY = splitStartPath.Data[i + 1].Y,
+                                    Degree = 1,
+                                });
+                            }
+                        }
+                    }
+                    for (int i = 1; i < nodes.Count - 1; i++)
+                    {
+                        pathEdges.Add(new()
+                        {
+                            StartX = nodes[i].X,
+                            StartY = nodes[i].Y,
+                            EndX = nodes[i + 1].X,
+                            EndY = nodes[i + 1].Y,
+                            ControlPoint1X = edges[i].ControlPoint1X,
+                            ControlPoint1Y = edges[i].ControlPoint1Y,
+                            ControlPoint2X = edges[i].ControlPoint2X,
+                            ControlPoint2Y = edges[i].ControlPoint2Y,
+                            Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3,
+                        });
+                    }
+                    return [.. pathEdges];
+                }
+            }
+        }
+        return [];
+    }
+
+    public bool UpdateGoal(NavigationNode goal)
+    {
+        if (NavigationGoal is null || NavigationGoal.Id != goal.Id)
+        {
+            if (NavigationPath is not null && NavigationPath.Any(node => node.Id == goal.Id))
+            {
+                NavigationGoal = goal;
+                MovePurePursuit?.UpdateGoal(goal);
+                return true;
+            }
+            return false;
+        }
+        return true;
+    }
+
+    private void ClearPath()
+    {
+        NavigationPath = null;
+        NavigationGoal = null;
+        InNode = null;
+        CheckingNodes.Clear();
+        TrafficManager.DeleteAgent(RobotModel.RobotId);
+    }
+
+    public void Dispose()
+    {
+        CheckingStop();
+        NavStop();
+        ClearPath();
+        NavigationState = NavigationStateType.Ready;
+        VelocityController.Stop();
+        IsCompleted = true;
+        IsProcessing = false;
+        GC.SuppressFinalize(this);
+    }
+
+    protected void NavStart()
+    {
+        navTimer = new(SampleTimeMilliseconds, NavigationHandler, Logger);
+        navTimer.Start();
+        IsCompleted = false;
+        IsCanceled = false;
+        IsError = false;
+        IsProcessing = true;
+    }
+
+    protected void NavStop()
+    {
+        navTimer?.Dispose();
+    }
+
+    protected void CheckingStart()
+    {
+        checkingTimer = new(CheckingTimeMilliseconds, CheckingHandler, Logger);
+        checkingTimer.Start();
+    }
+
+    public void CreateComledted()
+    {
+        IsCompleted = true;
+        IsCanceled = false;
+        IsError = false;
+    }
+
+    protected void CheckingStop()
+    {
+        checkingTimer?.Dispose();
+    }
+}

RobotNet.RobotManager/Services/Simulation/RobotSimulation.cs

@@ -0,0 +1,340 @@
+using Microsoft.EntityFrameworkCore;
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotManager.Services.Robot;
+using RobotNet.RobotManager.Services.Simulation.Models;
+using RobotNet.RobotManager.Services.Traffic;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.VDA5050.Factsheet;
+using RobotNet.RobotShares.VDA5050.FactsheetExtend;
+using RobotNet.RobotShares.VDA5050.State;
+using RobotNet.RobotShares.VDA5050.Visualization;
+using RobotNet.Shares;
+using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class RobotSimulation : IRobotController, IDisposable
+{
+    public string SerialNumber { get; }
+    public bool IsOnline { get; set; } = true;
+    public bool IsWorking => NavigationService is not null && NavigationService.IsProcessing;
+    public string State { get; } = "Simulation";
+    public AutoResetEvent RobotUpdated { get; set; } = new(false);
+    public string[] CurrentZones => NavigationService is null ? [] : [.. NavigationService.CurrentZones.Select(z => z.Name)];
+    public StateMsg StateMsg { get => GetState(); set { } }
+    public VisualizationMsg VisualizationMsg { get => GetVisualization(); set { } }
+    public FactSheetMsg FactSheetMsg { get; set; } = new();
+    public FactsheetExtendMsg FactsheetExtendMsg { get; set; } = new();
+    public VisualizationService Visualization { get; set; }
+    public NavigationPathEdge[] BasePath => NavigationService is null ? [] : NavigationService.BasePath;
+    public NavigationPathEdge[] FullPath => NavigationService is null ? [] : NavigationService.FullPath;
+    public RobotOrderDto OrderState => GetOrderState();
+    public RobotActionDto[] ActionStates => [];
+
+    private INavigationService? NavigationService;
+    private readonly IServiceProvider ServiceProvider;
+    private readonly LoggerController<RobotSimulation> Logger;
+    private readonly RobotSimulationModel RobotSimulationModel;
+    private CancellationTokenSource? CancelRandom;
+
+    public RobotSimulation(string robotid, RobotModel model, IServiceProvider serviceProvider)
+    {
+        SerialNumber = robotid;
+        RobotSimulationModel = new RobotSimulationModel()
+        {
+            RobotId = robotid,
+            Acceleration = 2,
+            Deceleration = 1,
+            Length = model.Length / 2,
+            Width = model.Width,
+            MaxAngularVelocity = 0.3,
+            MaxVelocity = 1.5,
+            RadiusWheel = 0.1,
+            NavigationType = model.NavigationType,
+        };
+        Visualization = new VisualizationService().WithRadiusWheel(RobotSimulationModel.RadiusWheel).WithRadiusRobot(RobotSimulationModel.Width);
+        ServiceProvider = serviceProvider;
+        Logger = ServiceProvider.GetRequiredService<LoggerController<RobotSimulation>>();
+    }
+
+    public void Initialize(double x, double y, double theta) => Visualization.LocalizationInitialize(x, y, theta);
+
+    public MessageResult Rotate(double angle)
+    {
+        if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
+        var olderNavigationService = NavigationService;
+        NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
+        if (NavigationService is not null)
+        {
+            NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
+            return NavigationService.Rotate(angle);
+        }
+        return new(false, "Model chưa được thiết lập");
+    }
+
+    public MessageResult MoveStraight(double x, double y)
+    {
+        try
+        {
+            var scope = ServiceProvider.CreateAsyncScope();
+            var PathPlanningService = scope.ServiceProvider.GetRequiredService<PathPlanner>();
+            var headRobotNode = new NodeDto()
+            {
+                Id = Guid.NewGuid(),
+                X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
+                Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
+            };
+            var currentRobotNode = new NodeDto()
+            {
+                Id = Guid.NewGuid(),
+                X = Visualization.X,
+                Y = Visualization.Y,
+            };
+            var goalNode = new NodeDto()
+            {
+                Id = Guid.NewGuid(),
+                X = x,
+                Y = y,
+            };
+            goalNode.Theta = MapEditorHelper.GetAngle(currentRobotNode, headRobotNode, goalNode) > 90 ? Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) : Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X);
+            currentRobotNode.Theta = goalNode.Theta;
+            NodeDto[] nodesplanning = [currentRobotNode, goalNode];
+            EdgeDto[] edgesplanning = [new()
+            {
+                Id= Guid.NewGuid(),
+                DirectionAllowed = DirectionAllowed.Both,
+                TrajectoryDegree = TrajectoryDegree.One,
+                StartNodeId = currentRobotNode.Id,
+                EndNodeId = goalNode.Id
+            }];
+            var resultSplit = PathPlanningService.PathSplit(nodesplanning, edgesplanning);
+            if (!resultSplit.IsSuccess) return new(false, resultSplit.Message);
+            if (resultSplit.Data is null || resultSplit.Data.Length < 1) return new(false, "Không tìm thấy đường dẫn tới đích");
+            if (resultSplit.Data.Length < 1) return new(false, "Dữ liệu truyền vào không đúng");
+            var navigationPath = resultSplit.Data.Select(n => new NavigationNode()
+            {
+                Id = Guid.NewGuid(),
+                X = n.X,
+                Y = n.Y,
+                Theta = n.Theta,
+                Direction = n.Direction == Direction.FORWARD ? RobotShares.Enums.RobotDirection.FORWARD : n.Direction == Direction.BACKWARD ? RobotShares.Enums.RobotDirection.BACKWARD : RobotShares.Enums.RobotDirection.NONE,
+                Actions = n.Actions,
+            }).ToArray();
+
+            if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
+            var olderNavigationService = NavigationService;
+            NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
+            if (NavigationService is not null)
+            {
+                NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
+                var moveTask = NavigationService.MoveStraight(navigationPath);
+                return moveTask;
+            }
+            return new(false, "Model chưa được thiết lập");
+        }
+        catch (Exception ex)
+        {
+            return new(false, ex.Message);
+        }
+    }
+
+    public Task<MessageResult> CancelOrder()
+    {
+        NavigationService?.Cancel();
+        CancelRandom?.Cancel();
+        return Task.FromResult<MessageResult>(new(true));
+    }
+
+    public async Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<Action>>? actions = null, double? lastAngle = null)
+    {
+        try
+        {
+            var scope = ServiceProvider.CreateAsyncScope();
+            var PathPlanner = scope.ServiceProvider.GetRequiredService<PathPlanner>();
+            var AppDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            var TrafficManager = ServiceProvider.GetRequiredService<TrafficManager>();
+            var robotDb = await AppDb.Robots.FirstOrDefaultAsync(r => r.RobotId == SerialNumber);
+            if (robotDb is null) return new(false, "RobotId không tồn tại");
+            var robotModelDb = await AppDb.RobotModels.FirstOrDefaultAsync(r => r.Id == robotDb.ModelId);
+            if (robotModelDb is null) return new(false, "Robot Model không tồn tại");
+
+            var path = await PathPlanner.Planning(Visualization.X, Visualization.Y, Visualization.Theta, RobotSimulationModel.NavigationType, robotDb.MapId, goalName);
+            if (!path.IsSuccess) return new(false, path.Message);
+            if (path.IsSuccess && path.Data.Nodes.Length < 2)
+            {
+                NavigationService?.CreateComledted();
+                return new(true, "");
+            }
+            if (path.Data.Nodes is null || path.Data.Edges is null) return new(false, $"Đường dẫn tới đích {goalName} từ [{Visualization.X} - {Visualization.Y} - {Visualization.Theta}] không tồn tại");
+
+            if (NavigationService is not null && NavigationService.NavigationState != NavigationStateType.Ready) return new(false, "Robot chưa sẵn sàng thực hiện nhiệm vụ");
+            var olderNavigationService = NavigationService;
+            NavigationService = NavigationManager.GetNavigation(Visualization, RobotSimulationModel, ServiceProvider);
+            if (NavigationService is not null)
+            {
+                var nodes = path.Data.Nodes;
+                var createAgent = TrafficManager.CreateAgent(robotDb.MapId, this, new()
+                {
+                    NavigationType = robotModelDb.NavigationType,
+                    Length = robotModelDb.Length,
+                    Width = robotModelDb.Width,
+                    NavigationPointX = robotModelDb.OriginX,
+                    NavigationPointY = robotModelDb.OriginY,
+                },
+                [..nodes.Select(n => new TrafficNodeDto()
+                {
+                    Id = n.Id,
+                    Name = n.Name,
+                    X = n.X,
+                    Y = n.Y,
+                    Theta = n.Theta,
+                    Direction = MapCompute.GetRobotDirection(n.Direction),
+                })], [..path.Data.Edges.Select(n => new TrafficEdgeDto()
+                {
+                    Id = n.Id,
+                    StartNodeId = n.StartNodeId,
+                    EndNodeId = n.EndNodeId,
+                    TrajectoryDegree = n.TrajectoryDegree,
+                    ControlPoint1X = n.ControlPoint1X,
+                    ControlPoint1Y = n.ControlPoint1Y,
+                    ControlPoint2X = n.ControlPoint2X,
+                    ControlPoint2Y = n.ControlPoint2Y,
+                })]);
+                if (!createAgent.IsSuccess)
+                {
+                    Logger.Warning($"{SerialNumber} - Không thể tạo traffic agent: {createAgent.Message}");
+                    return new(false, $"Không thể tạo traffic agent: {createAgent.Message}");
+                }
+                NavigationService.MapId = robotDb.MapId;
+                NavigationService.CurrentZones = olderNavigationService is null ? [] : olderNavigationService.CurrentZones;
+                var moveTask = NavigationService.Move(nodes, path.Data.Edges);
+                return moveTask;
+            }
+            return new(false, "Model chưa được thiết lập");
+        }
+        catch (Exception ex)
+        {
+            return new(false, ex.Message);
+        }
+    }
+
+    private VisualizationMsg GetVisualization()
+    {
+        var scope = ServiceProvider.CreateAsyncScope();
+        var AppDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+        var robotDb = AppDb.Robots.FirstOrDefault(r => r.RobotId == SerialNumber);
+        return new()
+        {
+            SerialNumber = SerialNumber,
+            MapId = robotDb is null ? string.Empty : robotDb.MapId.ToString(),
+            AgvPosition = new()
+            {
+                X = Visualization.X,
+                Y = Visualization.Y,
+                Theta = Visualization.Theta,
+                PositionInitialized = true,
+                LocalizationScore = 100,
+                DeviationRange = 100,
+            },
+            Velocity = new()
+            {
+                Vx = Visualization.Vx,
+                Vy = Visualization.Vy,
+                Omega = Visualization.Omega,
+            }
+        };
+    }
+
+    private StateMsg GetState()
+    {
+        return new()
+        {
+            SerialNumber = SerialNumber,
+            Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
+            NewBaseRequest = true,
+            BatteryState = new()
+            {
+                BatteryHealth = 100,
+                BatteryCharge = 0,
+                BatteryVoltage = 24,
+                Charging = false,
+            },
+            ActionStates = [],
+            Errors = [],
+            Information = [],
+            NodeStates = [],
+            Loads = [],
+        };
+    }
+
+    public Task<MessageResult<string>> InstantAction(Action action, bool waittingFeedback)
+    {
+        return Task.FromResult<MessageResult<string>>(new(true));
+    }
+
+    public void Log(string message, LogLevel level = LogLevel.Information)
+    {
+        switch (level)
+        {
+            case LogLevel.Trace: Logger.Trace($"{SerialNumber} - {message}"); break;
+            case LogLevel.Error: Logger.Error($"{SerialNumber} - {message}"); break;
+            case LogLevel.Warning: Logger.Warning($"{SerialNumber} - {message}"); break;
+            case LogLevel.Critical: Logger.Critical($"{SerialNumber} - {message}"); break;
+            case LogLevel.Information: Logger.Info($"{SerialNumber} - {message}"); break;
+            case LogLevel.Debug: Logger.Debug($"{SerialNumber} - {message}"); break;
+            default: Logger.Debug($"{SerialNumber} - {message}"); break;
+        }
+    }
+
+    public Task<MessageResult> MoveRandom(List<string> nodes)
+    {
+        try
+        {
+            CancelRandom = new CancellationTokenSource();
+            var random = new Random();
+            var randomTask = Task.Run(async () =>
+            {
+                while (!CancelRandom.IsCancellationRequested)
+                {
+                    var index = random.Next(nodes.Count);
+                    await MoveToNode(nodes[index]);
+                    while (!CancelRandom.IsCancellationRequested)
+                    {
+                        if (!IsWorking) break;
+                        await Task.Delay(1000);
+                    }
+                    await Task.Delay(2000);
+                }
+            }, cancellationToken: CancelRandom.Token);
+        }
+        catch { }
+        return Task.FromResult<MessageResult>(new(true));
+    }
+
+    public Task<MessageResult> CancelAction()
+    {
+        return Task.FromResult<MessageResult>(new(true, ""));
+    }
+
+    public RobotOrderDto GetOrderState()
+    {
+        return new()
+        {
+            OrderId = StateMsg.OrderId,
+            IsCompleted = NavigationService is not null && NavigationService.IsCompleted,
+            IsError = NavigationService is not null && NavigationService.IsError,
+            IsProcessing = NavigationService is not null && NavigationService.IsProcessing,
+            IsCanceled = NavigationService is not null && NavigationService.IsCanceled,
+            Errors = NavigationService is not null ? NavigationService.Errors : [],
+        };
+    }
+
+    public void Dispose()
+    {
+        GC.SuppressFinalize(this);
+    }
+}

RobotNet.RobotManager/Services/Simulation/VelocityController.cs

@@ -0,0 +1,75 @@
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class VelocityController(VisualizationService Visualization)
+{
+    private double Acceleration = 2;
+    private double Deceleration = 10;
+    private double AngularVelLeft;
+    private double AngularVelRight;
+    private double SampleTime = 0.05;
+
+    public VelocityController WithAcceleration(double acc)
+    {
+        Acceleration = acc;
+        return this;
+    }
+
+    public VelocityController WithDeceleration(double dec)
+    {
+        Deceleration = dec;
+        return this;
+    }
+
+    public VelocityController WithSampleTime(double time)
+    {
+        SampleTime = time;
+        return this;
+    }
+
+    private (double angularVelLeft, double angularVelRight) AccelerationCalculator(double wL, double wR, double wL_Current, double wR_Current)
+    {
+        var angularVelLeft = wL_Current;
+        var angularVelRight = wR_Current;
+        if (wL_Current == 0 || wL / wL_Current < 0)
+        {
+            if (wL != 0) angularVelLeft += wL / Math.Abs(wL) * Acceleration;
+            else angularVelLeft = wL;
+        }
+        else
+        {
+            if (Math.Abs(wL) - Math.Abs(wL_Current) > Acceleration) angularVelLeft += Acceleration * wL_Current / Math.Abs(wL_Current);
+            else if (Math.Abs(wL_Current) - Math.Abs(wL) > Deceleration) angularVelLeft -= Deceleration * wL_Current / Math.Abs(wL_Current);
+            else angularVelLeft = wL;
+        }
+
+        if (wR_Current == 0 || wR / wR_Current < 0)
+        {
+            if (wR != 0) angularVelRight += wR / Math.Abs(wR) * Acceleration;
+            else angularVelRight = wR;
+        }
+        else
+        {
+            if (Math.Abs(wR) - Math.Abs(wR_Current) > Acceleration) angularVelRight += Acceleration * wR_Current / Math.Abs(wR_Current);
+            else if (Math.Abs(wR_Current) - Math.Abs(wR) > Deceleration) angularVelRight -= Deceleration * wR_Current / Math.Abs(wR_Current);
+            else angularVelRight = wR;
+        }
+
+        if (Math.Abs(angularVelLeft) > Math.Abs(wL)) angularVelLeft = wL;
+        if (Math.Abs(angularVelRight) > Math.Abs(wR)) angularVelRight = wR;
+        return (angularVelLeft, angularVelRight);
+    }
+
+    public bool SetSpeed(double angularVelLeft, double angularVelRight)
+    {
+        (AngularVelLeft, AngularVelRight) = AccelerationCalculator(angularVelLeft, angularVelRight, AngularVelLeft, AngularVelRight);
+        //Console.WriteLine($"AngularVelLeft = {AngularVelLeft:0.####}, AngularVelRight = {AngularVelRight:0.####}");
+        _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, SampleTime);
+        return true;
+    }
+
+    public void Stop()
+    {
+        (AngularVelLeft, AngularVelRight) = (0, 0);
+        _ = Visualization.UpdatePosition(AngularVelLeft, AngularVelRight, SampleTime);
+    }
+}

RobotNet.RobotManager/Services/Simulation/VisualizationService.cs

@@ -0,0 +1,54 @@
+using RobotNet.RobotManager.Services.Simulation.Algorithm;
+using RobotNet.RobotManager.Services.Simulation.Models;
+
+namespace RobotNet.RobotManager.Services.Simulation;
+
+public class VisualizationService
+{
+    public double X { get; private set; }
+    public double Y { get; private set; }
+    public double Theta { get; private set; }
+    public double Vx { get; private set; }
+    public double Vy { get; private set; }
+    public double Omega { get; private set; }
+
+    private double RadiusWheel;
+    private double RadiusRobot;
+
+    public VisualizationService WithRadiusWheel(double radiusWheel)
+    {
+        RadiusWheel = radiusWheel;
+        return this;
+    }
+
+    public VisualizationService WithRadiusRobot(double radiusRobot)
+    {
+        RadiusRobot = radiusRobot;
+        return this;
+    }
+
+    public (double x, double y, double angle) UpdatePosition(double wL, double wR, double time)
+    {
+        Theta = (Theta + time * (-wR - wL) * RadiusWheel / RadiusRobot * MathExtension.ToDegree) % 360;
+        X += time * (-wR + wL) * RadiusWheel * Math.Cos(Theta * MathExtension.ToRad) / 2;
+        Y += time * (-wR + wL) * RadiusWheel * Math.Sin(Theta * MathExtension.ToRad) / 2;
+        _ = UpdateVelocity(wL, wR);
+        if (Theta > 180) Theta -= 360;
+        else if (Theta < -180) Theta += 360;
+        return (X, Y, Theta);
+    }
+
+    public (double vx, double vy, double omega) UpdateVelocity(double wL, double wR)
+    {
+        Vx = (-wR + wL) * RadiusWheel / 2;
+        Omega = (-wR - wL) * RadiusWheel / RadiusRobot;
+        return (Vx, 0, Omega);
+    }
+
+    public void LocalizationInitialize(double x, double y, double theta)
+    {
+        X = x;
+        Y = y;
+        Theta = theta;
+    }
+}

RobotNet.RobotManager/Services/Traffic/Agent.cs

@@ -0,0 +1,269 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+
+public class Agent
+{
+    public IRobotController Robot { get; set; } = null!;
+    public AgentModel AgentModel { get; set; } = new();
+    public Guid MapId { get; set; }
+    public int InNodeIndex { get; set; }
+    public TrafficNodeDto InNode { get; set; } = new();
+    public int ReleaseNodeIndex => Nodes.IndexOf(ReleaseNode);
+    public TrafficNodeDto ReleaseNode { get; set; } = new();
+    public TrafficNodeDto GoalNode { get; set; } = new();
+    public List<TrafficNodeDto> Nodes { get; set; } = [];
+    public List<TrafficEdgeDto> Edges { get; set; } = [];
+    public List<TrafficNodeDto> SubNodes { get; set; } = [];
+    public List<TrafficEdgeDto> SubEdges { get; set; } = [];
+    public List<TrafficNodeDto> GivewayNodes { get; set; } = [];
+    public List<TrafficEdgeDto> GivewayEdges { get; set; } = [];
+    public TrafficSolutionState State { get; set; }
+    public RefreshPathState RefreshPathState { get; set; } = RefreshPathState.Compeleted;
+    public string Message { get; set; } = "";
+
+    private static double GetDistance(List<TrafficNodeDto> nodes)
+    {
+        double distance = 0;
+        for (int i = 0; i < nodes.Count - 1; i++)
+        {
+            distance += Math.Sqrt(Math.Pow(nodes[i].X - nodes[i + 1].X, 2) + Math.Pow(nodes[i].Y - nodes[i + 1].Y, 2));
+        }
+        return distance;
+    }
+
+    public bool Checking(double trafficAvoidableNodeMax, double trafficDistanceMax)
+    {
+        if (State != TrafficSolutionState.GiveWay && State != TrafficSolutionState.LoopResolve &&
+            RefreshPathState != RefreshPathState.Created && RefreshPathState != RefreshPathState.Refreshing)
+        {
+            if (ReleaseNodeIndex != -1 && InNodeIndex <= ReleaseNodeIndex && InNodeIndex < Nodes.Count)
+            {
+                var releaseNodes = Nodes.GetRange(InNodeIndex + 1, ReleaseNodeIndex - InNodeIndex);
+                if (releaseNodes.Where(n => n.IsAvoidableNode).Count() < trafficAvoidableNodeMax) return true;
+                var distance = GetDistance([.. Nodes.GetRange(InNodeIndex, ReleaseNodeIndex - InNodeIndex + 1)]);
+                distance -= Math.Sqrt(Math.Pow(Robot.VisualizationMsg.AgvPosition.X - Nodes[InNodeIndex].X, 2) + Math.Pow(Robot.VisualizationMsg.AgvPosition.Y - Nodes[InNodeIndex].Y, 2));
+                if (distance < trafficDistanceMax) return true;
+            }
+        }
+        return false;
+    }
+
+    public TrafficNodeDto[] GetChekingNodes(double trafficAvoidableNodeMax, double trafficDistanceMin)
+    {
+        List<TrafficNodeDto> releaseNodes = [];
+        double distance = 0;
+        distance -= Math.Sqrt(Math.Pow(Nodes[InNodeIndex].X - Robot.VisualizationMsg.AgvPosition.X, 2) + Math.Pow(Nodes[InNodeIndex].Y - Robot.VisualizationMsg.AgvPosition.Y, 2));
+        int index = InNodeIndex < ReleaseNodeIndex ? InNodeIndex + 1 : InNodeIndex;
+        for (; index < Nodes.Count; index++)
+        {
+            releaseNodes.Add(Nodes[index]);
+            if (index < Nodes.Count - 1)
+            {
+                var remainingNodes = Nodes.GetRange(index + 1, Nodes.Count - (index + 1));
+                if (!remainingNodes.Any(n => n.IsAvoidableNode))
+                {
+                    index = Nodes.Count;
+                    break;
+                }
+            }
+            if (index > 0) distance += Math.Sqrt(Math.Pow(Nodes[index].X - Nodes[index - 1].X, 2) + Math.Pow(Nodes[index].Y - Nodes[index - 1].Y, 2));
+            if (distance < trafficDistanceMin || !Nodes[index].IsAvoidableNode) continue;
+            if (releaseNodes.Where(n => n.IsAvoidableNode).Count() >= trafficAvoidableNodeMax) break;
+        }
+        return index > ReleaseNodeIndex ? [.. Nodes.GetRange(ReleaseNodeIndex, index - ReleaseNodeIndex + (index < Nodes.Count ? 1 : 0))] : [];
+    }
+
+    public (TrafficSolutionState state, string message) UpdateGiveWay(TrafficNodeDto[] giveWayNodes, PathPlanner pathPlanner, MapManager map)
+    {
+        try
+        {
+            if (giveWayNodes.Length < 2) return (TrafficSolutionState.UnableResolve, "Lộ trình tránh đường không hợp lệ");
+            if (!giveWayNodes.Any(n => !Nodes.Contains(n)))
+            {
+                var giveNodeIndex = Nodes.IndexOf(giveWayNodes[^1]);
+                if (giveNodeIndex >= ReleaseNodeIndex)
+                {
+                    ReleaseNode = giveWayNodes[^1];
+                    State = TrafficSolutionState.Complete;
+                    return (TrafficSolutionState.Complete, "");
+                }
+            }
+            var giveWayIndex = Nodes.IndexOf(giveWayNodes[0]);
+            var releaseNodeInGivewaysIndex = Array.FindIndex(giveWayNodes, n=> n.Id == ReleaseNode.Id);
+            if (giveWayIndex != -1)
+            {
+                List<TrafficNodeDto> subGiveWayNodes = [];
+                List<EdgeDto> subGiveWayEdges = [];
+                if (releaseNodeInGivewaysIndex != -1) subGiveWayNodes = giveWayNodes.ToList().GetRange(releaseNodeInGivewaysIndex, giveWayNodes.Length - releaseNodeInGivewaysIndex);
+                else if (giveWayIndex == ReleaseNodeIndex) subGiveWayNodes = [.. giveWayNodes];
+                else if (giveWayIndex < ReleaseNodeIndex)
+                {
+                    subGiveWayNodes = Nodes.GetRange(giveWayIndex + 1, ReleaseNodeIndex - giveWayIndex);
+                    subGiveWayNodes.Reverse();
+                    subGiveWayNodes.AddRange(giveWayNodes);
+                }
+                else
+                {
+                    subGiveWayNodes = Nodes.GetRange(ReleaseNodeIndex, giveWayIndex - ReleaseNodeIndex);
+                    subGiveWayNodes.AddRange(giveWayNodes);
+                }
+                List<EdgeDto> edges = [..map.GetEdges(MapId, [..subGiveWayNodes.Select(n => new NodeDto
+                    {
+                        Id = n.Id,
+                        Name = n.Name,
+                        X = n.X,
+                        Y = n.Y,
+                        MapId = MapId,
+                    })])];
+                if (edges.Count > 0)
+                {
+                    double releaseTheta;
+                    if (ReleaseNodeIndex > 0)
+                    {
+                        var (nearNodeX, nearNodeY) = MapEditorHelper.Curve(0.9, new EdgeCaculatorModel
+                        {
+                            TrajectoryDegree = Edges[ReleaseNodeIndex - 1].TrajectoryDegree,
+                            ControlPoint1X = Edges[ReleaseNodeIndex - 1].ControlPoint1X,
+                            ControlPoint1Y = Edges[ReleaseNodeIndex - 1].ControlPoint1Y,
+                            ControlPoint2X = Edges[ReleaseNodeIndex - 1].ControlPoint2X,
+                            ControlPoint2Y = Edges[ReleaseNodeIndex - 1].ControlPoint2Y,
+                            X1 = Nodes[ReleaseNodeIndex - 1].X,
+                            Y1 = Nodes[ReleaseNodeIndex - 1].Y,
+                            X2 = Nodes[ReleaseNodeIndex].X,
+                            Y2 = Nodes[ReleaseNodeIndex].Y,
+                        });
+                        var relaseForward = Math.Atan2(Nodes[ReleaseNodeIndex].Y - nearNodeY, Nodes[ReleaseNodeIndex].X - nearNodeX) * 180 / Math.PI;
+                        var releaseBackward = Math.Atan2(nearNodeY - Nodes[ReleaseNodeIndex].Y, nearNodeX - Nodes[ReleaseNodeIndex].X) * 180 / Math.PI;
+                        releaseTheta = Nodes[ReleaseNodeIndex - 1].Direction == RobotDirection.FORWARD ? relaseForward : releaseBackward;
+                    }
+                    else
+                    {
+                        var (nearNodeX, nearNodeY) = MapEditorHelper.Curve(0.1, new EdgeCaculatorModel
+                        {
+                            TrajectoryDegree = Edges[ReleaseNodeIndex].TrajectoryDegree,
+                            ControlPoint1X = Edges[ReleaseNodeIndex].ControlPoint1X,
+                            ControlPoint1Y = Edges[ReleaseNodeIndex].ControlPoint1Y,
+                            ControlPoint2X = Edges[ReleaseNodeIndex].ControlPoint2X,
+                            ControlPoint2Y = Edges[ReleaseNodeIndex].ControlPoint2Y,
+                            X1 = Nodes[ReleaseNodeIndex].X,
+                            Y1 = Nodes[ReleaseNodeIndex].Y,
+                            X2 = Nodes[ReleaseNodeIndex + 1].X,
+                            Y2 = Nodes[ReleaseNodeIndex + 1].Y,
+                        });
+                        var releaseBackward = Math.Atan2(Nodes[ReleaseNodeIndex].Y - nearNodeY, Nodes[ReleaseNodeIndex].X - nearNodeX) * 180 / Math.PI;
+                        var relaseForward = Math.Atan2(nearNodeY - Nodes[ReleaseNodeIndex].Y, nearNodeX - Nodes[ReleaseNodeIndex].X) * 180 / Math.PI;
+                        releaseTheta = Nodes[0].Direction == RobotDirection.FORWARD ? relaseForward : releaseBackward;
+                    }
+                    List<NodeDto> nodes = [..PathPlanner.CalculatorDirection(releaseTheta, [..subGiveWayNodes.Select(n => new NodeDto
+                    {
+                        Id = n.Id,
+                        X = n.X,
+                        Y = n.Y,
+                        Name = n.Name,
+                        MapId = MapId,
+                    })], [..edges])];
+                    GivewayNodes = [.. nodes.Select(n => new TrafficNodeDto
+                    {
+                        Id = n.Id,
+                        X = n.X,
+                        Y = n.Y,
+                        Name = n.Name,
+                        Direction = MapCompute.GetRobotDirection(n.Direction),
+                    })];
+                    //Console.WriteLine($"{Robot.SerialNumber} giveway: [{string.Join(",", GivewayNodes.Select(n => $"({n.Name} - {n.Direction})"))}]");
+                    foreach (var node in GivewayNodes)
+                    {
+                        node.IsAvoidableNode = map.GetNegativeNodes(MapId, node.Id).Length > 2;
+                        if (node.IsAvoidableNode)
+                        {
+                            node.AvoidablePaths = [.. map.GetNegativePaths(MapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, 2)
+                                                .Where(path => path.Length > 0)
+                                                .Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())];
+                        }
+                    }
+                    GivewayEdges = [.. edges.Select(n => new TrafficEdgeDto
+                    {
+                        Id = n.Id,
+                        ControlPoint1X = n.ControlPoint1X,
+                        ControlPoint1Y = n.ControlPoint1Y,
+                        ControlPoint2X = n.ControlPoint2X,
+                        ControlPoint2Y = n.ControlPoint2Y,
+                        StartNodeId = n.StartNodeId,
+                        EndNodeId = n.EndNodeId,
+                        TrajectoryDegree = n.TrajectoryDegree,
+                    })];
+                    State = TrafficSolutionState.GiveWay;
+                    var angleForward = Math.Atan2(GivewayNodes[^1].Y - GivewayNodes[^2].Y, GivewayNodes[^1].X - GivewayNodes[^2].X) * 180 / Math.PI;
+                    var angleBackward = Math.Atan2(GivewayNodes[^2].Y - GivewayNodes[^1].Y, GivewayNodes[^2].X - GivewayNodes[^1].X) * 180 / Math.PI;
+                    RefreshPath(GivewayNodes[^1], GivewayNodes[^1].Direction == RobotDirection.FORWARD ? angleForward : angleBackward, pathPlanner, map);
+                    return (TrafficSolutionState.GiveWay, "");
+                }
+                return (TrafficSolutionState.UnableResolve, $"Không tìm thấy edge yêu cầu phải tránh [{GivewayNodes[0].Name} - {GivewayNodes[1].Name}]");
+            }
+            return (TrafficSolutionState.UnableResolve, $"Không tìm thấy điểm xung đột {GivewayNodes[0].Name}");
+        }
+        catch (Exception ex)
+        {
+            return (TrafficSolutionState.UnableResolve, $"Cập nhật lộ trình tránh đường xảy ra lỗi: {ex.Message}");
+        }
+    }
+
+    private void RefreshPath(TrafficNodeDto currentNode, double theta, PathPlanner planner, MapManager map)
+    {
+        RefreshPathState = RefreshPathState.Created;
+        var plannerTask = Task.Run(async () =>
+        {
+            RefreshPathState = RefreshPathState.Refreshing;
+            var path = await planner.Planning(currentNode.Id, theta, AgentModel.NavigationType, MapId, GoalNode.Id);
+            if (!path.IsSuccess)
+            {
+                RefreshPathState = RefreshPathState.Error;
+                Message = path.Message;
+                return;
+            }
+            if (path.Data.Nodes is null || path.Data.Edges is null || path.Data.Edges.Length == 0 || path.Data.Nodes.Length < 2)
+            {
+                RefreshPathState = RefreshPathState.Error;
+                Message = $"Đường dẫn tới đích [{GoalNode.Name} - {GoalNode.Id}] từ [{currentNode.Name} - {currentNode.Id}] không tồn tại";
+                return;
+            }
+            SubEdges = [..path.Data.Edges.Select(n => new TrafficEdgeDto()
+                {
+                    Id = n.Id,
+                    StartNodeId = n.StartNodeId,
+                    EndNodeId = n.EndNodeId,
+                    TrajectoryDegree = n.TrajectoryDegree,
+                    ControlPoint1X = n.ControlPoint1X,
+                    ControlPoint1Y = n.ControlPoint1Y,
+                    ControlPoint2X = n.ControlPoint2X,
+                    ControlPoint2Y = n.ControlPoint2Y,
+                })];
+            SubNodes = [..path.Data.Nodes.Select(n => new TrafficNodeDto()
+                {
+                    Id = n.Id,
+                    Name = n.Name,
+                    X = n.X,
+                    Y = n.Y,
+                    Direction = MapCompute.GetRobotDirection(n.Direction)
+                })];
+            foreach (var node in SubNodes)
+            {
+                node.IsAvoidableNode = map.GetNegativeNodes(MapId, node.Id).Length > 2;
+                if (node.IsAvoidableNode)
+                {
+                    node.AvoidablePaths = [.. map.GetNegativePaths(MapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, 2)
+                                        .Where(path => path.Length > 0)
+                                        .Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())];
+                }
+            }
+            //Console.WriteLine($"{Robot.SerialNumber} refresh path: [{string.Join(",", SubNodes.Select(n => $"({n.Name} - {n.Direction})"))}]");
+            RefreshPathState = RefreshPathState.Compeleted;
+        });
+    }
+}

RobotNet.RobotManager/Services/Traffic/AgentModel.cs

@@ -0,0 +1,13 @@
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class AgentModel
+{
+    public NavigationType NavigationType { get; set; }
+    public double Length { get; set; }
+    public double Width { get; set; }
+    public double NavigationPointX { get; set; }
+    public double NavigationPointY { get; set; }
+    public double TurningRadius { get; set; }
+}

RobotNet.RobotManager/Services/Traffic/TrafficAlarm.cs

@@ -0,0 +1,5 @@
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class TrafficAlarm
+{
+}

RobotNet.RobotManager/Services/Traffic/TrafficConflict.cs

@@ -0,0 +1,15 @@
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+#nullable disable
+
+public class TrafficConflict
+{
+    public Agent AgentRequest { get; set; }
+    public Agent AgentConflict { get; set; }
+    public TrafficNodeDto NodeConflict { get; set; }
+    public List<TrafficNodeDto> ReleaseNodes { get; set; }
+    public TrafficConflictState State { get; set; }
+}

RobotNet.RobotManager/Services/Traffic/TrafficEdgeDto.cs

@@ -0,0 +1,20 @@
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotShares.Dtos;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+#nullable disable
+
+public class TrafficEdgeDto
+{
+    public Guid Id { get; set; }
+    public Guid StartNodeId { get; set; }
+    public Guid EndNodeId { get; set; }
+    public TrafficNodeDto StartNode { get; set; }
+    public TrafficNodeDto EndNode { get; set; }
+    public double ControlPoint1X { get; set; }
+    public double ControlPoint1Y { get; set; }
+    public double ControlPoint2X { get; set; }
+    public double ControlPoint2Y { get; set; }
+    public TrajectoryDegree TrajectoryDegree { get; set; }
+}

RobotNet.RobotManager/Services/Traffic/TrafficGiveway.cs

@@ -0,0 +1,13 @@
+using RobotNet.RobotShares.Dtos;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+#nullable disable
+
+public class TrafficGiveway
+{
+    public string RobotGive { get; set; }
+    public string RobotReceive { get; set; }
+    public TrafficNodeDto[] Nodes { get; set; }
+    public bool IsGiveway { get; set; } = false;
+}

RobotNet.RobotManager/Services/Traffic/TrafficManager.cs

@@ -0,0 +1,857 @@
+using RobotNet.RobotManager.Data;
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+using System.Collections.Concurrent;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class TrafficManager(MapManager Map, IServiceProvider ServiceProvider, PathPlanner PathPlannger, IConfiguration Configuration, LoggerController<TrafficManager> Logger) : IHostedService
+{
+    private System.Threading.Timer? computeTimer;
+    private const int intervalTime = 1000;
+
+    private readonly double TrafficCheckingDistanceMax = Configuration.GetValue("TrafficConfig:CheckingDistanceMax", 5);
+    private readonly double TrafficCheckingDistanceMin = Configuration.GetValue("TrafficConfig:CheckingDistanceMin", 3);
+    private readonly int TrafficResolutionRepeat = Configuration.GetValue("TrafficConfig:ResolutionRepeat", 1);
+    private readonly int TrafficAvoidableNodeMax = Configuration.GetValue("TrafficConfig:AvoidableNodeMax", 3);
+    private readonly double TrafficOffsetLength = Configuration.GetValue("TrafficConfig:OffsetLength", 0.3);
+    private readonly double TrafficOffsetWidth = Configuration.GetValue("TrafficConfig:OffsetWidth", 0.2);
+    public bool Enable => Configuration.GetValue("TrafficConfig:Enable", false);
+
+    private readonly SemaphoreSlim AgentSemaphore = new(1, 1);
+    private readonly SemaphoreSlim UpdateSemaphore = new(1, 1);
+
+    public ConcurrentDictionary<Guid, TrafficMap> TrafficMaps { get; set; } = [];
+
+    private Agent NewAgent(Guid mapId, IRobotController robotController, AgentModel model, TrafficNodeDto[] nodes, TrafficEdgeDto[] edges)
+    {
+        model.TurningRadius = TrafficMath.CalTurningRadius(model, TrafficOffsetLength, TrafficOffsetWidth);
+        foreach (var node in nodes)
+        {
+            node.AvoidablePaths = [.. Map.GetNegativePaths(mapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, model.TurningRadius)
+                                .Where(path => path.Length > 0)
+                                .Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())];
+            node.IsAvoidableNode = Map.GetNegativeNodes(mapId, node.Id).Length > 2 && node.AvoidablePaths.Length > 0;
+            if (nodes.Length > 0 && node != nodes.Last() && model.NavigationType != NavigationType.Forklift) node.LockedShapes = new()
+            {
+                Type = TrafficLockedShapeType.Circle,
+                Radius = model.TurningRadius,
+            };
+            else node.LockedShapes = TrafficMath.CalRobotRectShape(model, TrafficOffsetLength, TrafficOffsetWidth, node.X, node.Y, node.Theta);
+        }
+        Agent agent = new()
+        {
+            Robot = robotController,
+            AgentModel = model,
+            Nodes = [.. nodes],
+            InNodeIndex = 0,
+            ReleaseNode = nodes[0],
+            Edges = [.. edges],
+            State = TrafficSolutionState.Complete,
+            MapId = mapId,
+            GoalNode = nodes[^1],
+        };
+        return agent;
+    }
+
+    public MessageResult CreateAgent(Guid mapId, IRobotController robotController, AgentModel model, TrafficNodeDto[] nodes, TrafficEdgeDto[] edges)
+    {
+        if (AgentSemaphore.Wait(2000))
+        {
+            try
+            {
+                if (TrafficMaps.TryGetValue(mapId, out TrafficMap? TrafficMap) && TrafficMap is not null)
+                {
+                    if (TrafficMap.Agents.TryGetValue(robotController.SerialNumber, out _)) TrafficMap.Agents.Remove(robotController.SerialNumber);
+                    TrafficMap.Agents.Add(robotController.SerialNumber, NewAgent(mapId, robotController, model, nodes, edges));
+                }
+                else
+                {
+                    bool result = TrafficMaps.TryAdd(mapId, new TrafficMap()
+                    {
+                        MapId = mapId,
+                        Agents = new Dictionary<string, Agent>()
+                        {
+                            {
+                                robotController.SerialNumber,
+                                NewAgent(mapId, robotController, model, nodes, edges)
+                            }
+                        },
+                    });
+
+                    if (!result) return new(false, $"Không thể tạo TrafficMap cho bản đồ {mapId} do đã tồn tại hoặc xảy ra lỗi khi thêm agent {robotController.SerialNumber}.");
+                }
+                //Logger.Info($"CreateAgent: Tạo Agent thành công - {robotController.SerialNumber} to {nodes[^1].Name}, nodes: [{string.Join(", ", nodes.Select(n => $"[({n.Name}) - ({n.Direction})]"))}]");
+
+                return new(true, "");
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Tạo mới traffic Agent xảy ra lỗi - {ex.Message}");
+                return new(false, $"Tạo mới traffic Agent xảy ra lỗi - {ex.Message}");
+            }
+            finally
+            {
+                AgentSemaphore.Release();
+            }
+        }
+        else return new(false, $"Không thể tạo trafficAgent do vượt quá thời gian chờ.");
+    }
+
+    public MessageResult DeleteAgent(string robotId)
+    {
+        if (AgentSemaphore.Wait(2000))
+        {
+            try
+            {
+                using var scope = ServiceProvider.CreateScope();
+                var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+                var robot = RobotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
+                if (robot is null) return new(false, $"Không tìm thấy robot Id {robotId} trong kho dữ liệu.");
+                if (TrafficMaps.TryGetValue(robot.MapId, out TrafficMap? TrafficMap) && TrafficMap is not null)
+                {
+                    if (TrafficMap.Agents.TryGetValue(robotId, out _))
+                    {
+                        var remove = TrafficMap.Agents.Remove(robotId);
+                        if (!remove) return new(false, $"Không thể xóa agent {robotId}.");
+                    }
+                }
+
+                //Logger.Info($"DeleteAgent: Xóa Agent thành công - {robotId}");
+                return new(true, "");
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"Xóa traffic Agent xảy ra lỗi - {ex.Message}");
+                return new(false, $"Xóa traffic Agent xảy ra lỗi - {ex.Message}");
+            }
+            finally
+            {
+                AgentSemaphore.Release();
+            }
+        }
+        else return new(false, $"Không thể xóa trafficAgent do vượt quá thời gian chờ.");
+    }
+
+    public TrafficSolution GetTrafficNode(string robotId)
+    {
+        try
+        {
+            using var scope = ServiceProvider.CreateScope();
+            var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+            var robot = RobotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
+            if (robot is null) return new() { State = TrafficSolutionState.None };
+
+            if (TrafficMaps.TryGetValue(robot.MapId, out TrafficMap? trafficmap) && trafficmap is not null)
+            {
+                if (trafficmap.Agents.TryGetValue(robotId, out Agent? agent) && agent is not null)
+                {
+                    TrafficSolution returnSolution = new()
+                    {
+                        State = agent.State,
+                        ReleaseNode = agent.ReleaseNode,
+                        GivewayEdges = agent.GivewayEdges,
+                        GivewayNodes = agent.GivewayNodes,
+                        Nodes = [.. agent.Nodes],
+                        Edges = [.. agent.Edges]
+                    }; ;
+                    switch (agent.State)
+                    {
+                        case TrafficSolutionState.GiveWay:
+                            if (agent.InNodeIndex != agent.ReleaseNodeIndex && (agent.GivewayNodes.Count > 0 && agent.ReleaseNode.Id != agent.GivewayNodes[^1].Id)) returnSolution.State = TrafficSolutionState.Waitting;
+                            else
+                            {
+                                if (agent.GivewayNodes.Count > 0 && agent.ReleaseNode.Id != agent.GivewayNodes[^1].Id) agent.ReleaseNode = agent.GivewayNodes[^1];
+                                if (agent.GivewayNodes.Count == 0 && agent.RefreshPathState == RefreshPathState.Compeleted)
+                                {
+                                    agent.State = TrafficSolutionState.RefreshPath;
+                                    returnSolution.State = TrafficSolutionState.RefreshPath;
+                                }
+                            }
+                            break;
+                        case TrafficSolutionState.LoopResolve: returnSolution.State = TrafficSolutionState.Complete; break;
+                    };
+                    // Console.WriteLine($"{robotId} Gettraffic: {returnSolution.State}, release: {returnSolution.ReleaseNode.Name}");
+                    return returnSolution;
+                }
+            }
+            return new() { State = TrafficSolutionState.None };
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{robotId} Lấy thông tin traffic xảy ra lỗi: {ex.Message}");
+            return new() { State = TrafficSolutionState.None };
+        }
+    }
+
+    public MessageResult UpdateInNode(string robotId, Guid nodeId)
+    {
+        if (UpdateSemaphore.Wait(2000))
+        {
+            try
+            {
+                using var scope = ServiceProvider.CreateScope();
+                var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
+                var robot = RobotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
+                if (robot is null) return new(false, $"Không tìm thấy robot Id {robotId} trong kho dữ liệu.");
+
+                if (TrafficMaps.TryGetValue(robot.MapId, out TrafficMap? trafficmap) && trafficmap is not null)
+                {
+                    if (trafficmap.Agents.TryGetValue(robotId, out Agent? agent) && agent is not null)
+                    {
+                        if (agent.State == TrafficSolutionState.GiveWay)
+                        {
+                            if (agent.GivewayNodes.Count > 1 && nodeId == agent.GivewayNodes[^1].Id)
+                            {
+                                var giveWayResolution = trafficmap.GivewayResolution.FirstOrDefault(n => n.RobotGive == robotId);
+                                if (giveWayResolution is not null)
+                                {
+                                    trafficmap.AddLocker(giveWayResolution.RobotReceive, [.. giveWayResolution.Nodes]);
+                                    giveWayResolution.IsGiveway = true;
+                                }
+                                agent.Nodes = agent.SubNodes;
+                                agent.Edges = agent.SubEdges;
+                                agent.GivewayNodes = [];
+                                agent.GivewayEdges = [];
+                            }
+
+                            List<TrafficNodeDto> lockedNodes = [];
+                            var nodeIndex = agent.Nodes.FindIndex(n => n.Id == nodeId);
+                            if (nodeIndex != -1)
+                            {
+                                agent.InNodeIndex = nodeIndex;
+                                if (agent.ReleaseNodeIndex != -1 && agent.InNodeIndex <= agent.ReleaseNodeIndex)
+                                    lockedNodes = agent.Nodes.GetRange(agent.InNodeIndex, agent.ReleaseNodeIndex - agent.InNodeIndex + 1);
+                                lockedNodes.AddRange(agent.GivewayNodes.Where(n => !lockedNodes.Any(ln => ln.Id == n.Id)));
+                            }
+                            else
+                            {
+                                nodeIndex = agent.GivewayNodes.FindIndex(n => n.Id == nodeId);
+                                if (nodeIndex != -1) lockedNodes = agent.GivewayNodes.GetRange(nodeIndex, agent.GivewayNodes.Count - nodeIndex);
+                            }
+
+                            trafficmap.UpdateLocker(robotId, [.. lockedNodes]);
+                        }
+                        else
+                        {
+                            var nodeIndex = agent.Nodes.FindIndex(n => n.Id == nodeId);
+                            if (nodeIndex != -1)
+                            {
+                                agent.InNodeIndex = nodeIndex;
+
+                                if (agent.ReleaseNodeIndex != -1 && agent.InNodeIndex <= agent.ReleaseNodeIndex)
+                                {
+                                    var lockedNodes = agent.Nodes.GetRange(agent.InNodeIndex, agent.ReleaseNodeIndex - agent.InNodeIndex + 1);
+                                    trafficmap.UpdateLocker(robotId, [.. lockedNodes]);
+                                    trafficmap.GivewayResolution.RemoveAll(s => s.RobotReceive == agent.Robot.SerialNumber && s.IsGiveway && !lockedNodes.Any(n => s.Nodes.Any(sn => sn.Id == n.Id)));
+                                }
+                                if (agent.State == TrafficSolutionState.LoopResolve && nodeId == agent.ReleaseNode.Id)
+                                {
+                                    var giveWayResolution = trafficmap.GivewayResolution.FirstOrDefault(n => n.RobotGive == robotId);
+                                    if (giveWayResolution is not null)
+                                    {
+                                        trafficmap.AddLocker(giveWayResolution.RobotReceive, [.. giveWayResolution.Nodes]);
+                                        giveWayResolution.IsGiveway = true;
+                                    }
+                                    agent.State = TrafficSolutionState.Waitting;
+                                }
+                            }
+                        }
+                    }
+                }
+                return new(true, "");
+            }
+            catch (Exception ex)
+            {
+                Logger.Warning($"{robotId} Cập nhật vị trí xảy ra lỗi: {ex.Message}");
+                return new(false, $"{robotId} Cập nhật vị trí xảy ra lỗi: {ex.Message}");
+            }
+            finally
+            {
+                UpdateSemaphore.Release();
+            }
+        }
+        else return new(false, $"Không thể xóa cập nhật dữ liệu vào hệ thống traffic do vượt quá thời gian chờ.");
+    }
+
+    private static (TrafficConflictState state, string robotId, TrafficNodeDto? nodeId) FirstCheckingReleaseNodes(TrafficNodeDto[] releaseNodes, Agent agent, TrafficMap trafficMap)
+    {
+        if (releaseNodes.Length != 0)
+        {
+            foreach (var releaseNode in releaseNodes)
+            {
+                foreach (var locked in trafficMap.Locked)
+                {
+                    if (locked.Key == agent.Robot.SerialNumber) continue;
+                    if (locked.Value.Any(n => n.Id == releaseNode.Id)) return (TrafficConflictState.Vertex, locked.Key, releaseNode);
+                    //foreach (var lockedNode in locked.Value)
+                    //{
+                    //    var distance = Math.Sqrt(Math.Pow(lockedNode.X - releaseNode.X, 2) + Math.Pow(lockedNode.Y - releaseNode.Y, 2));
+                    //    if(distance > (lockedNode.LockedShapes.Radius + releaseNode.LockedShapes.Radius)) continue;
+                    //    if (lockedNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
+                    //    {
+                    //        if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
+                    //        {
+                    //            if (distance < (lockedNode.LockedShapes.Radius + releaseNode.LockedShapes.Radius)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
+                    //        }
+                    //        else if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Rectangle)
+                    //        {
+                    //            if(TrafficMath.RectIntersectCircle(lockedNode, lockedNode.LockedShapes.Radius, 
+                    //                releaseNode.LockedShapes.X1, releaseNode.LockedShapes.Y1,
+                    //                releaseNode.LockedShapes.X2, releaseNode.LockedShapes.Y2,
+                    //                releaseNode.LockedShapes.X3, releaseNode.LockedShapes.Y3,
+                    //                releaseNode.LockedShapes.X4, releaseNode.LockedShapes.Y4)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
+                    //        }
+                    //    }
+                    //    else if (lockedNode.LockedShapes.Type == TrafficLockedShapeType.Rectangle)
+                    //    {
+                    //        if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
+                    //        {
+                    //            if(TrafficMath.RectIntersectRect(lockedNode.LockedShapes, releaseNode.LockedShapes)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
+                    //        }
+                    //        else if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
+                    //        {
+                    //            if (TrafficMath.RectIntersectCircle(releaseNode, releaseNode.LockedShapes.Radius,
+                    //                lockedNode.LockedShapes.X1, lockedNode.LockedShapes.Y1,
+                    //                lockedNode.LockedShapes.X2, lockedNode.LockedShapes.Y2,
+                    //                lockedNode.LockedShapes.X3, lockedNode.LockedShapes.Y3,
+                    //                lockedNode.LockedShapes.X4, lockedNode.LockedShapes.Y4)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
+                    //        }
+                    //    }
+                    //}
+                }
+            }
+        }
+        return (TrafficConflictState.None, string.Empty, null);
+    }
+
+    private static (bool IsSuccess, string robotId, TrafficNodeDto? nodeId) GivewayCheckingReleaseNodes(TrafficNodeDto[] releaseNodes, string robotId, TrafficMap trafficMap)
+    {
+        foreach (var node in releaseNodes)
+        {
+            foreach (var locked in trafficMap.Locked)
+            {
+                if (locked.Value.Any(n => (n.Id == node.Id)) && locked.Key != robotId)
+                    return (false, locked.Key, node);
+            }
+        }
+        return (true, string.Empty, null);
+    }
+
+    private void CheckConflict(Agent agent, TrafficMap trafficMap)
+    {
+        if (agent.Checking(TrafficAvoidableNodeMax, TrafficCheckingDistanceMax) && !trafficMap.GivewayResolution.Any(s => s.RobotGive == agent.Robot.SerialNumber))
+        {
+            var releaseNodes = agent.GetChekingNodes(TrafficAvoidableNodeMax, TrafficCheckingDistanceMin);
+            if (releaseNodes.Length > 0)
+            {
+                (var state, string conflictRobotId, TrafficNodeDto? conflictNode) = FirstCheckingReleaseNodes(releaseNodes, agent, trafficMap);
+                if (state == TrafficConflictState.None)
+                {
+
+                    trafficMap.AddLocker(agent.Robot.SerialNumber, [.. releaseNodes]);
+                    agent.ReleaseNode = releaseNodes[^1];
+                    agent.State = TrafficSolutionState.Complete;
+                    trafficMap.Conflicts.RemoveAll(conflict => conflict.AgentRequest.Robot.SerialNumber == agent.Robot.SerialNumber);
+                }
+                else
+                {
+                    if (conflictNode is null)
+                    {
+                        Logger.Warning($"{agent.Robot.SerialNumber} kiểm tra xung đột có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
+                        return;
+                    }
+
+                    if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
+                    {
+                        trafficMap.Conflicts.RemoveAll(c => c.AgentRequest.Robot.SerialNumber == agent.Robot.SerialNumber);
+                        var conflictState = GetConflictState(agent, conflictAgent, conflictNode);
+                        if (conflictState == TrafficConflictState.Confrontation || state == TrafficConflictState.Proximity)
+                        {
+                            Level1ConflictResolution(agent, conflictAgent, [.. releaseNodes], conflictNode, trafficMap);
+                        }
+                        else
+                        {
+
+                            trafficMap.Conflicts.Add(new()
+                            {
+                                AgentConflict = conflictAgent,
+                                AgentRequest = agent,
+                                NodeConflict = conflictNode,
+                                ReleaseNodes = [.. releaseNodes],
+                                State = TrafficConflictState.Vertex,
+                            });
+                            agent.State = TrafficSolutionState.Waitting;
+                        }
+                    }
+                    else
+                    {
+                        // cần xử lí khi bị xung đột với robot đang không di chuyển
+                    }
+                }
+            }
+        }
+    }
+
+    private static TrafficConflictState GetConflictState(Agent agent1, Agent agent2, TrafficNodeDto conflictNode)
+    {
+        var conflictNodeAgent2Index = agent2.Nodes.IndexOf(conflictNode);
+        if (conflictNodeAgent2Index != -1)
+        {
+            var remainingNodes = agent2.Nodes.GetRange(conflictNodeAgent2Index, agent2.Nodes.Count - conflictNodeAgent2Index);
+            if (remainingNodes.Any(n => n.Id == agent1.ReleaseNode.Id)) return TrafficConflictState.Confrontation;
+            return TrafficConflictState.Vertex;
+        }
+
+        return TrafficConflictState.None;
+    }
+
+    private TrafficSolutionState Level1ConflictResolution(Agent agent1, Agent agent2, List<TrafficNodeDto> releaseNodes, TrafficNodeDto conflictNode, TrafficMap trafficMap)
+    {
+        try
+        {
+            var remainingNodes = agent2.Nodes.GetRange(agent2.InNodeIndex, agent2.Nodes.Count - agent2.InNodeIndex);
+            var conflictIndex = releaseNodes.IndexOf(conflictNode);
+            if (conflictIndex != -1)
+            {
+                //double distance = 0;
+                for (int i = conflictIndex - 1; i >= 0; i--)
+                {
+                    //distance += Math.Sqrt(Math.Pow(releaseNodes[i].X - releaseNodes[i + 1].X, 2) + Math.Pow(releaseNodes[i].Y - releaseNodes[i + 1].Y, 2));
+                    //if (distance < TrafficLockedCircle) continue;
+                    if (!remainingNodes.Any(n => n.Id == releaseNodes[i].Id))
+                    {
+                        var givewayNodes = releaseNodes.GetRange(0, releaseNodes.Count - i);
+                        (var IsSuccess, string conflictRobotId, TrafficNodeDto? _) = GivewayCheckingReleaseNodes([.. givewayNodes], agent1.Robot.SerialNumber, trafficMap);
+                        if (IsSuccess)
+                        {
+                            trafficMap.AddLocker(agent1.Robot.SerialNumber, [.. givewayNodes]);
+                            agent1.State = TrafficSolutionState.Complete;
+                            agent1.ReleaseNode = givewayNodes[^1];
+                            trafficMap.Conflicts.RemoveAll(conflict => conflict.AgentRequest.Robot.SerialNumber == agent1.Robot.SerialNumber);
+                            return TrafficSolutionState.Complete;
+                        }
+                    }
+                    if (!releaseNodes[i].IsAvoidableNode) continue;
+
+                    var state = Level2ConflictResolution(agent1, agent2, [.. releaseNodes[i].AvoidablePaths], [.. releaseNodes], [.. releaseNodes.GetRange(0, i + 1)], trafficMap);
+                    if (state != TrafficSolutionState.UnableResolve) return state;
+                }
+                if (!trafficMap.GivewayResolution.Any(s => s.RobotReceive == agent1.Robot.SerialNumber))
+                {
+                    trafficMap.Conflicts.Add(new()
+                    {
+                        AgentRequest = agent1,
+                        AgentConflict = agent2,
+                        NodeConflict = conflictNode,
+                        ReleaseNodes = releaseNodes,
+                        State = TrafficConflictState.Confrontation,
+                    });
+                }
+                return TrafficSolutionState.UnableResolve;
+            }
+            else
+            {
+                Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 1 xảy ra lỗi: confilct node không được tìm thấy trong release node {conflictNode.Name}, release list [{string.Join(", ", releaseNodes.Select(n => n.Name))}]");
+            }
+
+            return TrafficSolutionState.UnableResolve;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 1 xảy ra lỗi: {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private TrafficSolutionState Level2ConflictResolution(Agent agent1, Agent agent2, TrafficNodeDto[][] avoidablePaths, TrafficNodeDto[] relaseNodes, TrafficNodeDto[] conflictNodes, TrafficMap trafficMap)
+    {
+        try
+        {
+            if (agent2.State == TrafficSolutionState.GiveWay) return TrafficSolutionState.Waitting;
+            List<(Agent conflictAgent, TrafficNodeDto conflictNode)> bufferConflict = [];
+            var remainingNodeAgent2 = agent2.Nodes.GetRange(agent2.InNodeIndex, agent2.Nodes.Count - agent2.InNodeIndex);
+            var remainingReleaseNodeAgent2 = agent2.Nodes.GetRange(agent2.InNodeIndex, agent2.ReleaseNodeIndex - agent2.InNodeIndex + 1);
+            foreach (var avoidablePath in avoidablePaths)
+            {
+                if (avoidablePath.Any(avoidableNode => remainingReleaseNodeAgent2.Any(n => n.Id == avoidableNode.Id)) ||
+                    remainingNodeAgent2.Any(avoidableNode => avoidableNode.Id == avoidablePath[^1].Id)) continue;
+
+                (var IsSuccess, string conflictRobotId, TrafficNodeDto? conflictNode) = GivewayCheckingReleaseNodes([.. conflictNodes, .. avoidablePath], agent1.Robot.SerialNumber, trafficMap);
+                if (IsSuccess)
+                {
+                    TrafficNodeDto[] givewayNodes = [.. conflictNodes, .. avoidablePath.Skip(1)];
+                    trafficMap.AddLocker(agent1.Robot.SerialNumber, givewayNodes);
+                    var (state, message) = agent1.UpdateGiveWay(givewayNodes, PathPlannger, Map);
+                    Logger.Info($"{agent1.Robot.SerialNumber} tránh Robot {agent2.Robot.SerialNumber} tại Node {givewayNodes[^1].Name} {(state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay ? "thành công" : $"xảy ra lỗi: {message}")}");
+                    if (state == TrafficSolutionState.GiveWay)
+                        trafficMap.GivewayResolution.Add(new()
+                        {
+                            RobotGive = agent1.Robot.SerialNumber,
+                            RobotReceive = agent2.Robot.SerialNumber,
+                            Nodes = [conflictNodes[^1]]
+                        });
+                    return state;
+                }
+                else
+                {
+                    if (conflictNode is null)
+                    {
+                        Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 2 có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
+                    }
+                    else
+                    {
+                        if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
+                            bufferConflict.Add((conflictAgent, conflictNode));
+                    }
+                }
+            }
+
+            foreach (var (conflictAgent, conflictNode) in bufferConflict)
+            {
+                var trafficState = Level3ConflictResolution(conflictAgent, agent1.Robot.SerialNumber, conflictNode, relaseNodes, trafficMap, 0);
+                if (trafficState == TrafficSolutionState.Complete || trafficState == TrafficSolutionState.GiveWay) return trafficState;
+            }
+
+            //return FindAvoidableResolution(agent1, agent2, trafficMap) ;
+            return TrafficSolutionState.UnableResolve;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 2 xảy ra lỗi: {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private TrafficSolutionState Level3ConflictResolution(Agent agent, string giveWayRobotId, TrafficNodeDto giveNode, TrafficNodeDto[] relaseNodes, TrafficMap trafficMap, int counter)
+    {
+        try
+        {
+            if (agent.State == TrafficSolutionState.GiveWay || agent.State == TrafficSolutionState.LoopResolve) return TrafficSolutionState.Waitting;
+            List<(Agent conflictAgent, TrafficNodeDto conflictNode)> bufferConflict = [];
+
+            var negativePaths = Map.GetNegativePaths(trafficMap.MapId, new() { Id = giveNode.Id, X = giveNode.X, Y = giveNode.Y, Name = giveNode.Name }, agent.AgentModel.TurningRadius)
+                                .Where(path => path.Length > 0)
+                                .Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())
+                                .ToList();
+
+            foreach (var negativePath in negativePaths)
+            {
+                if (negativePath.Any(negativeNode => relaseNodes.Any(n => n.Id == negativeNode.Id) && negativeNode.Id != giveNode.Id)) continue;
+
+                (var IsSuccess, string conflictRobotId, TrafficNodeDto? conflictNode) = GivewayCheckingReleaseNodes(negativePath, agent.Robot.SerialNumber, trafficMap);
+                if (IsSuccess)
+                {
+                    trafficMap.AddLocker(agent.Robot.SerialNumber, negativePath);
+                    var (state, message) = agent.UpdateGiveWay(negativePath, PathPlannger, Map);
+                    Logger.Info($"{agent.Robot.SerialNumber} tránh level3 Robot {giveWayRobotId} tại Node {negativePath[^1].Name} {(state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay ? "thành công" : $"xảy ra lỗi: {message}")}");
+                    if (state == TrafficSolutionState.GiveWay)
+                        trafficMap.GivewayResolution.Add(new()
+                        {
+                            RobotGive = agent.Robot.SerialNumber,
+                            RobotReceive = giveWayRobotId,
+                            Nodes = [giveNode]
+                        });
+                    return state;
+                }
+                else
+                {
+                    if (conflictNode is null)
+                    {
+                        Logger.Warning($"{agent.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 3 có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
+                    }
+                    else
+                    {
+                        if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
+                            bufferConflict.Add((conflictAgent, conflictNode));
+                    }
+                }
+            }
+
+            if (counter < TrafficResolutionRepeat)
+            {
+                foreach (var (conflictAgent, conflictNode) in bufferConflict)
+                {
+                    var trafficState = Level3ConflictResolution(conflictAgent, agent.Robot.SerialNumber, conflictNode, [.. relaseNodes, giveNode], trafficMap, ++counter);
+                    if (trafficState == TrafficSolutionState.Complete || trafficState == TrafficSolutionState.GiveWay) return trafficState;
+                }
+            }
+            return TrafficSolutionState.UnableResolve;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{agent.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 3 xảy ra lỗi: {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private TrafficSolutionState FindAvoidableResolution(Agent agent1, Agent agent2, TrafficMap trafficMap)
+    {
+        try
+        {
+            if (agent2.State == TrafficSolutionState.GiveWay || agent2.State == TrafficSolutionState.LoopResolve) return TrafficSolutionState.Waitting;
+            if (agent2.ReleaseNodeIndex == -1 || agent1.ReleaseNodeIndex == -1) return TrafficSolutionState.Waitting;
+            var conflictNodes = agent2.Nodes.GetRange(agent2.ReleaseNodeIndex, agent2.Nodes.Count - agent2.ReleaseNodeIndex);
+            List<(Agent conflictAgent, TrafficNodeDto conflictNode)> bufferConflict = [];
+            for (int i = 1; i < conflictNodes.Count; i++)
+            {
+                if (conflictNodes[i].IsAvoidableNode)
+                {
+                    var remainingNodeAgent2 = agent2.Nodes.GetRange(agent2.ReleaseNodeIndex, agent2.Nodes.Count - agent2.ReleaseNodeIndex);
+                    foreach (var avoidablePath in conflictNodes[i].AvoidablePaths)
+                    {
+                        if (remainingNodeAgent2.Any(n => n.Id == avoidablePath[^1].Id)) continue;
+                        var givewayagent2 = conflictNodes.GetRange(1, i);
+                        var mergeNode = givewayagent2.LastOrDefault(n => agent1.Nodes.Any(a1 => a1.Id == n.Id));
+                        if (mergeNode is not null)
+                        {
+                            var avoidableIndex = givewayagent2.FindIndex(n => n.Id == mergeNode.Id);
+                            if (avoidableIndex != -1)
+                            {
+                                var giveWayNodes = givewayagent2.GetRange(avoidableIndex, givewayagent2.Count - avoidableIndex - 1);
+                                giveWayNodes.AddRange(avoidablePath);
+                                (var IsSuccess, string conflictRobotId, TrafficNodeDto? conflictNode) = GivewayCheckingReleaseNodes([.. giveWayNodes], agent1.Robot.SerialNumber, trafficMap);
+                                if (IsSuccess)
+                                {
+                                    trafficMap.AddLocker(agent1.Robot.SerialNumber, [.. giveWayNodes]);
+                                    var (state, message) = agent1.UpdateGiveWay([.. giveWayNodes], PathPlannger, Map);
+                                    Logger.Info($"{agent1.Robot.SerialNumber} giải quyết đối đầu với robot {agent2.Robot.SerialNumber} tại node {giveWayNodes[^1].Name} {(state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay ? "thành công" : $"xảy ra lỗi: {message}")}");
+                                    if (state == TrafficSolutionState.GiveWay)
+                                        trafficMap.GivewayResolution.Add(new()
+                                        {
+                                            RobotGive = agent1.Robot.SerialNumber,
+                                            RobotReceive = agent2.Robot.SerialNumber,
+                                            Nodes = [conflictNodes[i]]
+                                        });
+                                    return state;
+                                }
+                                else
+                                {
+                                    if (conflictNode is null)
+                                    {
+                                        Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột đối đầu có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
+                                    }
+                                    else
+                                    {
+                                        if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
+                                            bufferConflict.Add((conflictAgent, conflictNode));
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+            foreach (var (conflictAgent, conflictNode) in bufferConflict)
+            {
+                var trafficState = Level3ConflictResolution(conflictAgent, agent1.Robot.SerialNumber, conflictNode, [.. conflictNodes], trafficMap, 0);
+                if (trafficState == TrafficSolutionState.Complete || trafficState == TrafficSolutionState.GiveWay) return trafficState;
+            }
+
+            return TrafficSolutionState.UnableResolve;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột đối đầu xảy ra lỗi: {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private TrafficSolutionState LoopConflictResolution(TrafficConflict[] conflicts, TrafficMap trafficMap)
+    {
+        try
+        {
+            for (int i = 0; i < conflicts.Length; i++)
+            {
+                var conflictAvoid = conflicts.FirstOrDefault(c => c.AgentConflict.Robot.SerialNumber == conflicts[i].AgentRequest.Robot.SerialNumber);
+                if (conflictAvoid is null) continue;
+                var conflictResolutionState = LoopConflictLevel1Resolution(conflicts[i], conflictAvoid, trafficMap);
+                if (conflictResolutionState == TrafficSolutionState.Complete) return conflictResolutionState;
+            }
+            for (int i = 0; i < conflicts.Length; i++)
+            {
+                var conflictAvoid = conflicts.FirstOrDefault(c => c.AgentConflict.Robot.SerialNumber == conflicts[i].AgentRequest.Robot.SerialNumber);
+                if (conflictAvoid is null) continue;
+                var conflictResolutionState = LoopConflictLevel2Resolution(conflicts[i], conflictAvoid, trafficMap);
+                if (conflictResolutionState == TrafficSolutionState.Complete) return conflictResolutionState;
+            }
+            return TrafficSolutionState.UnableResolve;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"LoopConflictResolution: Hệ thống xảy ra lỗi - {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private TrafficSolutionState LoopConflictLevel1Resolution(TrafficConflict conflict1, TrafficConflict conflict2, TrafficMap trafficMap)
+    {
+        try
+        {
+            var agent1ConflictIndex = conflict1.ReleaseNodes.IndexOf(conflict1.NodeConflict);
+            var agent2ConflictIndex = conflict2.ReleaseNodes.IndexOf(conflict2.NodeConflict);
+            if (agent1ConflictIndex == -1 || agent2ConflictIndex == -1) return TrafficSolutionState.UnableResolve;
+            var agent1ConflictNodes = conflict1.ReleaseNodes.GetRange(0, agent1ConflictIndex);
+            var agent2ConflictNodes = conflict2.ReleaseNodes.GetRange(0, agent2ConflictIndex + 1);
+
+            Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} - release 1: [{conflict1.ReleaseNodes[0].Name} => {conflict1.ReleaseNodes[^1].Name} - {conflict1.NodeConflict.Name}] - release 2: [{conflict2.ReleaseNodes[0].Name} => {conflict2.ReleaseNodes[^1].Name} - {conflict2.NodeConflict.Name}]");
+
+            var avoidableNode = agent1ConflictNodes.FirstOrDefault(n => !agent2ConflictNodes.Any(n2 => n2.Id == n.Id));
+            if (avoidableNode is not null)
+            {
+                TrafficNodeDto[] releaseNodes = [.. conflict1.ReleaseNodes.GetRange(0, conflict1.ReleaseNodes.IndexOf(avoidableNode) + 1)];
+                (var conflictState, string conflictRobotId, TrafficNodeDto? conflictNode) = FirstCheckingReleaseNodes(releaseNodes, conflict1.AgentRequest, trafficMap);
+                if (conflictState == TrafficConflictState.None)
+                {
+                    Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} giải quyết xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} tại node {avoidableNode.Name} - release {releaseNodes[^1].Name} ");
+                    trafficMap.AddLocker(conflict1.AgentRequest.Robot.SerialNumber, [.. releaseNodes]);
+                    conflict1.AgentRequest.ReleaseNode = releaseNodes[^1];
+                    conflict1.AgentRequest.State = TrafficSolutionState.LoopResolve;
+                    trafficMap.GivewayResolution.Add(new()
+                    {
+                        RobotGive = conflict1.AgentRequest.Robot.SerialNumber,
+                        RobotReceive = conflict2.AgentRequest.Robot.SerialNumber,
+                        Nodes = [.. agent2ConflictNodes]
+                    });
+                    return TrafficSolutionState.Complete;
+                }
+            }
+            return TrafficSolutionState.Waitting;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"LoopConflictLevel1Resolution: Hệ thống xảy ra lỗi - {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private TrafficSolutionState LoopConflictLevel2Resolution(TrafficConflict conflict1, TrafficConflict conflict2, TrafficMap trafficMap)
+    {
+        try
+        {
+            var agent2ConflictIndex = conflict2.ReleaseNodes.IndexOf(conflict2.NodeConflict);
+            if (agent2ConflictIndex == -1) return TrafficSolutionState.UnableResolve;
+            var agent2ConflictNodes = conflict2.ReleaseNodes.GetRange(0, agent2ConflictIndex + 1);
+
+            Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} - release 1: [{conflict1.ReleaseNodes[0].Name} => {conflict1.ReleaseNodes[^1].Name} - {conflict1.NodeConflict.Name}] - release 2: [{conflict2.ReleaseNodes[0].Name} => {conflict2.ReleaseNodes[^1].Name} - {conflict2.NodeConflict.Name}]");
+
+            var avoidableNode = conflict1.ReleaseNodes.FirstOrDefault(n => !agent2ConflictNodes.Any(n2 => n2.Id == n.Id));
+            if (avoidableNode is not null)
+            {
+                TrafficNodeDto[] releaseNodes = [.. conflict1.ReleaseNodes.GetRange(0, conflict1.ReleaseNodes.IndexOf(avoidableNode) + 1)];
+                (var conflictState, string conflictRobotId, TrafficNodeDto? conflictNode) = FirstCheckingReleaseNodes(releaseNodes, conflict1.AgentRequest, trafficMap);
+                if (conflictState == TrafficConflictState.None)
+                {
+                    Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} giải quyết xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} tại node {avoidableNode.Name} - release {releaseNodes[^1].Name} ");
+                    trafficMap.AddLocker(conflict1.AgentRequest.Robot.SerialNumber, [.. releaseNodes]);
+                    conflict1.AgentRequest.ReleaseNode = releaseNodes[^1];
+                    conflict1.AgentRequest.State = TrafficSolutionState.LoopResolve;
+                    trafficMap.GivewayResolution.Add(new()
+                    {
+                        RobotGive = conflict1.AgentRequest.Robot.SerialNumber,
+                        RobotReceive = conflict2.AgentRequest.Robot.SerialNumber,
+                        Nodes = [.. agent2ConflictNodes]
+                    });
+                    return TrafficSolutionState.Complete;
+                }
+                else
+                {
+                    if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
+                    {
+                        var state = Level3ConflictResolution(conflictAgent, conflict1.AgentRequest.Robot.SerialNumber, avoidableNode, [.. releaseNodes, .. agent2ConflictNodes], trafficMap, 0);
+                        if (state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay)
+                        {
+                            trafficMap.Conflicts.RemoveAll(c => c.AgentRequest.Robot.SerialNumber == conflictAgent.Robot.SerialNumber);
+                            return TrafficSolutionState.Complete;
+                        }
+                        return TrafficSolutionState.UnableResolve;
+                    }
+                }
+            }
+            return TrafficSolutionState.Waitting;
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"LoopConflictLevel2Resolution: Hệ thống xảy ra lỗi - {ex.Message}");
+            return TrafficSolutionState.UnableResolve;
+        }
+    }
+
+    private void ComputeHandler(object? state)
+    {
+        try
+        {
+            foreach (var trafficmap in TrafficMaps)
+            {
+                var agents = trafficmap.Value.Agents.Values.ToList();
+                foreach (var agent in agents)
+                {
+                    CheckConflict(agent, trafficmap.Value);
+                }
+                var conflicts = trafficmap.Value.Conflicts.ToList();
+                for (int i = 0; i < conflicts.Count; i++)
+                {
+                    int repeat = 0;
+                    bool isProccessed = false;
+                    string agentConflict = conflicts[i].AgentConflict.Robot.SerialNumber;
+                    List<TrafficConflict> loopConflicts = [conflicts[i]];
+                    while (repeat++ <= TrafficResolutionRepeat)
+                    {
+                        var loopConflict = conflicts.FirstOrDefault(cl => cl.AgentRequest.Robot.SerialNumber == agentConflict);
+                        if (loopConflict is null) break;
+                        loopConflicts.Add(loopConflict);
+                        if (loopConflict.AgentConflict.Robot.SerialNumber == conflicts[i].AgentRequest.Robot.SerialNumber)
+                        {
+                            if (loopConflicts.Count > 2)
+                            {
+                                var resolutionstate = LoopConflictResolution([.. loopConflicts], trafficmap.Value);
+                                isProccessed = resolutionstate == TrafficSolutionState.Complete;
+                            }
+                            break;
+                        }
+                        else agentConflict = loopConflict.AgentConflict.Robot.SerialNumber;
+                    }
+                    if (isProccessed) break;
+
+                    if (conflicts[i].State == TrafficConflictState.Confrontation)
+                    {
+                        if (conflicts.Any(c => c.State == TrafficConflictState.Confrontation && c.AgentRequest.Robot.SerialNumber == conflicts[i].AgentConflict.Robot.SerialNumber))
+                        {
+                            var resolutionstate = FindAvoidableResolution(conflicts[i].AgentRequest, conflicts[i].AgentConflict, trafficmap.Value);
+                            if (resolutionstate == TrafficSolutionState.Complete || resolutionstate == TrafficSolutionState.GiveWay)
+                            {
+                                trafficmap.Value.Conflicts.Remove(conflicts[i]);
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+
+            computeTimer?.Change(intervalTime, 0);
+        }
+        catch (Exception ex)
+        {
+            Logger.Warning($"TrafficCompute: Hệ thống xảy ra lỗi - {ex.Message}");
+            computeTimer?.Change(intervalTime, 0);
+        }
+    }
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        if (!Enable) return;
+        computeTimer = new Timer(ComputeHandler, null, Timeout.Infinite, Timeout.Infinite);
+        computeTimer.Change(intervalTime, 0);
+        await Task.Yield();
+    }
+
+    public async Task StopAsync(CancellationToken cancellationToken)
+    {
+        if (computeTimer != null)
+        {
+            computeTimer.Change(Timeout.Infinite, Timeout.Infinite);
+            await computeTimer.DisposeAsync();
+        }
+    }
+}

RobotNet.RobotManager/Services/Traffic/TrafficMap.cs

@@ -0,0 +1,26 @@
+using RobotNet.RobotShares.Dtos;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class TrafficMap
+{
+    public Guid MapId { get; set; }
+    public Dictionary<string, Agent> Agents { get; set; } = [];
+    public Dictionary<string, List<TrafficNodeDto>> Locked { get; set; } = [];
+    public List<TrafficConflict> Conflicts { get; set; } = [];
+    public List<TrafficGiveway> GivewayResolution { get; set; } = [];
+
+    public void AddLocker(string robotId, TrafficNodeDto[] releaseNodes)
+    {
+        //Console.WriteLine($"{robotId} Add Locker: {string.Join(", ", releaseNodes.Select(n => n.Name))}");
+        if (Locked.TryGetValue(robotId, out List<TrafficNodeDto>? lockedNodes) && lockedNodes is not null) Locked[robotId].AddRange(releaseNodes.Where(n => !lockedNodes.Any(ln => ln.Id == n.Id)));
+        else Locked.Add(robotId, [.. releaseNodes]);
+    }
+
+    public void UpdateLocker(string robotId, TrafficNodeDto[] releaseNodes)
+    {
+        //Console.WriteLine($"{robotId} Update Locker: {string.Join(", ", releaseNodes.Select(n => n.Name))}");
+        if (Locked.TryGetValue(robotId, out _)) Locked[robotId] = [.. releaseNodes];
+        else Locked.Add(robotId, [.. releaseNodes]);
+    }
+}

RobotNet.RobotManager/Services/Traffic/TrafficMath.cs

@@ -0,0 +1,158 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.RobotShares.Dtos;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class TrafficMath
+{
+    public static bool Edge1IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
+    {
+        if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.One) return true;
+
+        double dx = edge.X2 - edge.X1;
+        double dy = edge.Y2 - edge.Y1;
+        var length = Math.Sqrt(Math.Pow(dx, 2) + Math.Pow(dy, 2));
+        if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;
+
+        double distance = Math.Abs(dy * centerCircle.X - dx * centerCircle.Y + (edge.X2 * edge.Y1 - edge.X1 * edge.Y2)) / length;
+        if (distance > radius) return false;
+
+        double a = dx * dx + dy * dy;
+        double b = 2 * (dx * (edge.X1 - centerCircle.X) + dy * (edge.Y1 - centerCircle.Y));
+        double c = (edge.X1 - centerCircle.X) * (edge.X1 - centerCircle.X) + (edge.Y1 - centerCircle.Y) * (edge.Y1 - centerCircle.Y) - radius * radius;
+
+        double delta = b * b - 4 * a * c;
+        if (delta < 0) return false;
+
+        double t1 = (-b + Math.Sqrt(delta)) / (2 * a);
+        double t2 = (-b - Math.Sqrt(delta)) / (2 * a);
+
+        return (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1);
+    }
+
+    public static bool Edge2IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
+    {
+        if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.Two) return true;
+
+        var length = Math.Sqrt(Math.Pow(edge.X2 - edge.X1, 2) + Math.Pow(edge.Y2 - edge.Y1, 2));
+        if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;
+
+        double step = 0.1 / length;
+        for (double t = 0; t <= 1.001; t += step)
+        {
+            (double x1, double y1) = CurveDegreeTwo(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.X2, edge.Y2);
+            if (Math.Sqrt(Math.Pow(x1 - centerCircle.X, 2) + Math.Pow(y1 - centerCircle.Y, 2)) < radius) return true;
+        }
+        return false;
+    }
+
+    public static bool Edge3IntersectEdge(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
+    {
+        if (edge.TrajectoryDegree != MapShares.Enums.TrajectoryDegree.Three) return true;
+
+        var length = Math.Sqrt(Math.Pow(edge.X2 - edge.X1, 2) + Math.Pow(edge.Y2 - edge.Y1, 2));
+        if (length == 0) return Math.Sqrt(Math.Pow(edge.X1 - centerCircle.X, 2) + Math.Pow(edge.Y1 - centerCircle.Y, 2)) <= radius;
+
+        double step = 0.1 / length;
+        for (double t = 0; t <= 1.001; t += step)
+        {
+            (double x1, double y1) = CurveDegreeThree(t, edge.X1, edge.Y1, edge.ControlPoint1X, edge.ControlPoint1Y, edge.ControlPoint2X, edge.ControlPoint2Y, edge.X2, edge.Y2);
+            if (Math.Sqrt(Math.Pow(x1 - centerCircle.X, 2) + Math.Pow(y1 - centerCircle.Y, 2)) < radius) return true;
+        }
+        return false;
+    }
+
+    public static bool EdgeIntersectCircle(TrafficNodeDto centerCircle, double radius, EdgeCaculatorModel edge)
+    {
+        if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One) return Edge1IntersectEdge(centerCircle, radius, edge);
+        else if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two) return Edge2IntersectEdge(centerCircle, radius, edge);
+        else if (edge.TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Three) return Edge3IntersectEdge(centerCircle, radius, edge);
+        return true;
+    }
+
+    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 CalTurningRadius(AgentModel model, double offsetLength, double offsetWidth)
+    {
+        var navigationPointX = offsetLength - model.NavigationPointX;
+        var navigationPointY = offsetWidth - model.NavigationPointY;
+        var length = model.Length + 2 * offsetLength;
+        var width = model.Width + 2 * offsetWidth;
+        double d1 = Math.Sqrt(navigationPointX * navigationPointX + navigationPointY * navigationPointY);
+        double d2 = Math.Sqrt((length - navigationPointX) * (length - navigationPointX) + navigationPointY * navigationPointY);
+        double d3 = Math.Sqrt(navigationPointX * navigationPointX + (width - navigationPointY) * (width - navigationPointY));
+        double d4 = Math.Sqrt((length - navigationPointX) * (length - navigationPointX) + (width - navigationPointY) * (width - navigationPointY));
+
+        return Math.Max(Math.Max(d1, d2), Math.Max(d3, d4));
+    }
+
+    public static TrafficLockedShapeDto CalRobotRectShape(AgentModel model, double offsetLength, double offsetWidth, double x, double y, double theta)
+    {
+        double x1 = model.NavigationPointX - offsetLength;
+        double y1 = model.NavigationPointY - offsetWidth;
+        double x2 = model.Length + model.NavigationPointX + offsetLength;
+        double y2 = model.NavigationPointY - offsetWidth;
+        double x3 = model.Length + model.NavigationPointX + offsetLength;
+        double y3 = model.Width + model.NavigationPointY + offsetWidth;
+        double x4 = model.NavigationPointX - offsetLength;
+        double y4 = model.Width + model.NavigationPointY + offsetWidth;
+
+        return new()
+        {
+            Type = TrafficLockedShapeType.Rectangle,
+            X1 = x1 * Math.Cos(theta) - y1 * Math.Sin(theta) + x,
+            Y1 = x1 * Math.Sin(theta) + y1 * Math.Cos(theta) + y,
+            X2 = x2 + Math.Cos(theta) - y2 * Math.Sin(theta) + x,
+            Y2 = x2 * Math.Sin(theta) + y2 * Math.Cos(theta) + y,
+            X3 = x3 + Math.Cos(theta) - y3 * Math.Sin(theta) + x,
+            Y3 = x3 * Math.Sin(theta) + y3 * Math.Cos(theta) + y,
+            X4 = x4 + Math.Cos(theta) - y4 * Math.Sin(theta) + x,
+            Y4 = x4 * Math.Sin(theta) + y4 * Math.Cos(theta) + y
+        };
+    }
+
+    public static bool RectIntersectCircle(TrafficNodeDto centerCircle, double radius, double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4)
+    {
+        double xMin = Math.Min(Math.Min(x1, x2), Math.Min(x3, x4));
+        double xMax = Math.Max(Math.Max(x1, x2), Math.Max(x3, x4));
+        double yMin = Math.Min(Math.Min(y1, y2), Math.Min(y3, y4));
+        double yMax = Math.Max(Math.Max(y1, y2), Math.Max(y3, y4));
+
+        double xClosest = Math.Max(xMin, Math.Min(centerCircle.X, xMax));
+        double yClosest = Math.Max(yMin, Math.Min(centerCircle.Y, yMax));
+
+        double distance = Math.Sqrt((centerCircle.X - xClosest) * (centerCircle.X - xClosest) + (centerCircle.Y - yClosest) * (centerCircle.Y - yClosest));
+        return distance <= radius;
+    }
+
+    public static bool RectIntersectRect(TrafficLockedShapeDto rect1, TrafficLockedShapeDto rect2)
+    {
+        double x1Min = Math.Min(Math.Min(rect1.X1, rect1.X2), Math.Min(rect1.X3, rect1.X4));
+        double x1Max = Math.Max(Math.Max(rect1.X1, rect1.X2), Math.Max(rect1.X3, rect1.X4));
+        double y1Min = Math.Min(Math.Min(rect1.Y1, rect1.Y2), Math.Min(rect1.Y3, rect1.Y4));
+        double y1Max = Math.Max(Math.Max(rect1.Y1, rect1.Y2), Math.Max(rect1.Y3, rect1.Y4));
+
+        double x2Min = Math.Min(Math.Min(rect2.X1, rect2.X2), Math.Min(rect2.X3, rect2.X4));
+        double x2Max = Math.Max(Math.Max(rect2.X1, rect2.X2), Math.Max(rect2.X3, rect2.X4));
+        double y2Min = Math.Min(Math.Min(rect2.Y1, rect2.Y2), Math.Min(rect2.Y3, rect2.Y4));
+        double y2Max = Math.Max(Math.Max(rect2.Y1, rect2.Y2), Math.Max(rect2.Y3, rect2.Y4));
+
+        bool xOverlap = x1Min <= x2Max && x2Min <= x1Max;
+        bool yOverlap = y1Min <= y2Max && y2Min <= y1Max;
+
+        return xOverlap && yOverlap;
+    }
+}

RobotNet.RobotManager/Services/Traffic/TrafficPublisher.cs

@@ -0,0 +1,84 @@
+using Microsoft.AspNetCore.SignalR;
+using RobotNet.RobotManager.Hubs;
+using RobotNet.RobotShares.Dtos;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class TrafficPublisher(TrafficManager TrafficManager, IHubContext<TrafficHub> TrafficHub, LoggerController<TrafficPublisher> Logger) : IHostedService
+{
+    private const int intervalTime = 2000;
+    private WatchTimer<TrafficPublisher>? Timer;
+
+    public readonly Dictionary<Guid, string> TrafficMapActive = [];
+
+    public List<TrafficAgentDto> GetAgents(Guid MapId)
+    {
+        if (TrafficManager.TrafficMaps.TryGetValue(MapId, out TrafficMap? trafficMap))
+        {
+            List<TrafficAgentDto> agents = [];
+            List<TrafficNodeDto> lockedNodes = [];
+            foreach (var agent in trafficMap.Agents)
+            {
+                if (trafficMap.Locked.TryGetValue(agent.Key, out List<TrafficNodeDto>? locked) && locked is not null) lockedNodes = locked;
+                var trafficConflict = trafficMap.Conflicts.FirstOrDefault(c => c.AgentRequest.Robot.SerialNumber == agent.Key);
+                agents.Add(new()
+                {
+                    RobotId = agent.Key,
+                    State = agent.Value.State,
+                    ReleaseNode = agent.Value.ReleaseNode,
+                    Nodes = agent.Value.Nodes,
+                    InNode = agent.Value.Nodes[agent.Value.InNodeIndex],
+                    LockedNodes = lockedNodes,
+                    SubNodes = agent.Value.SubNodes,
+                    GiveWayNodes = agent.Value.GivewayNodes,
+                    ConflictAgentId = trafficConflict is null ? "" : trafficConflict.AgentConflict.Robot.SerialNumber,
+                    ConflictNode = trafficConflict?.NodeConflict,
+                });
+            }
+            foreach (var agent in trafficMap.Locked)
+            {
+                if (agents.Any(a => a.RobotId == agent.Key)) continue;
+                agents.Add(new()
+                {
+                    RobotId = agent.Key,
+                    LockedNodes = agent.Value,
+                });
+            }
+            return agents;
+        }
+        return [];
+    }
+
+    private void TimerHandler()
+    {
+        try
+        {
+            foreach (var mapActive in TrafficMapActive)
+            {
+                var agents = GetAgents(mapActive.Key);
+                if (agents.Count > 0)
+                {
+                    var pub = Task.Run(async () => await TrafficHub.Clients.Client(mapActive.Value).SendAsync("TrafficAgentUpdated", agents));
+                    pub.Wait();
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger.Error($"Robot Publisher Error: {ex}");
+        }
+    }
+
+    public async Task StartAsync(CancellationToken cancellationToken)
+    {
+        Timer = new(intervalTime, TimerHandler, Logger);
+        Timer.Start();
+        await Task.Yield();
+    }
+
+    public Task StopAsync(CancellationToken cancellationToken)
+    {
+        Timer?.Dispose();
+        return Task.CompletedTask;
+    }
+}

RobotNet.RobotManager/Services/Traffic/TrafficSolution.cs

@@ -0,0 +1,14 @@
+using RobotNet.RobotShares.Dtos;
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Traffic;
+
+public class TrafficSolution
+{
+    public TrafficSolutionState State { get; set; }
+    public TrafficNodeDto[] Nodes { get; set; } = [];
+    public TrafficEdgeDto[] Edges { get; set; } = [];
+    public TrafficNodeDto ReleaseNode { get; set; } = new();
+    public List<TrafficNodeDto> GivewayNodes { get; set; } = [];
+    public List<TrafficEdgeDto> GivewayEdges { get; set; } = [];
+}

RobotNet.RobotManager/Services/WatchTimer.cs

@@ -0,0 +1,76 @@
+using System.Diagnostics;
+
+namespace RobotNet.RobotManager.Services;
+
+public class WatchTimer<T>(int Interval, Action Callback, LoggerController<T>? Logger) : IDisposable where T : class
+{
+    private System.Threading.Timer? Timer;
+    private readonly Stopwatch Watch = new();
+    public bool Disposed;
+
+    private void Handler(object? state)
+    {
+        try
+        {
+            Watch.Restart();
+
+            Callback.Invoke();
+
+            Watch.Stop();
+            if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
+            {
+                Timer?.Change(Interval, Timeout.Infinite);
+            }
+            else
+            {
+                Timer?.Change(Interval - Watch.ElapsedMilliseconds, Timeout.Infinite);
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger?.Error($"WatchTimer Error: {ex}");
+            Timer?.Change(Interval, Timeout.Infinite);
+        }
+    }
+
+    public void Start()
+    {
+        if (!Disposed)
+        {
+            Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
+            Timer.Change(Interval, 0);
+        }
+        else throw new ObjectDisposedException(nameof(WatchTimer<T>));
+    }
+
+    public void Stop()
+    {
+        if (Disposed) return;
+        if (Timer != null)
+        {
+            Timer.Change(Timeout.Infinite, Timeout.Infinite);
+            Timer.Dispose();
+            Timer = null;
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        if (disposing) Stop();
+
+        Disposed = true;
+    }
+
+    ~WatchTimer()
+    {
+        Dispose(false);
+    }
+}

RobotNet.RobotManager/Services/WatchTimerAsync.cs

@@ -0,0 +1,76 @@
+using System.Diagnostics;
+
+namespace RobotNet.RobotManager.Services;
+
+public class WatchTimerAsync<T>(int Interval, Func<Task> Callback, LoggerController<T>? Logger) : IDisposable where T : class
+{
+    private System.Threading.Timer? Timer;
+    private readonly Stopwatch Watch = new();
+    public bool Disposed;
+
+    private async void Handler(object? state)
+    {
+        try
+        {
+            Watch.Restart();
+
+            await Callback.Invoke();
+
+            Watch.Stop();
+            if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
+            {
+                Timer?.Change(Interval, Timeout.Infinite);
+            }
+            else
+            {
+                Timer?.Change(Interval - Watch.ElapsedMilliseconds, Timeout.Infinite);
+            }
+        }
+        catch (Exception ex)
+        {
+            Logger?.Error($"WatchTimer Error: {ex}");
+            Timer?.Change(Interval, Timeout.Infinite);
+        }
+    }
+
+    public void Start()
+    {
+        if (!Disposed)
+        {
+            Timer = new Timer(Handler, null, Timeout.Infinite, Timeout.Infinite);
+            Timer.Change(Interval, 0);
+        }
+        else throw new ObjectDisposedException(nameof(WatchTimerAsync<T>));
+    }
+
+    public void Stop()
+    {
+        if (Disposed) return;
+        if (Timer != null)
+        {
+            Timer.Change(Timeout.Infinite, Timeout.Infinite);
+            Timer.Dispose();
+            Timer = null;
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (Disposed) return;
+
+        if (disposing) Stop();
+
+        Disposed = true;
+    }
+
+    ~WatchTimerAsync()
+    {
+        Dispose(false);
+    }
+}

RobotNet.RobotManager/appsettings.json

@@ -0,0 +1,78 @@
+{
+  "ConnectionStrings": {
+    "DefaultConnection": "Server=172.20.235.170;Database=RobotNet.RobotEditor;User Id=sa;Password=robotics@2022;TrustServerCertificate=True;MultipleActiveResultSets=true"
+  },
+  "Logging": {
+    "LogLevel": {
+      "Default": "Information",
+      "Microsoft.AspNetCore": "Warning",
+      "System.Net.Http.HttpClient": "Warning",
+      "OpenIddict.Validation.OpenIddictValidationDispatcher": "Warning",
+      "OpenIddict.Client.OpenIddictClientDispatcher": "Warning",
+      "Microsoft.EntityFrameworkCore.Database": "Warning",
+      "Polly": "Warning"
+    }
+  },
+  "AllowedHosts": "*",
+  "MinIO": {
+    "UsingLocal": false,
+    "Endpoint": "172.20.235.170:9000",
+    "Bucket": "mapeditor",
+    "User": "minio",
+    "Password": "robotics"
+  },
+  "PathPlanning": {
+    "ResolutionSplit": 0.1,
+    "Type": "Angle", // Angle, StartDirection, EndDirection with Diffirental and Omidrive Model
+    "StartDirection": "None", // None, FORWARD, BACKWARD, NON Ewith Diffirental and Omidrive Model
+    "EndDirection": "None" // None, FORWARD, BACKWARD, NON Ewith Diffirental and Omidrive Model 
+  },
+  "VDA5050Setting": {
+    "ServerEnable": true,
+    "HostServer": "127.0.0.1",
+    "Port": "1883",
+    "UserName": "robotics",
+    "PassWord": "robotics",
+    "Manufacturer": "phenikaaX",
+    "Version": "0.0.1",
+    "Repeat": 2,
+    "ConnectionTimeoutSeconds": 30,
+    "ConnectionBacklog": 10,
+    "KeepAliveInterval": 60,
+    "CheckingRobotMsgTimout": 60
+  },
+  "OpenIddictClientProviderOptions": {
+    "Issuer": "https://localhost:7061/",
+    "Audiences": [
+      "robotnet-robot-manager"
+    ],
+    "ClientId": "robotnet-robot-manager",
+    "ClientSecret": "469B2DEB-660E-4C91-97C7-D69550D9969D",
+    "Scopes": [
+      "robotnet-map-api"
+    ]
+  },
+  "MapManager": {
+    "Url": "https://localhost:7177",
+    "Scopes": [
+      "robotnet-map-api"
+    ]
+  },
+  "ACSStatusConfig": {
+    "SiteCode": "VN03",
+    "AreaCode": "DA3_FL1",
+    "AreaArea": "DA3_WM",
+    "BatteryId": "bidpnkx0001"
+  },
+  "TrafficConfig": {
+    "Enable": false,
+    "IntervalTime": 1000,
+    "CheckingDistanceMax": 10,
+    "CheckingDistanceMin": 5,
+    "OffsetLength": 0.2,
+    "OffsetWidth": 0.2,
+    "ResolutionRepeat": 20,
+    "AvoidableNodeMax": 3,
+    "DeviationDistance": 0.5
+  }
+}

RobotNet.RobotManager/nlog.config

@@ -0,0 +1,34 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<nlog xmlns="http://www.nlog-project.org/schemas/NLog.xsd"
+      xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+      autoReload="true">
+
+	<extensions>
+		<add assembly="NLog.Web.AspNetCore"/>
+	</extensions>
+
+	<targets>
+		<target xsi:type="File" name="robotManagerLogFile" fileName="${basedir}/robotManagerlogs/${shortdate}.log" maxArchiveFiles="90" archiveEvery="Day" >
+			<layout type='JsonLayout'>
+				<attribute name='time' layout='${date:format=HH\:mm\:ss.ffff}' />
+				<attribute name='level' layout='${level:upperCase=true}'/>
+				<attribute name='logger' layout='${logger}' />
+				<attribute name='message' layout='${message}' />
+				<attribute name='exception' layout='${exception:format=tostring}' />
+			</layout>
+		</target>
+		<target xsi:type="File" name="openACSLogFile" fileName="${basedir}/openACSlogs/${shortdate}.log" maxArchiveFiles="90" archiveEvery="Day" >
+			<layout type='JsonLayout'>
+				<attribute name='time' layout='${date:format=HH\:mm\:ss.ffff}' />
+				<attribute name='level' layout='${level:upperCase=true}'/>
+				<attribute name='logger' layout='${logger}' />
+				<attribute name='message' layout='${message}' />
+				<attribute name='exception' layout='${exception:format=tostring}' />
+			</layout>
+		</target>
+	</targets>
+	<rules>
+		<logger name="RobotNet.RobotManager.*" minlevel="Debug" writeto="robotManagerLogFile" />
+		<logger name="RobotNet.RobotManager.Services.OpenACS.*" minlevel="Debug" writeto="openACSLogFile" />
+	</rules>
+</nlog>