Compare commits
1 Commits
d6fe1d9d52
...
dangnv-lap
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
4aed0da992 |
5
RobotApp.Client/Components/Mapping/MapView.razor
Normal file
5
RobotApp.Client/Components/Mapping/MapView.razor
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
<h3>MapView</h3>
|
||||||
|
|
||||||
|
@code {
|
||||||
|
|
||||||
|
}
|
||||||
@@ -36,6 +36,12 @@
|
|||||||
</NavLink>
|
</NavLink>
|
||||||
</div>
|
</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>
|
<AuthorizeView>
|
||||||
<Authorized>
|
<Authorized>
|
||||||
<div class="nav-item px-3">
|
<div class="nav-item px-3">
|
||||||
|
|||||||
181
RobotApp.Client/Pages/MapsManager.razor
Normal file
181
RobotApp.Client/Pages/MapsManager.razor
Normal 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -11,6 +11,11 @@
|
|||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly" Version="9.0.1" />
|
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly" Version="9.0.1" />
|
||||||
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Authentication" 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>
|
</ItemGroup>
|
||||||
|
|
||||||
</Project>
|
</Project>
|
||||||
|
|||||||
@@ -8,3 +8,4 @@
|
|||||||
@using Microsoft.AspNetCore.Components.Web.Virtualization
|
@using Microsoft.AspNetCore.Components.Web.Virtualization
|
||||||
@using Microsoft.JSInterop
|
@using Microsoft.JSInterop
|
||||||
@using RobotApp.Client
|
@using RobotApp.Client
|
||||||
|
@using Microsoft.AspNetCore.Authorization
|
||||||
13
RobotApp.Common.Shares/Dtos/MapDto.cs
Normal file
13
RobotApp.Common.Shares/Dtos/MapDto.cs
Normal 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; }
|
||||||
|
}
|
||||||
@@ -1,7 +0,0 @@
|
|||||||
namespace RobotApp.Common.Shares.Enums;
|
|
||||||
|
|
||||||
public enum RobotDirection
|
|
||||||
{
|
|
||||||
FORWARD,
|
|
||||||
BACKWARD,
|
|
||||||
}
|
|
||||||
@@ -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,
|
|
||||||
}
|
|
||||||
@@ -1,8 +0,0 @@
|
|||||||
namespace RobotApp.Common.Shares.Enums;
|
|
||||||
|
|
||||||
public enum TrajectoryDegree
|
|
||||||
{
|
|
||||||
One,
|
|
||||||
Two,
|
|
||||||
Three,
|
|
||||||
}
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -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; }
|
|
||||||
}
|
|
||||||
@@ -2,6 +2,8 @@
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public enum ActionStatus
|
public enum ActionStatus
|
||||||
{
|
{
|
||||||
WAITING,
|
WAITING,
|
||||||
@@ -13,11 +15,11 @@ public enum ActionStatus
|
|||||||
}
|
}
|
||||||
public class ActionState
|
public class ActionState
|
||||||
{
|
{
|
||||||
public string ActionType { get; set; } = string.Empty;
|
public string ActionType { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string ActionId { get; set; } = string.Empty;
|
public string ActionId { get; set; }
|
||||||
public string ActionDescription { get; set; } = string.Empty;
|
public string ActionDescription { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string ActionStatus { get; set; } = string.Empty;
|
public string ActionStatus { get; set; }
|
||||||
public string ResultDescription { get; set; } = string.Empty;
|
public string ResultDescription { get; set; }
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -3,15 +3,17 @@ using System.ComponentModel.DataAnnotations;
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public class EdgeState
|
public class EdgeState
|
||||||
{
|
{
|
||||||
|
|
||||||
[Required]
|
[Required]
|
||||||
public string EdgeId { get; set; } = string.Empty;
|
public string EdgeId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public int SequenceId { get; set; }
|
public int SequenceId { get; set; }
|
||||||
public string EdgeDescription { get; set; } = string.Empty;
|
public string EdgeDescription { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public bool Released { get; set; }
|
public bool Released { get; set; }
|
||||||
public Trajectory Trajectory { get; set; } = new();
|
public Trajectory Trajectory { get; set; }
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -2,33 +2,28 @@
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public enum ErrorLevel
|
public enum ErrorLevel
|
||||||
{
|
{
|
||||||
NONE,
|
NONE,
|
||||||
WARNING,
|
WARNING,
|
||||||
FATAL
|
FATAL
|
||||||
}
|
}
|
||||||
|
|
||||||
public class ErrorReferences
|
public class ErrorReferences
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public string ReferenceKey { get; set; } = string.Empty;
|
public string ReferenceKey { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string ReferenceValue { get; set; } = string.Empty;
|
public string ReferenceValue { get; set; }
|
||||||
}
|
}
|
||||||
|
|
||||||
public enum ErrorType
|
|
||||||
{
|
|
||||||
INITIALIZE_ORDER,
|
|
||||||
}
|
|
||||||
|
|
||||||
public class Error
|
public class Error
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public string ErrorType { get; set; } = string.Empty;
|
public string ErrorType { get; set; }
|
||||||
public ErrorReferences[] ErrorReferences { get; set; } = [];
|
public ErrorReferences[] ErrorReferences { get; set; }
|
||||||
public string ErrorDescription { get; set; } = string.Empty;
|
public string ErrorDescription { get; set; }
|
||||||
public string ErrorHint { get; set; } = string.Empty;
|
public string ErrorHint { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string ErrorLevel { get; set; } = string.Empty;
|
public string ErrorLevel { get; set; }
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -2,6 +2,7 @@
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public enum InfoLevel
|
public enum InfoLevel
|
||||||
{
|
{
|
||||||
@@ -11,16 +12,16 @@ public enum InfoLevel
|
|||||||
public class InfomationReferences
|
public class InfomationReferences
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public string ReferenceKey { get; set; } = string.Empty;
|
public string ReferenceKey { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string ReferenceValue { get; set; } = string.Empty;
|
public string ReferenceValue { get; set; }
|
||||||
}
|
}
|
||||||
public class Information
|
public class Information
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public string InfoType { get; set; } = string.Empty;
|
public string InfoType { get; set; }
|
||||||
public InfomationReferences[] InfoReferences { get; set; } = [];
|
public InfomationReferences[] InfoReferences { get; set; }
|
||||||
public string InfoDescription { get; set; } = string.Empty;
|
public string InfoDescription { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string InfoLevel { get; set; } = string.Empty;
|
public string InfoLevel { get; set; }
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -2,13 +2,15 @@
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public class Load
|
public class Load
|
||||||
{
|
{
|
||||||
public string LoadId { get; set; } = string.Empty;
|
public string LoadId { get; set; }
|
||||||
public string LoadType { get; set; } = string.Empty;
|
public string LoadType { get; set; }
|
||||||
public string LoadPosition { get; set; } = string.Empty;
|
public string LoadPosition { get; set; }
|
||||||
public BoundingBoxReference BoundingBoxReference { get; set; } = new();
|
public BoundingBoxReference BoundingBoxReference { get; set; }
|
||||||
public LoadDimensions LoadDimensions { get; set; } = new();
|
public LoadDimensions LoadDimensions { get; set; }
|
||||||
public double Weight { get; set; }
|
public double Weight { get; set; }
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,18 +1,20 @@
|
|||||||
using System.ComponentModel.DataAnnotations;
|
using RobotApp.VDA5050.Order;
|
||||||
|
using System.ComponentModel.DataAnnotations;
|
||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public class NodeState
|
public class NodeState
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public string NodeId { get; set; } = string.Empty;
|
public string NodeId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public int SequenceId { get; set; }
|
public int SequenceId { get; set; }
|
||||||
public string NodeDescription { get; set; } = string.Empty;
|
public string NodeDescription { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public bool Released { get; set; }
|
public bool Released { get; set; }
|
||||||
public NodePosition NodePosition { get; set; } = new();
|
public NodePosition NodePosition { get; set; }
|
||||||
}
|
}
|
||||||
|
|
||||||
public class NodePosition
|
public class NodePosition
|
||||||
@@ -23,6 +25,6 @@ public class NodePosition
|
|||||||
public double Y { get; set; }
|
public double Y { get; set; }
|
||||||
public double Theta { get; set; }
|
public double Theta { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string MapId { get; set; } = string.Empty;
|
public string MapId { get; set; } = "";
|
||||||
|
|
||||||
}
|
}
|
||||||
@@ -3,6 +3,8 @@ using System.ComponentModel.DataAnnotations;
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.State;
|
namespace RobotApp.VDA5050.State;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public enum OperatingMode
|
public enum OperatingMode
|
||||||
{
|
{
|
||||||
AUTOMATIC,
|
AUTOMATIC,
|
||||||
@@ -15,21 +17,22 @@ public class StateMsg
|
|||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public uint HeaderId { get; set; }
|
public uint HeaderId { get; set; }
|
||||||
public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
|
|
||||||
[Required]
|
[Required]
|
||||||
public string Version { get; set; } = "1.0.0";
|
public string Timestamp { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string Manufacturer { get; set; } = "PhenikaaX";
|
public string Version { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string SerialNumber { get; set; } = string.Empty;
|
public string Manufacturer { get; set; }
|
||||||
|
[Required]
|
||||||
|
public string SerialNumber { get; set; }
|
||||||
public Map[] Maps { get; set; } = [];
|
public Map[] Maps { get; set; } = [];
|
||||||
[Required]
|
[Required]
|
||||||
public string OrderId { get; set; } = string.Empty;
|
public string OrderId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public int OrderUpdateId { get; set; }
|
public int OrderUpdateId { get; set; }
|
||||||
public string ZoneSetId { get; set; } = string.Empty;
|
public string ZoneSetId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string LastNodeId { get; set; } = string.Empty;
|
public string LastNodeId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public int LastNodeSequenceId { get; set; }
|
public int LastNodeSequenceId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
@@ -38,7 +41,7 @@ public class StateMsg
|
|||||||
public bool NewBaseRequest { get; set; }
|
public bool NewBaseRequest { get; set; }
|
||||||
public double DistanceSinceLastNode { get; set; }
|
public double DistanceSinceLastNode { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string OperatingMode { get; set; } = string.Empty;
|
public string OperatingMode { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public NodeState[] NodeStates { get; set; } = [];
|
public NodeState[] NodeStates { get; set; } = [];
|
||||||
[Required]
|
[Required]
|
||||||
|
|||||||
@@ -2,6 +2,8 @@
|
|||||||
|
|
||||||
namespace RobotApp.VDA5050.Visualization;
|
namespace RobotApp.VDA5050.Visualization;
|
||||||
|
|
||||||
|
#nullable disable
|
||||||
|
|
||||||
public class AgvPosition
|
public class AgvPosition
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
@@ -9,7 +11,7 @@ public class AgvPosition
|
|||||||
[Required]
|
[Required]
|
||||||
public double Y { get; set; }
|
public double Y { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public string MapId { get; set; } = string.Empty;
|
public string MapId { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
public double Theta { get; set; }
|
public double Theta { get; set; }
|
||||||
[Required]
|
[Required]
|
||||||
|
|||||||
@@ -1,16 +1,16 @@
|
|||||||
using System.ComponentModel.DataAnnotations;
|
namespace RobotApp.VDA5050.Visualization;
|
||||||
|
|
||||||
namespace RobotApp.VDA5050.Visualization;
|
#nullable disable
|
||||||
|
|
||||||
public class VisualizationMsg
|
public class VisualizationMsg
|
||||||
{
|
{
|
||||||
public uint HeaderId { get; set; }
|
public uint HeaderId { get; set; }
|
||||||
public string Timestamp { get; set; } = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ");
|
public string Timestamp { get; set; }
|
||||||
public string Version { get; set; } = "1.0.0";
|
public string Version { get; set; }
|
||||||
public string Manufacturer { get; set; } = "PhenikaaX";
|
public string Manufacturer { get; set; }
|
||||||
public string SerialNumber { get; set; } = string.Empty;
|
public string SerialNumber { get; set; }
|
||||||
public string MapId { get; set; } = string.Empty;
|
public string MapId { get; set; }
|
||||||
public string MapDescription { get; set; } = string.Empty;
|
public string MapDescription { get; set; }
|
||||||
public AgvPosition AgvPosition { get; set; } = new();
|
public AgvPosition AgvPosition { get; set; } = new();
|
||||||
public Velocity Velocity { get; set; } = new();
|
public Velocity Velocity { get; set; } = new();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,7 +1,7 @@
|
|||||||
|
|
||||||
Microsoft Visual Studio Solution File, Format Version 12.00
|
Microsoft Visual Studio Solution File, Format Version 12.00
|
||||||
# Visual Studio Version 17
|
# Visual Studio Version 18
|
||||||
VisualStudioVersion = 17.12.35707.178
|
VisualStudioVersion = 18.0.11018.127 d18.0
|
||||||
MinimumVisualStudioVersion = 10.0.40219.1
|
MinimumVisualStudioVersion = 10.0.40219.1
|
||||||
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp", "RobotApp\RobotApp.csproj", "{BF0BB137-2EF9-4E1B-944E-9BF41C5284F7}"
|
Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "RobotApp", "RobotApp\RobotApp.csproj", "{BF0BB137-2EF9-4E1B-944E-9BF41C5284F7}"
|
||||||
EndProject
|
EndProject
|
||||||
@@ -37,4 +37,7 @@ Global
|
|||||||
GlobalSection(SolutionProperties) = preSolution
|
GlobalSection(SolutionProperties) = preSolution
|
||||||
HideSolutionNode = FALSE
|
HideSolutionNode = FALSE
|
||||||
EndGlobalSection
|
EndGlobalSection
|
||||||
|
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||||
|
SolutionGuid = {CCB0B2E5-3C19-4B2E-B229-08A74F6EF27D}
|
||||||
|
EndGlobalSection
|
||||||
EndGlobal
|
EndGlobal
|
||||||
|
|||||||
@@ -1,5 +0,0 @@
|
|||||||
namespace RobotApp.Interfaces;
|
|
||||||
|
|
||||||
public interface IBattery
|
|
||||||
{
|
|
||||||
}
|
|
||||||
@@ -1,9 +0,0 @@
|
|||||||
using RobotApp.VDA5050.State;
|
|
||||||
|
|
||||||
namespace RobotApp.Interfaces;
|
|
||||||
|
|
||||||
public interface IError
|
|
||||||
{
|
|
||||||
bool HasFatalError { get; }
|
|
||||||
void AddError(Error error, TimeSpan? clearAfter = null);
|
|
||||||
}
|
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
namespace RobotApp.Interfaces;
|
|
||||||
|
|
||||||
public interface IInfomation
|
|
||||||
{
|
|
||||||
}
|
|
||||||
@@ -1,14 +1,5 @@
|
|||||||
using RobotApp.VDA5050.State;
|
namespace RobotApp.Interfaces;
|
||||||
using Action = RobotApp.VDA5050.InstantAction.Action;
|
|
||||||
|
|
||||||
namespace RobotApp.Interfaces;
|
|
||||||
|
|
||||||
public interface IInstanceActions
|
public interface IInstanceActions
|
||||||
{
|
{
|
||||||
ActionState[] ActionStates { get; }
|
|
||||||
bool HasActionRunning { get; }
|
|
||||||
bool AddOrderActions(Action[] actions);
|
|
||||||
bool StartAction(string actionId);
|
|
||||||
bool AddInstanceAction(Action action);
|
|
||||||
bool StopAction();
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -4,9 +4,6 @@ namespace RobotApp.Interfaces;
|
|||||||
|
|
||||||
public enum NavigationState
|
public enum NavigationState
|
||||||
{
|
{
|
||||||
None,
|
|
||||||
Initializing,
|
|
||||||
Initialized,
|
|
||||||
Idle,
|
Idle,
|
||||||
Moving,
|
Moving,
|
||||||
Rotating,
|
Rotating,
|
||||||
@@ -16,7 +13,7 @@ public enum NavigationState
|
|||||||
|
|
||||||
public enum NavigationProccess
|
public enum NavigationProccess
|
||||||
{
|
{
|
||||||
None,
|
Started,
|
||||||
InProgress,
|
InProgress,
|
||||||
Completed,
|
Completed,
|
||||||
Failed,
|
Failed,
|
||||||
@@ -25,13 +22,12 @@ public enum NavigationProccess
|
|||||||
|
|
||||||
public interface INavigation
|
public interface INavigation
|
||||||
{
|
{
|
||||||
bool Driving { get; }
|
|
||||||
NavigationState State { get; }
|
NavigationState State { get; }
|
||||||
void Move(Node[] nodes, Edge[] edges);
|
NavigationProccess Proccess { get; }
|
||||||
void MoveStraight(double x, double y);
|
bool Move(Node nodes, Edge edges);
|
||||||
void Rotate(double angle);
|
bool MoveStraight(double x, double y);
|
||||||
void Paused();
|
bool Rotate(double angle);
|
||||||
void Resume();
|
bool Paused();
|
||||||
void UpdateOrder(int lastBaseSequence);
|
bool Resume();
|
||||||
void CancelMovement();
|
bool CancelMovement();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,19 +1,15 @@
|
|||||||
using RobotApp.VDA5050.Order;
|
using RobotApp.VDA5050.State;
|
||||||
using RobotApp.VDA5050.State;
|
|
||||||
|
|
||||||
namespace RobotApp.Interfaces;
|
namespace RobotApp.Interfaces;
|
||||||
|
|
||||||
public interface IOrder
|
public interface IOrder
|
||||||
{
|
{
|
||||||
string OrderId { get; }
|
string OrderId { get; }
|
||||||
int OrderUpdateId { get; }
|
VDA5050.InstantAction.Action[] Actions { get; }
|
||||||
string LastNodeId { get; }
|
|
||||||
int LastNodeSequenceId { get; }
|
|
||||||
|
|
||||||
NodeState[] NodeStates { get; }
|
NodeState[] NodeStates { get; }
|
||||||
EdgeState[] EdgeStates { get; }
|
EdgeState[] EdgeStates { get; }
|
||||||
|
|
||||||
void StartOrder(string orderId, Node[] nodes, Edge[] edges);
|
bool StartOrder();
|
||||||
void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
|
bool UpdateOrder();
|
||||||
void StopOrder();
|
bool StopOrder();
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,14 +0,0 @@
|
|||||||
namespace RobotApp.Interfaces;
|
|
||||||
|
|
||||||
public enum OperatingMode
|
|
||||||
{
|
|
||||||
AUTOMATIC,
|
|
||||||
MANUAL,
|
|
||||||
SEMIAUTOMATIC,
|
|
||||||
TEACHIN,
|
|
||||||
SERVICE,
|
|
||||||
}
|
|
||||||
public interface IPeripheral
|
|
||||||
{
|
|
||||||
OperatingMode OperatingMode { get; }
|
|
||||||
}
|
|
||||||
@@ -1,6 +0,0 @@
|
|||||||
namespace RobotApp.Interfaces
|
|
||||||
{
|
|
||||||
public class IRFHandler
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
namespace RobotApp.Interfaces;
|
|
||||||
|
|
||||||
public interface ISafety
|
|
||||||
{
|
|
||||||
}
|
|
||||||
@@ -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)
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@@ -1,8 +0,0 @@
|
|||||||
namespace RobotApp.Modbus;
|
|
||||||
|
|
||||||
|
|
||||||
public enum RegisterOrder
|
|
||||||
{
|
|
||||||
LowHigh = 0,
|
|
||||||
HighLow = 1
|
|
||||||
}
|
|
||||||
@@ -24,7 +24,6 @@
|
|||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<Folder Include="Services\Robot\Simulation\" />
|
|
||||||
<Folder Include="wwwroot\lib\" />
|
<Folder Include="wwwroot\lib\" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
|||||||
@@ -1,5 +0,0 @@
|
|||||||
namespace RobotApp.Services.Exceptions;
|
|
||||||
|
|
||||||
public class ActionException : OrderException
|
|
||||||
{
|
|
||||||
}
|
|
||||||
@@ -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; }
|
|
||||||
}
|
|
||||||
@@ -27,6 +27,7 @@ public class MQTTClient : IAsyncDisposable
|
|||||||
Logger = logger;
|
Logger = logger;
|
||||||
|
|
||||||
MqttClientFactory = new MqttClientFactory();
|
MqttClientFactory = new MqttClientFactory();
|
||||||
|
MqttClient = MqttClientFactory.CreateMqttClient();
|
||||||
MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
|
MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
|
||||||
.WithTcpServer(setting.HostServer, setting.Port)
|
.WithTcpServer(setting.HostServer, setting.Port)
|
||||||
.WithCredentials(setting.UserName, setting.Password)
|
.WithCredentials(setting.UserName, setting.Password)
|
||||||
@@ -37,11 +38,6 @@ public class MQTTClient : IAsyncDisposable
|
|||||||
.WithTopicFilter(f => f.WithTopic(VDA5050Topic.ORDER.ToTopicString()))
|
.WithTopicFilter(f => f.WithTopic(VDA5050Topic.ORDER.ToTopicString()))
|
||||||
.WithTopicFilter(f => f.WithTopic(VDA5050Topic.INSTANTACTIONS.ToTopicString()))
|
.WithTopicFilter(f => f.WithTopic(VDA5050Topic.INSTANTACTIONS.ToTopicString()))
|
||||||
.Build();
|
.Build();
|
||||||
}
|
|
||||||
|
|
||||||
public async Task ConnectAsync(CancellationToken cancellationToken = default)
|
|
||||||
{
|
|
||||||
MqttClient = MqttClientFactory.CreateMqttClient();
|
|
||||||
MqttClient.DisconnectedAsync += async delegate (MqttClientDisconnectedEventArgs args)
|
MqttClient.DisconnectedAsync += async delegate (MqttClientDisconnectedEventArgs args)
|
||||||
{
|
{
|
||||||
if (args.ClientWasConnected && !IsReconnecing)
|
if (args.ClientWasConnected && !IsReconnecing)
|
||||||
@@ -56,10 +52,15 @@ public class MQTTClient : IAsyncDisposable
|
|||||||
IsReconnecing = false;
|
IsReconnecing = false;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public async Task ConnectAsync(CancellationToken cancellationToken = default)
|
||||||
|
{
|
||||||
while (!cancellationToken.IsCancellationRequested)
|
while (!cancellationToken.IsCancellationRequested)
|
||||||
{
|
{
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
|
MqttClient ??= MqttClientFactory.CreateMqttClient();
|
||||||
var connection = await MqttClient.ConnectAsync(MqttClientOptions, cancellationToken);
|
var connection = await MqttClient.ConnectAsync(MqttClientOptions, cancellationToken);
|
||||||
if (connection.ResultCode != MqttClientConnectResultCode.Success || !MqttClient.IsConnected)
|
if (connection.ResultCode != MqttClientConnectResultCode.Success || !MqttClient.IsConnected)
|
||||||
Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");
|
Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");
|
||||||
|
|||||||
@@ -1,4 +1,4 @@
|
|||||||
namespace RobotApp.Services.Robot.Navigation.Algorithm;
|
namespace RobotApp.Services.Navigation.Algorithm;
|
||||||
|
|
||||||
public class FuzzyLogic
|
public class FuzzyLogic
|
||||||
{
|
{
|
||||||
@@ -1,4 +1,4 @@
|
|||||||
namespace RobotApp.Services.Robot.Navigation.Algorithm;
|
namespace RobotApp.Services.Navigation.Algorithm;
|
||||||
|
|
||||||
public class PID
|
public class PID
|
||||||
{
|
{
|
||||||
5
RobotApp/Services/Navigation/NavigationController.cs
Normal file
5
RobotApp/Services/Navigation/NavigationController.cs
Normal file
@@ -0,0 +1,5 @@
|
|||||||
|
namespace RobotApp.Services.Navigation;
|
||||||
|
|
||||||
|
public class NavigationController
|
||||||
|
{
|
||||||
|
}
|
||||||
@@ -1,4 +1,4 @@
|
|||||||
namespace RobotApp.Services.Robot.Navigation
|
namespace RobotApp.Services.Navigation
|
||||||
{
|
{
|
||||||
public class NavigationManager
|
public class NavigationManager
|
||||||
{
|
{
|
||||||
@@ -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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -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;
|
|
||||||
}
|
|
||||||
@@ -1,31 +1,6 @@
|
|||||||
using RobotApp.Interfaces;
|
namespace RobotApp.Services.Robot
|
||||||
using RobotApp.VDA5050.State;
|
|
||||||
|
|
||||||
namespace RobotApp.Services.Robot;
|
|
||||||
|
|
||||||
public class RobotAction : IInstanceActions
|
|
||||||
{
|
{
|
||||||
public ActionState[] ActionStates { get; private set; } = [];
|
public class RobotAction
|
||||||
|
|
||||||
public bool HasActionRunning => throw new NotImplementedException();
|
|
||||||
|
|
||||||
public bool AddInstanceAction(VDA5050.InstantAction.Action action)
|
|
||||||
{
|
{
|
||||||
throw new NotImplementedException();
|
|
||||||
}
|
|
||||||
|
|
||||||
public bool AddOrderActions(VDA5050.InstantAction.Action[] actions)
|
|
||||||
{
|
|
||||||
throw new NotImplementedException();
|
|
||||||
}
|
|
||||||
|
|
||||||
public bool StartAction(string actionId)
|
|
||||||
{
|
|
||||||
throw new NotImplementedException();
|
|
||||||
}
|
|
||||||
|
|
||||||
public bool StopAction()
|
|
||||||
{
|
|
||||||
throw new NotImplementedException();
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,6 +0,0 @@
|
|||||||
namespace RobotApp.Services.Robot
|
|
||||||
{
|
|
||||||
public class RobotBattery
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,96 +1,6 @@
|
|||||||
using RobotApp.Interfaces;
|
namespace RobotApp.Services.Robot
|
||||||
using RobotApp.Services.Exceptions;
|
|
||||||
using RobotApp.VDA5050.Order;
|
|
||||||
using RobotApp.VDA5050.State;
|
|
||||||
|
|
||||||
namespace RobotApp.Services.Robot;
|
|
||||||
|
|
||||||
public class RobotController(IOrder OrderManager,
|
|
||||||
INavigation NavigationManager,
|
|
||||||
IInstanceActions ActionManager,
|
|
||||||
IBattery BatteryManager,
|
|
||||||
ILocalization Localization,
|
|
||||||
IPeripheral PeripheralManager,
|
|
||||||
ISafety SafetyManager,
|
|
||||||
IError ErrorManager,
|
|
||||||
IInfomation InfomationManager,
|
|
||||||
Logger<RobotController> Logger,
|
|
||||||
RobotConnection ConnectionManager) : BackgroundService
|
|
||||||
{
|
{
|
||||||
private readonly Mutex NewOrderMutex = new();
|
public class RobotController
|
||||||
private readonly Mutex NewInstanceMutex = new();
|
|
||||||
|
|
||||||
private WatchTimer<RobotController>? UpdateStateTimer;
|
|
||||||
private const int UpdateStateInterval = 1000;
|
|
||||||
|
|
||||||
protected override Task ExecuteAsync(CancellationToken stoppingToken)
|
|
||||||
{
|
{
|
||||||
UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
|
|
||||||
UpdateStateTimer.Start();
|
|
||||||
return Task.CompletedTask;
|
|
||||||
}
|
|
||||||
|
|
||||||
private void UpdateStateHandler()
|
|
||||||
{
|
|
||||||
// xử lý cập nhật trạng thái robot và gửi thông tin qua kết nối
|
|
||||||
}
|
|
||||||
|
|
||||||
public void NewOrderUpdated(OrderMsg order)
|
|
||||||
{
|
|
||||||
if (NewOrderMutex.WaitOne(2000))
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
if (!string.IsNullOrEmpty(OrderManager.OrderId))
|
|
||||||
{
|
|
||||||
if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderManager.OrderId}, OrderId mới: {order.OrderId}"));
|
|
||||||
OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges);
|
|
||||||
}
|
|
||||||
else if (PeripheralManager.OperatingMode != Interfaces.OperatingMode.AUTOMATIC) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1006", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi chế độ vận hành không phải là TỰ ĐỘNG. Chế độ hiện tại: {PeripheralManager.OperatingMode}"));
|
|
||||||
else if(ActionManager.HasActionRunning) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1007", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có action đang thực hiện. Vui lòng chờ hoàn thành các action hiện tại."));
|
|
||||||
else if(ErrorManager.HasFatalError) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1008", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi có lỗi nghiêm trọng. Vui lòng kiểm tra và xử lý lỗi."));
|
|
||||||
else if(NavigationManager.Driving) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1009", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi robot đang di chuyển. Vui lòng dừng robot."));
|
|
||||||
else OrderManager.StartOrder(order.OrderId, order.Nodes, order.Edges);
|
|
||||||
}
|
|
||||||
catch (OrderException orEx)
|
|
||||||
{
|
|
||||||
if (orEx.Error is not null)
|
|
||||||
{
|
|
||||||
ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
|
|
||||||
Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
catch (Exception ex)
|
|
||||||
{
|
|
||||||
Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
|
|
||||||
}
|
|
||||||
finally
|
|
||||||
{
|
|
||||||
NewOrderMutex.ReleaseMutex();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
public void NewInstanceActionUpdated()
|
|
||||||
{
|
|
||||||
if (NewInstanceMutex.WaitOne(2000))
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
catch (ActionException acEx)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
catch (Exception ex)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
finally
|
|
||||||
{
|
|
||||||
NewInstanceMutex.ReleaseMutex();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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 = []
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -29,6 +29,6 @@ public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService
|
|||||||
await Task.Delay(1000, stoppingToken);
|
await Task.Delay(1000, stoppingToken);
|
||||||
if (RobotConnection.IsConnected) break;
|
if (RobotConnection.IsConnected) break;
|
||||||
}
|
}
|
||||||
await PubFactsheet();
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,6 +0,0 @@
|
|||||||
namespace RobotApp.Services.Robot
|
|
||||||
{
|
|
||||||
public class RobotInfomations
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,6 +0,0 @@
|
|||||||
namespace RobotApp.Services.Robot
|
|
||||||
{
|
|
||||||
public class RobotLoads
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,182 +1,30 @@
|
|||||||
using RobotApp.Interfaces;
|
using RobotApp.Interfaces;
|
||||||
using RobotApp.Services.Exceptions;
|
|
||||||
using RobotApp.VDA5050.Order;
|
|
||||||
using RobotApp.VDA5050.State;
|
using RobotApp.VDA5050.State;
|
||||||
using Action = RobotApp.VDA5050.InstantAction.Action;
|
|
||||||
|
|
||||||
namespace RobotApp.Services.Robot;
|
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 string OrderId => throw new NotImplementedException();
|
||||||
public int OrderUpdateId { get; private set; }
|
|
||||||
public NodeState[] NodeStates { get; private set; } = [];
|
|
||||||
public EdgeState[] EdgeStates { get; private set; } = [];
|
|
||||||
public string LastNodeId { get; private set; } = string.Empty;
|
|
||||||
public int LastNodeSequenceId { get; private set; }
|
|
||||||
|
|
||||||
private readonly Dictionary<string, Action[]> OrderActions = [];
|
public VDA5050.InstantAction.Action[] Actions => throw new NotImplementedException();
|
||||||
private readonly Mutex OrderMutex = new();
|
|
||||||
|
|
||||||
protected const int CycleHandlerMilliseconds = 100;
|
public NodeState[] NodeStates => throw new NotImplementedException();
|
||||||
private WatchTimer<RobotOrderController>? OrderTimer;
|
|
||||||
|
|
||||||
private int BaseSequenceId = 0;
|
public EdgeState[] EdgeStates => throw new NotImplementedException();
|
||||||
public void StartOrder(string orderId, Node[] nodes, Edge[] edges)
|
|
||||||
{
|
|
||||||
if (OrderMutex.WaitOne(2000))
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1001", ErrorLevel.WARNING, $"Có order đang được thực hiện. OrderId: {OrderId}, OrderId mới: {orderId}"));
|
|
||||||
if (nodes.Length < 2) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1002", ErrorLevel.WARNING, $"Order Nodes không hợp lệ. Kích thước: {nodes.Length}"));
|
|
||||||
if (edges.Length < 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1003", ErrorLevel.WARNING, $"Order Edges không hợp lệ. Kích thước: {edges.Length}"));
|
|
||||||
if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1004", ErrorLevel.WARNING, $"Order không hợp lệ do kích thước giữa Nodes và Edges không phù hợp. Kích thước Edges: {edges.Length}, kích thước nodes: {nodes.Length}"));
|
|
||||||
if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1012", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi hệ thống điều hướng không ở trạng thái sẵn sàng. Trạng thái hiện tại: {NavigationManager.State}"));
|
|
||||||
|
|
||||||
for (int i = 0; i < nodes.Length; i++)
|
public bool StartOrder()
|
||||||
{
|
{
|
||||||
if (nodes[i].Actions is not null && nodes[i].Actions.Length > 0)
|
throw new NotImplementedException();
|
||||||
{
|
|
||||||
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
|
public bool UpdateOrder()
|
||||||
{
|
{
|
||||||
NodeId = n.NodeId,
|
throw new NotImplementedException();
|
||||||
Released = n.Released,
|
|
||||||
SequenceId = n.SequenceId,
|
|
||||||
NodeDescription = n.NodeDescription,
|
|
||||||
NodePosition = new()
|
|
||||||
{
|
|
||||||
X = n.NodePosition.X,
|
|
||||||
Y = n.NodePosition.Y,
|
|
||||||
Theta = n.NodePosition.Theta,
|
|
||||||
MapId = n.NodePosition.MapId
|
|
||||||
}
|
|
||||||
}).ToArray();
|
|
||||||
EdgeStates = edges.Select(e => new EdgeState
|
|
||||||
{
|
|
||||||
EdgeId = e.EdgeId,
|
|
||||||
Released = e.Released,
|
|
||||||
EdgeDescription = e.EdgeDescription,
|
|
||||||
SequenceId = e.SequenceId,
|
|
||||||
Trajectory = e.Trajectory
|
|
||||||
}).ToArray();
|
|
||||||
|
|
||||||
OrderId = orderId;
|
|
||||||
ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
|
|
||||||
NavigationManager.Move(nodes, edges);
|
|
||||||
HandleNavigationStart();
|
|
||||||
}
|
|
||||||
catch (OrderException orEx)
|
|
||||||
{
|
|
||||||
if (orEx.Error is not null)
|
|
||||||
{
|
|
||||||
ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
|
|
||||||
Logger.Warning($"Lỗi khi khởi tạo Order: {orEx.Error.ErrorDescription}");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
catch (Exception ex)
|
|
||||||
{
|
|
||||||
Logger.Warning($"Lỗi khi khởi tạo Order: {ex.Message}");
|
|
||||||
}
|
|
||||||
finally
|
|
||||||
{
|
|
||||||
OrderMutex.ReleaseMutex();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Logger.Warning($"Lỗi khi khởi tạo Order: có order đang được khởi tạo chưa xong");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public void UpdateOrder(int orderUpdateId,Node[] nodes, Edge[] edges)
|
public bool StopOrder()
|
||||||
{
|
{
|
||||||
if (OrderMutex.WaitOne(2000))
|
throw new NotImplementedException();
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.CreateError(ErrorType.INITIALIZE_ORDER, "1005", ErrorLevel.WARNING, $"Không có order đang được thực hiện."));
|
|
||||||
if (orderUpdateId > OrderUpdateId)
|
|
||||||
{
|
|
||||||
OrderUpdateId = orderUpdateId;
|
|
||||||
// Check Order Update hợp lệ
|
|
||||||
// Check Order update Hoziron hay không
|
|
||||||
}
|
|
||||||
}
|
|
||||||
catch (OrderException orEx)
|
|
||||||
{
|
|
||||||
if (orEx.Error is not null)
|
|
||||||
{
|
|
||||||
ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
|
|
||||||
Logger.Warning($"Lỗi khi cập nhật Order: {orEx.Error.ErrorDescription}");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
catch (Exception ex)
|
|
||||||
{
|
|
||||||
Logger.Warning($"Lỗi khi cập nhật Order: {ex.Message}");
|
|
||||||
}
|
|
||||||
finally
|
|
||||||
{
|
|
||||||
OrderMutex.ReleaseMutex();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Logger.Warning($"Lỗi khi cập nhật Order: có order đang được cập nhật chưa xong");
|
|
||||||
}
|
|
||||||
|
|
||||||
public void StopOrder()
|
|
||||||
{
|
|
||||||
HandleNavigationStop();
|
|
||||||
NavigationManager.CancelMovement();
|
|
||||||
|
|
||||||
OrderId = string.Empty;
|
|
||||||
OrderActions.Clear();
|
|
||||||
OrderUpdateId = 0;
|
|
||||||
BaseSequenceId = 0;
|
|
||||||
LastNodeSequenceId = 0;
|
|
||||||
NodeStates = [];
|
|
||||||
EdgeStates = [];
|
|
||||||
}
|
|
||||||
|
|
||||||
private void HandleNavigationStart()
|
|
||||||
{
|
|
||||||
OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
|
|
||||||
OrderTimer.Start();
|
|
||||||
}
|
|
||||||
|
|
||||||
private void HandleNavigationStop()
|
|
||||||
{
|
|
||||||
OrderTimer?.Dispose();
|
|
||||||
}
|
|
||||||
|
|
||||||
private void OrderHandler()
|
|
||||||
{
|
|
||||||
if(!string.IsNullOrEmpty(OrderId) && BaseSequenceId > 0)
|
|
||||||
{
|
|
||||||
if(NavigationManager.State == NavigationState.Initializing)
|
|
||||||
{
|
|
||||||
// khởi tạo Order
|
|
||||||
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
// xử lí khi có Order
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -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];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,6 +0,0 @@
|
|||||||
namespace RobotApp.Services.Robot
|
|
||||||
{
|
|
||||||
public class RobotSafety
|
|
||||||
{
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
namespace RobotApp.Services.Robot;
|
|
||||||
|
|
||||||
public class RobotStates
|
|
||||||
{
|
|
||||||
}
|
|
||||||
@@ -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() { }
|
|
||||||
}
|
|
||||||
@@ -19,7 +19,6 @@ public class WatchTimer<T>(int Interval, Action Callback, Logger<T>? Logger) : I
|
|||||||
Watch.Stop();
|
Watch.Stop();
|
||||||
if (Watch.ElapsedMilliseconds >= Interval || Interval - Watch.ElapsedMilliseconds <= 50)
|
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);
|
Timer?.Change(Interval, Timeout.Infinite);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
|||||||
Reference in New Issue
Block a user