DangNV/RobotApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Đăng Nguyễn 2025-10-22 11:16:19 +07:00
parent 9ac5270885
commit ab5d3e1a1a
40 changed files with 1769 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
RobotApp.VDA5050/Factsheet/AgvActions.cs
View File

@ -9,7 +9,8 @@ public enum ActionScopes
NODE,
EDGE,
}
public class AgvActions
public class AgvAction
{
[Required]
public string ActionType { get; set; } = string.Empty;

2
RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs
View File

@ -8,5 +8,5 @@ public class ProtocolFeatures
[Required]
public OptionalParameters[] OptionalParameters { get; set; } = [];
[Required]
public AgvActions[] AgvActions { get; set; } = [];
public AgvAction[] AgvActions { get; set; } = [];
}

42
RobotApp.VDA5050/Type/ActionType.cs
View File

@ -2,16 +2,32 @@
public enum ActionType
{
START_PAUSE,
STOP_PAUSE,
START_CHARGING,
STOP_CHARGING,
INITIALIZATION_POSITION,
PICK,
DROP,
CANCEL_ORDER,
ROTATE,
REQUEST_FACTSHEET,
REQUEST_VISUALIZATION,
REQUEST_STATE,
}
startPause,
stopPause,
startCharging,
stopCharging,
initPosition,
stateRequest,
factsheetRequest,
logReport,
pick,
drop,
detectObject,
finePositioning,
waitForTrigger,
cancelOrder,
liftUp,
liftDown,
liftRotate,
rotate,
rotateKeepLift,
mutedBaseOn,
mutedBaseOff,
mutedLoadOn,
mutedLoadOff,
dockTo,
moveStraightToCoor,
moveStraightWithDistance,
}

10
RobotApp/Interfaces/IInstantActions.cs
View File

@ -7,8 +7,10 @@ public interface IInstantActions
{
ActionState[] ActionStates { get; }
bool HasActionRunning { get; }
bool AddOrderActions(Action[] actions);
bool StartAction(string actionId);
bool AddInstanceAction(Action action);
bool StopAction();
void AddOrderActions(Action[] actions);
void AddInstantAction(Action action);
void StartOrderAction(string actionId, bool wait);
void ClearInstantActions();
void PauseActions();
void ResumeActions();
}

24
RobotApp/Interfaces/ILocalization.cs
View File

@ -60,11 +60,11 @@ public interface ILocalization
MessageResult SetInitializePosition(double x, double y, double theta);
/// <summary>
/// Bắt đầu quá trình lập bản đồ với độ phân giải chỉ định.
/// Bắt đầu quá trình lập bản đồ.
/// </summary>
/// <param name="resolution">đơn vị mét/px</param>
/// <returns></returns>
MessageResult StartMapping(double resolution);
MessageResult StartMapping();
/// <summary>
/// Dừng quá trình lập bản đồ hiện tại và lưu bản đồ với tên chỉ định.
@ -100,7 +100,7 @@ public interface ILocalization
/// <param name="y"></param>
/// <param name="theta"></param>
/// <returns></returns>
MessageResult SwitchMap(string mapName, double x, double y, double theta);
MessageResult SwitchMap(string mapName, bool useInitialPose, double x, double y, double theta);
/// <summary>
/// Thay đổi gốc tọa độ của bản đồ hiện tại.
@ -110,4 +110,22 @@ public interface ILocalization
/// <param name="theta"></param>
/// <returns></returns>
MessageResult ChangeMapOrigin(double x, double y, double theta);
/// <summary>
/// Bắt đầu quá trình cập nhật bản đồ.
/// </summary>
/// <returns></returns>
MessageResult StartUpdateMap();
/// <summary>
/// Kết thúc quá trình cập nhật bản đồ.
/// </summary>
/// <returns></returns>
MessageResult StopUpdateMap(bool save);
/// <summary>
/// Xóa bỏ các lỗi Slam
/// </summary>
/// <returns></returns>
MessageResult ResetSlamError();
}

2
RobotApp/Interfaces/IOrder.cs
View File

@ -16,4 +16,6 @@ public interface IOrder
void StartOrder(string orderId, Node[] nodes, Edge[] edges);
void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
void StopOrder();
void PauseOrder();
void ResumeOrder();
}

285
RobotApp/Protos/xloc.proto Normal file
View File

