1 Commits

Author SHA1 Message Date
Đăng Nguyễn
4aed0da992 update mapmanager pages 2025-09-25 21:03:10 +07:00
60 changed files with 327 additions and 1234 deletions

View File

@@ -0,0 +1,5 @@
<h3>MapView</h3>
@code {
}

View File

@@ -36,6 +36,12 @@
</NavLink>
</div>
<div class="nav-item px-3">
<NavLink class="nav-link" href="maps-manager">
<span class="bi bi-lock-nav-menu" aria-hidden="true"></span> Map Manager
</NavLink>
</div>
<AuthorizeView>
<Authorized>
<div class="nav-item px-3">

View File

@@ -0,0 +1,181 @@
@page "/maps-manager"
@using MudBlazor
@using RobotApp.Common.Shares.Dtos
@attribute [Authorize]
@inject HttpClient Http
<PageTitle>Map Manager</PageTitle>
<style>
.selected {
background-color: #3399ff !important;
}
.selected > td {
color: white !important;
}
.selected > td .mud-input {
color: white !important;
}
</style>
<div class="d-flex flex-row w-100 h-100 p-2 overflow-hidden">
<div class="d-flex h-100 flex-column flex-grow-1 pe-2">
<div class="d-flex justify-content-between align-items-center mb-2">
<MudTextField Value="txtSearch" T="string" Label="Search" Variant="Variant.Outlined" Adornment="Adornment.End" AdornmentIcon="@Icons.Material.Filled.Search"
AdornmentColor="Color.Secondary" ValueChanged="TextSearchChanged" Style="max-width: 550px" />
<MudButton Class="ms-3" StartIcon="@Icons.Material.Filled.Add" Variant="Variant.Filled" Color="Color.Primary" Size="Size.Large">
NEW
</MudButton>
</div>
<div class="d-flex" style="height: 92%">
<MudTable Class="w-100" @ref="Table" Items="@MapsShow" T="MapDto" Dense=true Hover=true ReadOnly=true FixedHeader=true RowClass="cursor-pointer" Striped="true"
ServerData="ReloadData" Loading=@IsLoading Outlined="true" RowClassFunc="@SelectedRowClassFunc" OnRowClick="RowClickEvent" Height="95%">
<HeaderContent>
<MudTh>Nr</MudTh>
<MudTh>Name</MudTh>
<MudTh>Width (m)</MudTh>
<MudTh>Height (m)</MudTh>
<MudTh>Resolution (m/px)</MudTh>
<MudTh>OriginX</MudTh>
<MudTh>OriginY</MudTh>
<MudTh></MudTh>
</HeaderContent>
<RowTemplate>
<MudTd DataLabel="Nr">
@(Table?.CurrentPage * Table?.RowsPerPage + MapsShow.IndexOf(context) + 1)
</MudTd>
<MudTd DataLabel="Name">
@context.Name
</MudTd>
<MudTd DataLabel="Width">
@context.Width
</MudTd>
<MudTd DataLabel="Height">
@context.Height
</MudTd>
<MudTd DataLabel="Resolution">
@context.Resolution
</MudTd>
<MudTd DataLabel="OriginX">
@context.OriginX
</MudTd>
<MudTd DataLabel="OriginY">
@context.OriginY
</MudTd>
<MudTd>
<div class="d-flex flex-row-reverse">
<MudMenu Icon="@Icons.Material.Filled.MoreVert" AnchorOrigin="Origin.BottomCenter">
<MudMenuItem Icon="@Icons.Material.Filled.Edit" IconColor="Color.Info">Edit</MudMenuItem>
<MudMenuItem Icon="@Icons.Material.Filled.Delete" IconColor="Color.Error">Delete</MudMenuItem>
</MudMenu>
</div>
</MudTd>
</RowTemplate>
<PagerContent>
<div class="d-flex w-100 flex-row-reverse">
<MudTablePager Style="width: 100%;" PageSizeOptions="new[] {25 , 100, 200}" />
</div>
</PagerContent>
</MudTable>
</div>
</div>
<div class="map-preview">
</div>
</div>
@code {
private string txtSearch = "";
private bool IsLoading = false;
private List<MapDto> Maps = [];
private List<MapDto> MapsShow = [];
private int selectedRowNumber = -1;
private MapDto MapSelected = new();
private MudTable<MapDto>? Table; protected override async Task OnAfterRenderAsync(bool firstRender)
{
await base.OnAfterRenderAsync(firstRender);
if (!firstRender) return;
await LoadMaps();
}
private async Task LoadMaps()
{
try
{
IsLoading = true;
Maps.Clear();
StateHasChanged();
var maps = await Http.GetFromJsonAsync<IEnumerable<MapDto>>($"api/MapsManager?txtSearch={txtSearch}");
Maps.AddRange(maps ?? []);
Table?.ReloadServerData();
IsLoading = false;
StateHasChanged();
}
catch
{
return;
}
}
private void TextSearchChanged(string text)
{
txtSearch = text;
Table?.ReloadServerData();
}
private bool FilterFunc(MapDto map)
{
if (string.IsNullOrWhiteSpace(txtSearch))
return true;
if (map.Name is not null && map.Name.Contains(txtSearch, StringComparison.OrdinalIgnoreCase))
return true;
if ($"{map.Name}".Contains(txtSearch))
return true;
return false;
}
private Task<TableData<MapDto>> ReloadData(TableState state, CancellationToken _)
{
MapsShow.Clear();
var tasks = new List<MapDto>();
Maps.ForEach(map =>
{
if (FilterFunc(map)) tasks.Add(map);
});
MapsShow = tasks.Skip(state.Page * state.PageSize).Take(state.PageSize).ToList();
return Task.FromResult(new TableData<MapDto>() { TotalItems = tasks.Count, Items = MapsShow });
}
private void RowClickEvent(TableRowClickEventArgs<MapDto> tableRowClickEventArgs) { }
private string SelectedRowClassFunc(MapDto element, int rowNumber)
{
if (selectedRowNumber == rowNumber && Table?.SelectedItem != null && !Table.SelectedItem.Equals(element))
{
return string.Empty;
}
else if (selectedRowNumber == rowNumber && Table?.SelectedItem != null && Table.SelectedItem.Equals(element))
{
return "selected";
}
else if (Table?.SelectedItem != null && Table.SelectedItem.Equals(element))
{
selectedRowNumber = rowNumber;
MapSelected = element;
// NavigationMapPreviewRef.SetMapPreview(MapSelected);
return "selected";
}
else
{
return string.Empty;
}
}
}