@ -0,0 +1,285 @@
syntax = "proto3";
package xloc;
import "google/protobuf/timestamp.proto";
import "google/protobuf/duration.proto";
import "google/protobuf/empty.proto";
// Std Messages
message Header
{
uint32 seq = 1;
google.protobuf.Timestamp stamp = 2;
string frame_id = 3;
}
message ColorRGBA
{
float r = 1;
float g = 2;
float b = 3;
float a = 4;
}
// Geometry Messages
message Point{
double x = 1;
double y = 2;
double z = 3;
}
message Point32
{
float x = 1;
float y = 2;
float z = 3;
}
message PointStamped
{
Header header = 1;
Point point = 2;
}
message Quaternion
{
double x = 1;
double y = 2;
double z = 3;
double w = 4;
}
message Pose
{
Point position = 1;
Quaternion orientation = 2;
}
message Pose2D
{
double x = 1;
double y = 2;
double theta = 3;
}
message PoseArray
{
Header header = 1;
repeated Pose poses = 2;
}
message PoseStamped
{
Header header = 1;
Pose pose = 2;
}
message Vector3
{
double x = 1;
double y = 2;
double z = 3;
}
message Transform
{
Vector3 translation = 1;
Quaternion rotation = 2;
}
message TransformStamped
{
Header header = 1;
string child_frame_id = 2;
Transform transform = 3;
}
// Nav Messages
message MapMetaData
{
google.protobuf.Timestamp map_load_time = 1;
float resolution = 2;
uint32 width = 3;
uint32 height = 4;
Point origin = 5;
}
message OccupancyGrid
{
Header header = 1;
MapMetaData info = 2;
repeated int32 data = 3;
}
// Visualization Messages
message Marker
{
enum MarkerType
{
ARROW = 0;
CUBE = 1;
SPHERE = 2;
CYLINDER = 3;
LINE_STRIP = 4;
LINE_LIST = 5;
CUBE_LIST = 6;
SPHERE_LIST = 7;
POINTS = 8;
TEXT_VIEW_FACING = 9;
MESH_RESOURCE = 10;
TRIANGLE_LIST = 11;
}
enum Action
{
ADD = 0;
MODIFY = 1;
DELETE = 2;
DELETEALL = 3;
}
Header header = 1;
string ns = 2;
int32 id = 3;
MarkerType type = 4;
Action action = 5;
Pose pose = 6;
Vector3 scale = 7;
ColorRGBA color = 8;
google.protobuf.Duration lifetime = 9;
bool frame_locked = 10;
repeated Point points = 11;
repeated ColorRGBA colors = 12;
string text = 13;
string mesh_resource = 14;
bool mesh_use_embedded_materials = 15;
}
message MarkerArray
{
repeated Marker markers = 1;
}
// XLOC Custom Messages
message StatusResponse
{
enum StatusCode
{
OK=0;
CANCELLED=1;
UNKNOWN=2;
INVALID_ARGUMENT=3;
DEADLINE_EXCEEDED=4;
NOT_FOUND=5;
ALREADY_EXISTS=6;
PERMISSION_DENIED=7;
RESOURCE_EXHAUSTED=8;
FAILED_PRECONDITION=9;
ABORTED=10;
OUT_OF_RANGE=11;
UNIMPLEMENTED=12;
INTERNAL=13;
UNAVAILABLE=14;
DATA_LOSS=15;
}
StatusCode code = 1;
string message = 2;
}
message XlocDiagnostics
{
enum SlamState
{
MAPPING = 0;
LOCALIZATION = 1;
PROCESSING = 2;
READY = 3;
ERROR = 4;
}
Header header = 1;
SlamState slam_state = 2;
string slam_state_detail = 3;
string current_active_map = 4;
float reliability = 5;
float matching_score = 6;
}
// Service definitions
message ActivateMapRequest
{
string map_file_name = 1;
}
message ActivateMapResponse
{
StatusResponse status = 1;
}
message SwitchMapRequest
{
string map_file_name = 1;
bool use_initial_pose = 2;
Pose initial_pose = 3;
}
message SwitchMapResponse
{
StatusResponse status = 1;
}
message StartMappingResponse
{
StatusResponse status = 1;
}
message StopMappingRequest
{
string save_map_filename = 1;
}
message StopMappingResponse
{
StatusResponse status = 1;
}
message StartLocalizationResponse
{
StatusResponse status = 1;
int32 trajectory_id = 2;
}
message StopLocalizationResponse
{
StatusResponse status = 1;
}
message ChangeMapOriginRequest
{
Pose new_map_origin = 1;
}
message ChangeMapOriginResponse
{
StatusResponse status = 1;
}
message StartUpdateMapResponse
{
StatusResponse status = 1;
}
message StopUpdateMapRequest
{
bool save_updated_map = 1;
}
message StopUpdateMapResponse
{
StatusResponse status = 1;
}
message SetInitialPoseRequest
{
Pose initial_pose = 1;
}
message SetInitialPoseResponse
{
StatusResponse status = 1;
}
message ResetSlamErrorResponse
{
StatusResponse status = 1;
}
service XlocServices {
rpc ActivateMap (ActivateMapRequest) returns (ActivateMapResponse);
rpc SwitchMap (SwitchMapRequest) returns (SwitchMapResponse);
rpc StartMapping (google.protobuf.Empty) returns (StartMappingResponse);
rpc StopMapping (StopMappingRequest) returns (StopMappingResponse);
rpc StartLocalization (google.protobuf.Empty) returns (StartLocalizationResponse);
rpc StopLocalization (google.protobuf.Empty) returns (StopLocalizationResponse);
rpc ChangeMapOrigin (ChangeMapOriginRequest) returns (ChangeMapOriginResponse);
rpc StartUpdateMap (google.protobuf.Empty) returns (StartUpdateMapResponse);
rpc StopUpdateMap (StopUpdateMapRequest) returns (StopUpdateMapResponse);
rpc SetInitialPose (SetInitialPoseRequest) returns (SetInitialPoseResponse);
rpc ResetSlamError (google.protobuf.Empty) returns (ResetSlamErrorResponse);
rpc GetPose (google.protobuf.Empty) returns (stream PoseStamped);
rpc GetStaticMap (google.protobuf.Empty) returns (OccupancyGrid);
rpc GetStreamMap (google.protobuf.Empty) returns (stream OccupancyGrid);
rpc GetTrajectoryNodeListMarker (google.protobuf.Empty) returns (stream MarkerArray);
rpc GetConstraintListMarker (google.protobuf.Empty) returns (stream MarkerArray);
rpc GetDiagnostics (google.protobuf.Empty) returns (stream XlocDiagnostics);
}

71
RobotApp/RobotApp.csproj
View File

@ -1,36 +1,45 @@
<Project Sdk="Microsoft.NET.Sdk.Web">
<Project Sdk="Microsoft.NET.Sdk.Web">
<PropertyGroup>
<TargetFramework>net9.0</TargetFramework>
<Nullable>enable</Nullable>
<ImplicitUsings>enable</ImplicitUsings>
<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
</PropertyGroup>
<PropertyGroup>
<TargetFramework>net9.0</TargetFramework>
<Nullable>enable</Nullable>
<ImplicitUsings>enable</ImplicitUsings>
<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
</PropertyGroup>
<ItemGroup>
<Compile Remove="Layout\**" />
<Content Remove="Layout\**" />
<EmbeddedResource Remove="Layout\**" />
<None Remove="Layout\**" />
</ItemGroup>
<ItemGroup>
<Compile Remove="Layout\**" />
<Content Remove="Layout\**" />
<EmbeddedResource Remove="Layout\**" />
<None Remove="Layout\**" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\RobotApp.Client\RobotApp.Client.csproj" />
<ProjectReference Include="..\RobotApp.VDA5050\RobotApp.VDA5050.csproj" />
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Server" Version="9.0.9" />
<PackageReference Include="Microsoft.AspNetCore.Diagnostics.EntityFrameworkCore" Version="9.0.9" />
<PackageReference Include="Microsoft.AspNetCore.Identity.EntityFrameworkCore" Version="9.0.9" />
<PackageReference Include="Microsoft.EntityFrameworkCore.Sqlite" Version="9.0.9" />
<PackageReference Include="Microsoft.EntityFrameworkCore.SqlServer" Version="9.0.9" />
<PackageReference Include="Microsoft.EntityFrameworkCore.Tools" Version="9.0.9">
<PrivateAssets>all</PrivateAssets>
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
</PackageReference>
<PackageReference Include="MQTTnet" Version="5.0.1.1416" />
</ItemGroup>
<ItemGroup>
<Folder Include="Tests\" />
</ItemGroup>
<ItemGroup>
<ProjectReference Include="..\RobotApp.Client\RobotApp.Client.csproj" />
<ProjectReference Include="..\RobotApp.VDA5050\RobotApp.VDA5050.csproj" />
<PackageReference Include="Google.Protobuf" Version="3.33.0" />
<PackageReference Include="Grpc.Net.Client" Version="2.71.0" />
<PackageReference Include="Grpc.Tools" Version="2.72.0">
<PrivateAssets>all</PrivateAssets>
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
</PackageReference>
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Server" Version="9.0.9" />
<PackageReference Include="Microsoft.AspNetCore.Diagnostics.EntityFrameworkCore" Version="9.0.9" />
<PackageReference Include="Microsoft.AspNetCore.Identity.EntityFrameworkCore" Version="9.0.9" />
<PackageReference Include="Microsoft.EntityFrameworkCore.Sqlite" Version="9.0.9" />
<PackageReference Include="Microsoft.EntityFrameworkCore.SqlServer" Version="9.0.9" />
<PackageReference Include="Microsoft.EntityFrameworkCore.Tools" Version="9.0.9">
<PrivateAssets>all</PrivateAssets>
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
</PackageReference>
<PackageReference Include="MQTTnet" Version="5.0.1.1416" />
</ItemGroup>
<ItemGroup>
<Folder Include="Tests\" />
</ItemGroup>
<ItemGroup>
<Protobuf Include="Protos\xloc.proto" GrpcServices="Client" />
</ItemGroup>
</Project>

191
RobotApp/Services/Robot/Actions/RobotAction.cs Normal file
View File

@ -0,0 +1,191 @@
using Grpc.Core;
using RobotApp.VDA5050.Factsheet;
using RobotApp.VDA5050.InstantAction;
using RobotApp.VDA5050.State;
using RobotApp.VDA5050.Type;
namespace RobotApp.Services.Robot.Actions;
public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisposable
{
public ActionType Type { get; private set; }
public string Id { get; private set; } = "";
public string Description { get; private set; } = "";
public BlockingType BlockingType { get; private set; }
public ActionParameter[] Parameters { get; private set; } = [];
public ActionStatus Status { get; protected set; } = ActionStatus.WAITING;
public string ResultDescription { get; set; } = "";
public bool IsCompleted => Status == ActionStatus.FINISHED || Status == ActionStatus.FAILED;
public ActionScopes ActionScope { get; set; }
private WatchTimerAsync<RobotAction>? ActionTimer;
private const int ActionInterval = 200;
protected IServiceProvider ServiceProvider = serviceProvider;
protected VDA5050.InstantAction.Action? Action;
protected IServiceScope? Scope;
protected Logger<RobotAction>? Logger;
protected AgvAction? AgvAction;
protected bool IsPaused = false;
protected ActionStatus HistoryStatus;
public bool Initialize(ActionScopes actionScope, VDA5050.InstantAction.Action action)
{
Status = ActionStatus.WAITING;
ActionScope = actionScope;
Action = action;
return Initialize();
}
public void Start()
{
if (Status != ActionStatus.WAITING) return;
Scope ??= ServiceProvider.CreateScope();
Logger = Scope.ServiceProvider.GetRequiredService<Logger<RobotAction>>();
ActionTimer = new(ActionInterval, ActionHandler, Logger);
Status = ActionStatus.INITIALIZING;
ActionTimer.Start();
}
public void Pause()
{
HistoryStatus = Status;
Status = ActionStatus.PAUSED;
}
public void Resume()
{
if (Status == ActionStatus.PAUSED) Status = ActionStatus.WAITING;
}
public void Cancel()
{
Dispose();
}
public async Task WaitAsync(CancellationToken cancellationToken)
{
while (!cancellationToken.IsCancellationRequested && !IsCompleted)
{
await Task.Delay(500, cancellationToken).ConfigureAwait(false);
}
}
public void Wait(CancellationToken cancellationToken)
{
while (!cancellationToken.IsCancellationRequested && !IsCompleted)
{
Task.Delay(500, cancellationToken).Wait(cancellationToken);
}
}
protected virtual Task StartAction()
{
return Task.CompletedTask;
}
protected virtual Task ExecuteAction()
{
return Task.CompletedTask;
}
protected virtual Task PauseAction()
{
return Task.CompletedTask;
}
protected virtual Task ResumeAction()
{
return Task.CompletedTask;
}
protected virtual bool Initialize()
{
if (Action is null) return false;
Scope ??= ServiceProvider.CreateScope();
var FactsheetService = Scope.ServiceProvider.GetRequiredService<RobotFactsheet>();
if (Enum.TryParse(Action.ActionType, out ActionType type)) Type = type;
else return false;
var actionStore = FactsheetService.GetAction(Type);
if (actionStore is null) return false;
if (Enum.TryParse(Action.BlockingType, out BlockingType blockingType)) BlockingType = blockingType;
else return false;
if (!actionStore.BlockingTypes.Any(bt => bt == BlockingType.ToString()) || !actionStore.ActionScopes.Any(sp => sp == ActionScope.ToString()))
{
return false;
}
if (actionStore.ActionParameters.Length > 0)
{
foreach (var parameterStore in actionStore.ActionParameters)
{
if (!parameterStore.IsOptional)
{
if (!Action.ActionParameters.Any(a => a.Key == parameterStore.Key))
{
return false;
}
}
}
Parameters = Action.ActionParameters;
}
Id = Action.ActionId;
Description = Action.ActionDescription;
AgvAction = actionStore;
Status = ActionStatus.WAITING;
return true;
}
private async Task ActionHandler()
{
try
{
if (Status == ActionStatus.INITIALIZING)
{
Logger?.Info($"Thực hiện action {Type}");
Status = ActionStatus.RUNNING;
await StartAction();
}
else if (Status == ActionStatus.RUNNING)
{
await ExecuteAction();
}
else if (Status == ActionStatus.PAUSED)
{
await PauseAction();
}
else if (Status == ActionStatus.WAITING && IsPaused)
{
await ResumeAction();
}
if (IsCompleted)
{
await Dispose();
}
}
catch (Exception ex)
{
Logger?.Error($"Thực hiện action {Type} xảy ra lỗi: {ex.Message}");
Status = ActionStatus.FAILED;
ResultDescription = $"Thực hiện action {Type} xảy ra lỗi: {ex.Message}";
}
}
private Task Dispose()
{
ActionTimer?.Dispose();
return Task.CompletedTask;
}
public async ValueTask DisposeAsync()
{
await Dispose();
GC.SuppressFinalize(this);
}
}