View File

@@ -11,6 +11,11 @@
<ItemGroup>
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly" Version="9.0.1" />
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Authentication" Version="9.0.1" />
<PackageReference Include="MudBlazor" Version="8.12.0" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\RobotApp.Common.Shares\RobotApp.Common.Shares.csproj" />
</ItemGroup>
</Project>

View File

@@ -8,3 +8,4 @@
@using Microsoft.AspNetCore.Components.Web.Virtualization
@using Microsoft.JSInterop
@using RobotApp.Client
@using Microsoft.AspNetCore.Authorization

View File

@@ -0,0 +1,13 @@
namespace RobotApp.Common.Shares.Dtos;
public class MapDto
{
public Guid Id { get; set; }
public string Name { get; set; } = string.Empty;
public string Description { get; set; } = string.Empty;
public double Width { get; set; }
public double Height { get; set; }
public double Resolution { get; set; }
public double OriginX { get; set; }
public double OriginY { get; set; }
}

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@@ -1,7 +1,7 @@
Microsoft Visual Studio Solution File, Format Version 12.00
# Visual Studio Version 17
VisualStudioVersion = 17.12.35707.178
# Visual Studio Version 18
VisualStudioVersion = 18.0.11018.127 d18.0
MinimumVisualStudioVersion = 10.0.40219.1
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp", "RobotApp\RobotApp.csproj", "{BF0BB137-2EF9-4E1B-944E-9BF41C5284F7}"
EndProject
@@ -37,4 +37,7 @@ Global
GlobalSection(SolutionProperties) = preSolution
HideSolutionNode = FALSE
EndGlobalSection
GlobalSection(ExtensibilityGlobals) = postSolution
SolutionGuid = {CCB0B2E5-3C19-4B2E-B229-08A74F6EF27D}
EndGlobalSection
EndGlobal

View File

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

View File

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

View File

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

View File

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

View File