14
RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotDockToAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotDockToAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotFactsheetRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotLiftDownAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotLiftRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotLiftUpAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotMoveStraightToCoorAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotMoveStraightToCoorAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotMoveStraightWithDistanceAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotMoveStraightWithDistanceAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotRotateAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotStartChargingAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

23
RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs Normal file
View File

@ -0,0 +1,23 @@
using RobotApp.VDA5050.State;
namespace RobotApp.Services.Robot.Actions;
public class RobotStartPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
Scope ??= ServiceProvider.CreateScope();
var robotController = Scope.ServiceProvider.GetRequiredService<RobotController>();
robotController.Pause();
Status = ActionStatus.FINISHED;
ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription;
return base.StartAction();
}
protected override Task ExecuteAction()
{
Status = ActionStatus.FINISHED;
ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription;
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotStateRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotStopChargingAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

14
RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs Normal file
View File

@ -0,0 +1,14 @@
namespace RobotApp.Services.Robot.Actions;
public class RobotStopPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
{
return base.StartAction();
}
protected override Task ExecuteAction()
{
return base.ExecuteAction();
}
}

31
RobotApp/Services/Robot/RobotAction.cs
View File

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

118
RobotApp/Services/Robot/RobotActionController.cs Normal file
View File

@ -0,0 +1,118 @@
using RobotApp.Interfaces;
using RobotApp.Services.Robot.Actions;
using RobotApp.VDA5050.Factsheet;
using RobotApp.VDA5050.State;
using RobotApp.VDA5050.Type;
using System.Collections.Concurrent;
namespace RobotApp.Services.Robot;
public class RobotActionController(Logger<RobotActionController> Logger, RobotActionStorage ActionStorage) : BackgroundService, IInstantActions
{
public ActionState[] ActionStates => [.. Actions.Values.Select(a => new ActionState
{
ActionId = a.Id,
ActionType = a.Type.ToString(),
ActionDescription = a.Description,
ActionStatus = a.Status.ToString(),
ResultDescription = a.ResultDescription,
})];
public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => !a.IsCompleted);
private readonly ConcurrentDictionary<string, RobotAction> Actions = [];
private readonly ConcurrentQueue<(ActionScopes scope, VDA5050.InstantAction.Action action)> ActionQueue = [];
private WatchTimer<RobotActionController>? HandlerTimer;
private const int HandlerInterval = 200;
public void AddInstantAction(VDA5050.InstantAction.Action action)
{
ActionQueue.Enqueue((ActionScopes.INSTANT, action));
}
public void AddOrderActions(VDA5050.InstantAction.Action[] actions)
{
foreach (var action in actions)
{
ActionQueue.Enqueue((ActionScopes.NODE, action));
}
}
public void StartOrderAction(string actionId, bool wait)
{
if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null)
{
robotAction.Start();
if(wait)
{
robotAction.Wait(CancellationToken.None);
}
}
}
public void PauseActions()
{
foreach(var action in Actions.Values)
{
action.Pause();
}
}
public void ResumeActions()
{
foreach (var action in Actions.Values)
{
action.Resume();
}
}
private void ActionHandler()
{
try
{
while(!ActionQueue.IsEmpty)
{
if (ActionQueue.TryDequeue(out var result))
{
if (Actions.ContainsKey(result.action.ActionId)) return;
if (Enum.TryParse(result.action.ActionType, out ActionType actionType))
{
var robotAction = ActionStorage[actionType];
if (robotAction is not null)
{
var init = robotAction.Initialize(result.scope, result.action);
if (init)
{
Actions.TryAdd(result.action.ActionId, robotAction);
if(result.scope == ActionScopes.INSTANT) robotAction.Start();
}
}
}
}
}
}
catch (Exception ex)
{
Logger?.Error($"Khởi tạo action xảy ra lỗi: {ex.Message}");
}
}
public void ClearInstantActions()
{
ActionQueue.Clear();
Actions.Clear();
}
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
await Task.Yield();
HandlerTimer = new(HandlerInterval, ActionHandler, Logger);
HandlerTimer.Start();
}
public override Task StopAsync(CancellationToken cancellationToken)
{
HandlerTimer?.Dispose();
return base.StopAsync(cancellationToken);
}
}

33
RobotApp/Services/Robot/RobotActionStorage.cs Normal file
View File

@ -0,0 +1,33 @@
using RobotApp.Services.Robot.Actions;
using RobotApp.VDA5050.Type;
namespace RobotApp.Services.Robot;
public class RobotActionStorage(IServiceProvider ServiceProvider)
{
public RobotAction? this[ActionType type] => type switch
{
ActionType.startPause => new RobotStartPauseAction(ServiceProvider),
ActionType.stopPause => new RobotStopPauseAction(ServiceProvider),
ActionType.startCharging => new RobotStartChargingAction(ServiceProvider),
ActionType.stopCharging => new RobotStopChargingAction(ServiceProvider),
ActionType.initPosition => new RobotInitPositionAction(ServiceProvider),
ActionType.stateRequest => new RobotStateRequestAction(ServiceProvider),
ActionType.factsheetRequest => new RobotFactsheetRequestAction(ServiceProvider),
ActionType.cancelOrder => new RobotCancelOrderAction(ServiceProvider),
ActionType.liftUp => new RobotLiftUpAction(ServiceProvider),
ActionType.liftDown => new RobotLiftDownAction(ServiceProvider),
ActionType.liftRotate => new RobotLiftRotateAction(ServiceProvider),
ActionType.rotate => new RobotRotateAction(ServiceProvider),
ActionType.rotateKeepLift => new RobotRotateKeepLift(ServiceProvider),
ActionType.mutedBaseOn => new RobotMutedBaseOnAction(ServiceProvider),
ActionType.mutedBaseOff => new RobotMutedBaseOffAction(ServiceProvider),
ActionType.mutedLoadOn => new RobotMutedLoadOnAction(ServiceProvider),
ActionType.mutedLoadOff => new RobotMutedLoadOffAction(ServiceProvider),
ActionType.dockTo => new RobotDockToAction(ServiceProvider),
ActionType.moveStraightToCoor => new RobotMoveStraightToCoorAction(ServiceProvider),
ActionType.moveStraightWithDistance => new RobotMoveStraightWithDistanceAction(ServiceProvider),
_ => null,
};
}

2
RobotApp/Services/Robot/RobotConfiguration.cs
View File