@@ -4,9 +4,6 @@ namespace RobotApp.Interfaces;
public enum NavigationState
{
None,
Initializing,
Initialized,
Idle,
Moving,
Rotating,
@@ -16,7 +13,7 @@ public enum NavigationState
public enum NavigationProccess
{
None,
Started,
InProgress,
Completed,
Failed,
@@ -25,13 +22,12 @@ public enum NavigationProccess
public interface INavigation
{
bool Driving { get; }
NavigationState State { get; }
void Move(Node[] nodes, Edge[] edges);
void MoveStraight(double x, double y);
void Rotate(double angle);
void Paused();
void Resume();
void UpdateOrder(int lastBaseSequence);
void CancelMovement();
NavigationProccess Proccess { get; }
bool Move(Node nodes, Edge edges);
bool MoveStraight(double x, double y);
bool Rotate(double angle);
bool Paused();
bool Resume();
bool CancelMovement();
}

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@@ -24,7 +24,6 @@
</ItemGroup>
<ItemGroup>
<Folder Include="Services\Robot\Simulation\" />
<Folder Include="wwwroot\lib\" />
</ItemGroup>

View File

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

View File

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

View File

@@ -27,6 +27,7 @@ public class MQTTClient : IAsyncDisposable
Logger = logger;
MqttClientFactory = new MqttClientFactory();
MqttClient = MqttClientFactory.CreateMqttClient();
MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
.WithTcpServer(setting.HostServer, setting.Port)
.WithCredentials(setting.UserName, setting.Password)
@@ -37,18 +38,13 @@ public class MQTTClient : IAsyncDisposable
.WithTopicFilter(f => f.WithTopic(VDA5050Topic.ORDER.ToTopicString()))
.WithTopicFilter(f => f.WithTopic(VDA5050Topic.INSTANTACTIONS.ToTopicString()))
.Build();
}
public async Task ConnectAsync(CancellationToken cancellationToken = default)
{
MqttClient = MqttClientFactory.CreateMqttClient();
MqttClient.DisconnectedAsync += async delegate (MqttClientDisconnectedEventArgs args)
{
if (args.ClientWasConnected && !IsReconnecing)
{
IsReconnecing = true;
Logger.Warning("Mất kết nối tới broker, đang cố gắng kết nối lại...");
if (MqttClient.IsConnected) await MqttClient.DisconnectAsync();
if (MqttClient.IsConnected) await MqttClient.DisconnectAsync();
MqttClient.Dispose();
await ConnectAsync();
@@ -56,10 +52,15 @@ public class MQTTClient : IAsyncDisposable
IsReconnecing = false;
}
};
}
public async Task ConnectAsync(CancellationToken cancellationToken = default)
{
while (!cancellationToken.IsCancellationRequested)
{
try
{
MqttClient ??= MqttClientFactory.CreateMqttClient();
var connection = await MqttClient.ConnectAsync(MqttClientOptions, cancellationToken);
if (connection.ResultCode != MqttClientConnectResultCode.Success || !MqttClient.IsConnected)
Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");

View File

@@ -1,4 +1,4 @@
namespace RobotApp.Services.Robot.Navigation.Algorithm;
namespace RobotApp.Services.Navigation.Algorithm;
public class FuzzyLogic
{

View File

@@ -1,4 +1,4 @@
namespace RobotApp.Services.Robot.Navigation.Algorithm;
namespace RobotApp.Services.Navigation.Algorithm;
public class PID
{

View File

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

View File

@@ -1,4 +1,4 @@
namespace RobotApp.Services.Robot.Navigation
namespace RobotApp.Services.Navigation
{
public class NavigationManager
{

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@@ -29,6 +29,6 @@ public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService
await Task.Delay(1000, stoppingToken);
if (RobotConnection.IsConnected) break;
}
await PubFactsheet();
}
}

View File

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

View File

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

View File

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

View File

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

View File

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

View File

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

View File

@@ -1,8 +0,0 @@
namespace RobotApp.Services.State;
public abstract class RobotState<T>(T type) where T : Enum
{
public readonly T Type = type;
public virtual void Enter() { }
public virtual void Exit() { }
}

View File

@@ -19,7 +19,6 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
Watch.Stop();
if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
{
if(Watch.ElapsedMilliseconds > Interval) Logger?.Warning($"WatchTimer Warning: Elapsed time {Watch.ElapsedMilliseconds}ms exceeds interval {Interval}ms.");
Timer?.Change(Interval, Timeout.Infinite);
}
else