@ -22,4 +22,6 @@ public class RobotConfiguration
public byte PLCUnitId { get; set; } = 1;
public bool IsSimulation { get; set; } = true;
public SimulationModel SimulationModel { get; set; } = new();
public string XlocAddress { get; set; } = "";
}

2
RobotApp/Services/Robot/RobotConnection.cs
View File

@ -54,7 +54,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration,
public async Task StartConnection(CancellationToken cancellationToken)
{
MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger);
MqttClient = new MQTTClient(RobotConfiguration.SerialNumber, VDA5050Setting, MQTTClientLogger);
MqttClient.OrderChanged += OrderChanged;
MqttClient.InstanceActionsChanged += InstanceActionsChanged;
await MqttClient.ConnectAsync(cancellationToken);

14
RobotApp/Services/Robot/RobotController.cs
View File

@ -26,10 +26,10 @@ public partial class RobotController(IOrder OrderManager,
await InitializationingHandler(stoppingToken);
}
public override async Task StopAsync(CancellationToken cancellationToken)
public override Task StopAsync(CancellationToken cancellationToken)
{
StopHandler();
await base.StopAsync(cancellationToken);
return base.StopAsync(cancellationToken);
}
public void NewOrderUpdated(OrderMsg order)
@ -90,4 +90,14 @@ public partial class RobotController(IOrder OrderManager,
}
}
}
public void Pause()
{
}
public void Resume()
{
}
}

328
RobotApp/Services/Robot/RobotFactsheet.cs
View File

@ -1,34 +1,348 @@

using RobotApp.Common.Shares;
using RobotApp.Common.Shares;
using RobotApp.VDA5050;
using RobotApp.VDA5050.Factsheet;
using RobotApp.VDA5050.InstantAction;
using RobotApp.VDA5050.Type;
using System.Text.Json;
namespace RobotApp.Services.Robot;
public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService
public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration RobotConfiguration) : BackgroundService
{
private readonly Dictionary<ActionType, AgvAction> AgvActions = new()
{
{ ActionType.startPause, StartPause},
{ ActionType.stopPause, StopPause},
{ ActionType.startCharging, StartCharging},
{ ActionType.stopCharging, StopCharging},
{ ActionType.initPosition, InitPosition},
{ ActionType.stateRequest, StateRequest},
{ ActionType.factsheetRequest, FactsheetRequest},
{ ActionType.cancelOrder, CancelOrder},
{ ActionType.liftUp, LiftUp},
{ ActionType.liftDown, LiftDown},
{ ActionType.liftRotate, LiftRotate},
{ ActionType.rotate, Rotate},
{ ActionType.rotateKeepLift, RotateKeepLift},
{ ActionType.mutedBaseOn, MutedBaseOn},
{ ActionType.mutedBaseOff, MutedBaseOff},
{ ActionType.mutedLoadOn, MutedLoadOn},
{ ActionType.mutedLoadOff, MutedLoadOff},
{ ActionType.dockTo, DockTo},
{ ActionType.moveStraightToCoor, MoveStraightToCoor},
{ ActionType.moveStraightWithDistance, MoveStraightWithDistance},
};
public AgvAction? GetAction(ActionType actionType) => AgvActions.TryGetValue(actionType, out AgvAction? value) && value is not null ? value : null;
public async Task PubFactsheet()
{
if (!RobotConnection.IsConnected) return;
FactSheetMsg factSheet = new()
{
SerialNumber = RobotConnection.SerialNumber,
SerialNumber = RobotConfiguration.SerialNumber,
ProtocolFeatures = new()
{
AgvActions = [],
AgvActions = [..AgvActions.Values],
}
};
string factSheetJson = JsonSerializer.Serialize(factSheet, JsonOptionExtends.Write);
await RobotConnection.Publish(VDA5050Topic.FACTSHEET.ToTopicString(), factSheetJson);
}
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
while(!stoppingToken.IsCancellationRequested)
await Task.Yield();
while (!stoppingToken.IsCancellationRequested)
{
await Task.Delay(1000, stoppingToken);
if (RobotConnection.IsConnected) break;
await Task.Delay(1000);
}
await PubFactsheet();
}
public readonly static AgvAction StartPause = new()
{
ActionType = ActionType.startPause.ToString(),
ActionDescription = "Tam dừng robot.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã tạm dừng.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction StopPause = new()
{
ActionType = ActionType.stopPause.ToString(),
ActionDescription = "Tiếp tục hoạt động robot sau khi tạm dừng.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã tiếp tục hoạt động.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()],
};
public readonly static AgvAction StartCharging = new()
{
ActionType = ActionType.startCharging.ToString(),
ActionDescription = "Bắt đầu quá trình sạc pin.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã bắt đầu sạc pin.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction StopCharging = new()
{
ActionType = ActionType.stopCharging.ToString(),
ActionDescription = "Kết thúc quá trình sạc pin.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã kết thúc sạc pin.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction InitPosition = new()
{
ActionType = ActionType.initPosition.ToString(),
ActionDescription = "Khởi tạo vị trí robot.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [
new()
{
Key = "x",
Description = "Tọa độ X của vị trí khởi tạo.",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
},
new()
{
Key = "y",
Description = "Tọa độ Y của vị trí khởi tạo.",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
},
new()
{
Key = "theta",
Description = "Góc quay (theta) của vị trí khởi tạo. (rad)",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
}],
ResultDescription = "Robot đã khởi tạo vị trí.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction StateRequest = new()
{
ActionType = ActionType.stateRequest.ToString(),
ActionDescription = "Yêu cầu gửi trạng thái robot ngay lập tức.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã gửi trạng thái ngay lập tức.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction FactsheetRequest = new()
{
ActionType = ActionType.factsheetRequest.ToString(),
ActionDescription = "Yêu cầu gửi Factsheet robot ngay lập tức.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã gửi Factsheet ngay lập tức.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction CancelOrder = new()
{
ActionType = ActionType.cancelOrder.ToString(),
ActionDescription = "Hủy bỏ Order hiện tại của robot.",
ActionScopes = [ActionScopes.INSTANT.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã hủy bỏ Order hiện tại.",
BlockingTypes = [BlockingType.NONE.ToString()],
};
public readonly static AgvAction LiftUp = new()
{
ActionType = ActionType.liftUp.ToString(),
ActionDescription = "Nâng cao bàn nâng của robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã nâng cao bàn nâng.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction LiftDown = new()
{
ActionType = ActionType.liftDown.ToString(),
ActionDescription = "Hạ thấp bàn nâng của robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã hạ thấp bàn nâng.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction LiftRotate = new()
{
ActionType = ActionType.liftRotate.ToString(),
ActionDescription = "Xoay bàn nâng của robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [
new()
{
Key = "angle",
Description = "Góc xoay của bàn nâng. (rad)",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
}],
ResultDescription = "Robot đã xoay bàn nâng.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction Rotate = new()
{
ActionType = ActionType.rotate.ToString(),
ActionDescription = "Xoay robot tại chỗ.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [
new()
{
Key = "angle",
Description = "Góc xoay của robot. (rad)",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
}],
ResultDescription = "Robot đã xoay tại chỗ.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction RotateKeepLift = new()
{
ActionType = ActionType.rotateKeepLift.ToString(),
ActionDescription = "Xoay robot tại chỗ giữ nguyên trạng thái bàn nâng.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [
new()
{
Key = "angle",
Description = "Góc xoay của robot. (rad)",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
}],
ResultDescription = "Robot đã xoay tại chỗ giữ nguyên trạng thái bàn nâng.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction MutedBaseOn = new()
{
ActionType = ActionType.mutedBaseOn.ToString(),
ActionDescription = "Bật chế độ muted base robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã bật chế độ muted base.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction MutedBaseOff = new()
{
ActionType = ActionType.mutedBaseOff.ToString(),
ActionDescription = "Tắt chế độ muted base robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã tắt chế độ muted base.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction MutedLoadOn = new()
{
ActionType = ActionType.mutedLoadOn.ToString(),
ActionDescription = "Bật chế độ muted load robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã bật chế độ muted load.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction MutedLoadOff = new()
{
ActionType = ActionType.mutedLoadOff.ToString(),
ActionDescription = "Tắt chế độ muted load robot.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã tắt chế độ muted load.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
};
public readonly static AgvAction DockTo = new()
{
ActionType = ActionType.dockTo.ToString(),
ActionDescription = "Robot di chuyển vào vị trí đặc biết",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [
new()
{
Key = "dockId",
Description = "ID của vị trí dock.",
ValueDataType = ValueDataType.STRING.ToString(),
IsOptional = false,
}],
ResultDescription = "Robot đã dock đến vị trí sạc hoặc bến đỗ.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction MoveStraightToCoor = new()
{
ActionType = ActionType.moveStraightToCoor.ToString(),
ActionDescription = "Di chuyển thẳng đến tọa độ xác định.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [
new()
{
Key = "x",
Description = "Tọa độ X đích đến.",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
},
new()
{
Key = "y",
Description = "Tọa độ Y đích đến.",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
}],
ResultDescription = "Robot đã di chuyển thẳng đến tọa độ xác định.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
public readonly static AgvAction MoveStraightWithDistance = new()
{
ActionType = ActionType.moveStraightWithDistance.ToString(),
ActionDescription = "Di chuyển thẳng với khoảng cách xác định.",
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [
new()
{
Key = "distance",
Description = "Khoảng cách di chuyển. (m)",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = false,
},
new()
{
Key = "direction",
Description = "Hướng di chuyển: 1 - tiến, -1 - lùi.",
ValueDataType = ValueDataType.INTEGER.ToString(),
IsOptional = false,
},
new()
{
Key = "angle",
Description = "Góc di chuyển so với hướng hiện tại của robot. (rad)",
ValueDataType = ValueDataType.FLOAT.ToString(),
IsOptional = true,
}],
ResultDescription = "Robot đã di chuyển thẳng với khoảng cách xác định.",
BlockingTypes = [BlockingType.HARD.ToString()],
};
}

449
RobotApp/Services/Robot/RobotLocalization.cs
View File

@ -1,83 +1,438 @@
using RobotApp.Common.Shares;
using Google.Protobuf.WellKnownTypes;
using Grpc.Core;
using Grpc.Net.Client;
using RobotApp.Common.Shares;
using RobotApp.Interfaces;
using RobotApp.Services.Robot.Simulation;
using System;
using Xloc;
using static Xloc.XlocDiagnostics.Types;
namespace RobotApp.Services.Robot;
public class Xloc
public class XlocData
{
public UInt32 Sequence { get; set; }
public Timestamp Stamp { get; set; } = new();
public string FrameId { get; set; } = "";
public double X { get; set; }
public double Y { get; set; }
public double Theta { get; set; }
public string SlamState { get; set; } = string.Empty;
public SlamState SlamState { get; set; }
public string SlamStateDetail { get; set; } = string.Empty;
public string CurrentActiveMap { get; set; } = string.Empty;
public double Reliability { get; set; }
public double MatchingScore { get; set; }
public bool IsReady { get; set; }
public bool IsReady { get; set; } = false;
}
public class RobotLocalization(RobotConfiguration RobotConfiguration, SimulationVisualization SimVisualization) : ILocalization
public class RobotLocalization(RobotConfiguration RobotConfiguration, SimulationVisualization SimVisualization, Logger<RobotLocalization> Logger) : BackgroundService, ILocalization
{
public double X => IsSimulation ? SimVisualization.X : Xloc.X;
public UInt32 Sequence => IsSimulation ? 0 : XlocData.Sequence;
public Timestamp Stamp => IsSimulation ? new() : XlocData.Stamp;
public string FrameId => IsSimulation ? "" : XlocData.FrameId;
public double X => IsSimulation ? SimVisualization.X : XlocData.X;
public double Y => IsSimulation ? SimVisualization.Y : XlocData.Y;
public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : XlocData.Theta;
public string SlamState => IsSimulation ? "Localization" : XlocData.SlamState.ToString();
public string SlamStateDetail => IsSimulation ? "" : XlocData.SlamStateDetail;
public string CurrentActiveMap => IsSimulation ? "" : XlocData.CurrentActiveMap;
public double Reliability => IsSimulation ? 100 : XlocData.Reliability;
public double MatchingScore => IsSimulation ? 100 : XlocData.MatchingScore;
public bool IsReady => IsSimulation || XlocData.IsReady;
public double Y => IsSimulation ? SimVisualization.Y : Xloc.Y;
public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : Xloc.Theta;
public string SlamState => IsSimulation ? "Localization" : Xloc.SlamState;
public string SlamStateDetail => IsSimulation ? "" : Xloc.SlamStateDetail;
public string CurrentActiveMap => IsSimulation ? "" : Xloc.CurrentActiveMap;
public double Reliability => IsSimulation ? 100 : Xloc.Reliability;
public double MatchingScore => IsSimulation ? 100 : Xloc.MatchingScore;
public bool IsReady => IsSimulation || Xloc.IsReady;
private readonly Xloc Xloc = new();
private readonly XlocData XlocData = new();
private readonly bool IsSimulation = RobotConfiguration.IsSimulation;
private readonly XlocServices.XlocServicesClient XlocClient = new(GrpcChannel.ForAddress(RobotConfiguration.XlocAddress));
private WatchTimerAsync<RobotLocalization>? ReaderTimer;
private const int ReaderInterval = 50;
private int TimerCounter = 0;
private AsyncServerStreamingCall<PoseStamped>? XlocPose;
private AsyncServerStreamingCall<XlocDiagnostics>? XlocDiagnostics;
public MessageResult ActivateMap(string mapName)
{
throw new NotImplementedException();
try
{
var response = XlocClient.ActivateMap(new ActivateMapRequest()
{
MapFileName = mapName
});
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Kích hoạt bản đồ '{mapName}' thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, $"Kích hoạt bản đồ '{mapName}' thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Kích hoạt bản đồ '{mapName}' thất bại: {ex.Message}");
return new(false, $"Kích hoạt bản đồ '{mapName}' thất bại");
}
}
public MessageResult ChangeMapOrigin(double x, double y, double theta)
public MessageResult SwitchMap(string mapName, bool useInitialPose, double x, double y, double theta)
{
throw new NotImplementedException();
try
{
var xyzw = QuaternionToXYZW(0, 0, theta);
var response = XlocClient.SwitchMap(new SwitchMapRequest()
{
MapFileName = mapName,
UseInitialPose = useInitialPose,
InitialPose = new Pose()
{
Position = new Point()
{
X = x,
Y = y,
Z = 0,
},
Orientation = new Quaternion()
{
X = xyzw.x,
Y = xyzw.y,
Z = xyzw.z,
W = xyzw.z
}
}
});
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Chuyển đổi bản đồ '{mapName}' thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, $"Chuyển đổi bản đồ '{mapName}' thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Chuyển đổi bản đồ '{mapName}' thất bại: {ex.Message}");
return new(false, $"Chuyển đổi bản đồ '{mapName}' thất bại");
}
}
public MessageResult SetInitializePosition(double x, double y, double theta)
public MessageResult StartMapping()
{
throw new NotImplementedException();
}
public MessageResult StartLocalization()
{
throw new NotImplementedException();
}
public MessageResult StartMapping(double resolution)
{
throw new NotImplementedException();
}
public MessageResult StopLocalization()
{
throw new NotImplementedException();
try
{
var response = XlocClient.StartMapping(new());
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Bắt đầu quá trình Mapping thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Bắt đầu quá trình Mapping thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Bắt đầu quá trình Mapping thất bại: {ex.Message}");
return new(false, $"Bắt đầu quá trình Mapping thất bại");
}
}
public MessageResult StopMapping(string mapName)
{
throw new NotImplementedException();
try
{
var response = XlocClient.StopMapping(new StopMappingRequest()
{
SaveMapFilename = mapName,
});
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Kết thúc quá trình Mapping với tên '{mapName}' thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, $"Kết thúc quá trình Mapping với tên '{mapName}' thất bại.");
}
}
catch (Exception ex)
{
Logger.Warning($"Kết thúc quá trình Mapping với tên '{mapName}' thất bại: {ex.Message}");
return new(false, $"Kết thúc quá trình Mapping với tên '{mapName}' thất bại.");
}
}
public MessageResult SwitchMap(string mapName, double x, double y, double theta)
public MessageResult StartLocalization()
{
throw new NotImplementedException();
try
{
var response = XlocClient.StartLocalization(new());
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Bắt đầu quá trình Localization thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Bắt đầu quá trình Localization thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Bắt đầu quá trình Localization thất bại: {ex.Message}");
return new(false, $"Bắt đầu quá trình Localization thất bại");
}
}
public MessageResult StopLocalization()
{
try
{
var response = XlocClient.StopLocalization(new());
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Kết thúc quá trình Localization thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Kết thúc quá trình Localization thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Kết thúc quá trình Localization thất bại: {ex.Message}");
return new(false, $"Kết thúc quá trình Localization thất bại");
}
}
public MessageResult ChangeMapOrigin(double x, double y, double theta)
{
try
{
var xyzw = QuaternionToXYZW(0, 0, theta);
var response = XlocClient.ChangeMapOrigin(new ChangeMapOriginRequest()
{
NewMapOrigin = new Pose()
{
Position = new Point()
{
X = x,
Y = y,
Z = 0,
},
Orientation = new Quaternion()
{
X = xyzw.x,
Y = xyzw.y,
Z = xyzw.z,
W = xyzw.w
}
}
});
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Thay đổi gốc bản đồ thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Thay đổi gốc bản đồ thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Thay đổi gốc bản đồ thất bại: {ex.Message}");
return new(false, $"Thay đổi gốc bản đồ thất bại");
}
}
public MessageResult StartUpdateMap()
{
try
{
var response = XlocClient.StartUpdateMap(new());
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Bắt đầu quá trình cập nhật bản đồ thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Bắt đầu quá trình cập nhật bản đồ thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Bắt đầu quá trình cập nhật bản đồ thất bại: {ex.Message}");
return new(false, $"Bắt đầu quá trình cập nhật bản đồ thất bại");
}
}
public MessageResult StopUpdateMap(bool save)
{
try
{
var response = XlocClient.StopUpdateMap(new StopUpdateMapRequest()
{
SaveUpdatedMap = save,
});
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Kết thúc quá trình cập nhật bản đồ thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Kết thúc quá trình cập nhật bản đồ thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Kết thúc quá trình cập nhật bản đồ thất bại: {ex.Message}");
return new(false, $"Kết thúc quá trình cập nhật bản đồ thất bại");
}
}
public MessageResult SetInitializePosition(double x, double y, double theta)
{
try
{
var xyzw = QuaternionToXYZW(0, 0, theta);
var response = XlocClient.SetInitialPose(new SetInitialPoseRequest()
{
InitialPose = new Pose()
{
Position = new Point()
{
X = x,
Y = y,
Z = 0,
},
Orientation = new Quaternion()
{
X = xyzw.x,
Y = xyzw.y,
Z = xyzw.z,
W = xyzw.w
}
}
});
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning("Khởi tạo vị trí cho robot thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Khởi tạo vị trí cho robot thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Khởi tạo vị trí cho robot thất bại: {ex.Message}");
return new(false, $"Khởi tạo vị trí cho robot thất bại");
}
}
public MessageResult ResetSlamError()
{
try
{
var response = XlocClient.ResetSlamError(new());
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
else
{
Logger.Warning($"Xóa bỏ lỗi Slam thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}");
return new(false, "Xóa bỏ lỗi Slam thất bại");
}
}
catch (Exception ex)
{
Logger.Warning($"Xóa bỏ lỗi Slam thất bại: {ex.Message}");
return new(false, $"Xóa bỏ lỗi Slam thất bại");
}
}
private static (double roll, double pitch, double yaw) QuaternionToRPY(double x, double y, double z, double w)
{
double sinr_cosp = 2.0 * (w * x + y * z);
double cosr_cosp = 1.0 - 2.0 * (x * x + y * y);
double roll = Math.Atan2(sinr_cosp, cosr_cosp);
double sinp = 2.0 * (w * y - z * x);
double pitch;
if (sinp >= 1.0) pitch = Math.PI / 2.0;
else if (sinp <= -1.0) pitch = -Math.PI / 2.0;
else pitch = Math.Asin(sinp);
double siny_cosp = 2.0 * (w * z + x * y);
double cosy_cosp = 1.0 - 2.0 * (y * y + z * z);
double yaw = Math.Atan2(siny_cosp, cosy_cosp);
return (roll, pitch, yaw);
}
private static (double x, double y, double z, double w) QuaternionToXYZW(double roll, double pitch, double yaw)
{
double cy = Math.Cos(yaw * 0.5);
double sy = Math.Sin(yaw * 0.5);
double cp = Math.Cos(pitch * 0.5);
double sp = Math.Sin(pitch * 0.5);
double cr = Math.Cos(roll * 0.5);
double sr = Math.Sin(roll * 0.5);
double w = cr * cp * cy + sr * sp * sy;
double x = sr * cp * cy - cr * sp * sy;
double y = cr * sp * cy + sr * cp * sy;
double z = cr * cp * sy - sr * sp * cy;
return (x, y, z, w);
}
private async Task XlocReader()
{
try
{
if (XlocPose is null || !await XlocPose.ResponseStream.MoveNext())
{
XlocPose = XlocClient.GetPose(new());
return;
}
XlocData.Sequence = XlocPose.ResponseStream.Current.Header.Seq;
XlocData.Stamp = XlocPose.ResponseStream.Current.Header.Stamp;
XlocData.FrameId = XlocPose.ResponseStream.Current.Header.FrameId;
XlocData.X = XlocPose.ResponseStream.Current.Pose.Position.X;
XlocData.Y = XlocPose.ResponseStream.Current.Pose.Position.Y;
XlocData.Theta = QuaternionToRPY(XlocPose.ResponseStream.Current.Pose.Orientation.X,
XlocPose.ResponseStream.Current.Pose.Orientation.Y,
XlocPose.ResponseStream.Current.Pose.Orientation.Z,
XlocPose.ResponseStream.Current.Pose.Orientation.W).yaw;
if (TimerCounter++ >= 10)
{
if (XlocDiagnostics is null || !await XlocDiagnostics.ResponseStream.MoveNext())
{
XlocDiagnostics = XlocClient.GetDiagnostics(new());
return;
}
XlocData.SlamState = XlocDiagnostics.ResponseStream.Current.SlamState;
XlocData.SlamStateDetail = XlocDiagnostics.ResponseStream.Current.SlamStateDetail;
XlocData.CurrentActiveMap = XlocDiagnostics.ResponseStream.Current.CurrentActiveMap;
XlocData.Reliability = XlocDiagnostics.ResponseStream.Current.Reliability;
XlocData.MatchingScore = XlocDiagnostics.ResponseStream.Current.MatchingScore;
}
}
catch (RpcException rpcEx)
{
XlocData.IsReady = false;
Logger.Warning($"Lỗi giao tiếp rpc với module Xloc: {rpcEx.Message}");
await Task.Delay(1000);
}
catch (Exception ex)
{
XlocData.IsReady = false;
Logger.Warning($"Lỗi giao tiếp với module Xloc: {ex.Message}");
await Task.Delay(1000);
}
}
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
{
if (IsSimulation) return;
while (!stoppingToken.IsCancellationRequested)
{
XlocPose = XlocClient.GetPose(new(), cancellationToken: stoppingToken);
XlocDiagnostics = XlocClient.GetDiagnostics(new(), cancellationToken: stoppingToken);
if (await XlocPose.ResponseStream.MoveNext(stoppingToken) && await XlocPose.ResponseStream.MoveNext(stoppingToken)) break;
await Task.Delay(2000);
}
ReaderTimer = new(ReaderInterval, XlocReader, Logger);
ReaderTimer.Start();
XlocData.IsReady = true;
}
public override Task StopAsync(CancellationToken cancellationToken)
{
ReaderTimer?.Dispose();
return base.StopAsync(cancellationToken);
}
}

10
RobotApp/Services/Robot/RobotOrderController.cs
View File

@ -154,6 +154,16 @@ public class RobotOrderController(INavigation NavigationManager, IInstantActions
EdgeStates = [];
}
public void PauseOrder()
{
throw new NotImplementedException();
}
public void ResumeOrder()
{
throw new NotImplementedException();
}
private void HandleNavigationStart()
{
OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);

7
RobotApp/Services/RobotExtensions.cs
View File

@ -18,12 +18,13 @@ public static class RobotExtensions
services.AddInterfaceServiceSingleton<IDriver, RobotDriver>();
services.AddInterfaceServiceSingleton<IError, RobotErrors>();
services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
services.AddInterfaceServiceSingleton<IInstantActions, RobotAction>();
services.AddInterfaceServiceSingleton<ILocalization, RobotLocalization>();
services.AddInterfaceServiceSingleton<IInstantActions, RobotActionController>();
services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
services.AddHostedInterfaceServiceSingleton<ILocalization, RobotLocalization>();
services.AddHostedServiceSingleton<RobotController>();
services.AddHostedServiceSingleton<RobotVisualization>();
return services;

1
RobotApp/Services/State/RobotState.cs
View File

@ -14,7 +14,6 @@ public abstract class RobotState<T>(Enum name, IRobotState? superState = null) :
public event Action<IRobotState>? OnEnter;
public event Action<IRobotState>? OnExit;
public event Action<IRobotState, IRobotState>? OnTransition;
public virtual void Enter()
{