update
This commit is contained in:
Đăng Nguyễn
parent
9ac5270885
commit
ab5d3e1a1a
|
|
@ -9,7 +9,8 @@ public enum ActionScopes
|
||||||
NODE,
|
NODE,
|
||||||
EDGE,
|
EDGE,
|
||||||
}
|
}
|
||||||
public class AgvActions
|
|
||||||
|
public class AgvAction
|
||||||
{
|
{
|
||||||
[Required]
|
[Required]
|
||||||
public string ActionType { get; set; } = string.Empty;
|
public string ActionType { get; set; } = string.Empty;
|
||||||
|
|
|
||||||
|
|
@ -8,5 +8,5 @@ public class ProtocolFeatures
|
||||||
[Required]
|
[Required]
|
||||||
public OptionalParameters[] OptionalParameters { get; set; } = [];
|
public OptionalParameters[] OptionalParameters { get; set; } = [];
|
||||||
[Required]
|
[Required]
|
||||||
public AgvActions[] AgvActions { get; set; } = [];
|
public AgvAction[] AgvActions { get; set; } = [];
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -2,16 +2,32 @@
|
||||||
|
|
||||||
public enum ActionType
|
public enum ActionType
|
||||||
{
|
{
|
||||||
START_PAUSE,
|
startPause,
|
||||||
STOP_PAUSE,
|
stopPause,
|
||||||
START_CHARGING,
|
startCharging,
|
||||||
STOP_CHARGING,
|
stopCharging,
|
||||||
INITIALIZATION_POSITION,
|
initPosition,
|
||||||
PICK,
|
stateRequest,
|
||||||
DROP,
|
factsheetRequest,
|
||||||
CANCEL_ORDER,
|
|
||||||
ROTATE,
|
logReport,
|
||||||
REQUEST_FACTSHEET,
|
pick,
|
||||||
REQUEST_VISUALIZATION,
|
drop,
|
||||||
REQUEST_STATE,
|
detectObject,
|
||||||
}
|
finePositioning,
|
||||||
|
waitForTrigger,
|
||||||
|
|
||||||
|
cancelOrder,
|
||||||
|
liftUp,
|
||||||
|
liftDown,
|
||||||
|
liftRotate,
|
||||||
|
rotate,
|
||||||
|
rotateKeepLift,
|
||||||
|
mutedBaseOn,
|
||||||
|
mutedBaseOff,
|
||||||
|
mutedLoadOn,
|
||||||
|
mutedLoadOff,
|
||||||
|
dockTo,
|
||||||
|
moveStraightToCoor,
|
||||||
|
moveStraightWithDistance,
|
||||||
|
}
|
||||||
|
|
@ -7,8 +7,10 @@ public interface IInstantActions
|
||||||
{
|
{
|
||||||
ActionState[] ActionStates { get; }
|
ActionState[] ActionStates { get; }
|
||||||
bool HasActionRunning { get; }
|
bool HasActionRunning { get; }
|
||||||
bool AddOrderActions(Action[] actions);
|
void AddOrderActions(Action[] actions);
|
||||||
bool StartAction(string actionId);
|
void AddInstantAction(Action action);
|
||||||
bool AddInstanceAction(Action action);
|
void StartOrderAction(string actionId, bool wait);
|
||||||
bool StopAction();
|
void ClearInstantActions();
|
||||||
|
void PauseActions();
|
||||||
|
void ResumeActions();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -60,11 +60,11 @@ public interface ILocalization
|
||||||
MessageResult SetInitializePosition(double x, double y, double theta);
|
MessageResult SetInitializePosition(double x, double y, double theta);
|
||||||
|
|
||||||
/// <summary>
|
/// <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>
|
/// </summary>
|
||||||
/// <param name="resolution">đơn vị mét/px</param>
|
/// <param name="resolution">đơn vị mét/px</param>
|
||||||
/// <returns></returns>
|
/// <returns></returns>
|
||||||
MessageResult StartMapping(double resolution);
|
MessageResult StartMapping();
|
||||||
|
|
||||||
/// <summary>
|
/// <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.
|
/// 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="y"></param>
|
||||||
/// <param name="theta"></param>
|
/// <param name="theta"></param>
|
||||||
/// <returns></returns>
|
/// <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>
|
/// <summary>
|
||||||
/// Thay đổi gốc tọa độ của bản đồ hiện tại.
|
/// 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>
|
/// <param name="theta"></param>
|
||||||
/// <returns></returns>
|
/// <returns></returns>
|
||||||
MessageResult ChangeMapOrigin(double x, double y, double theta);
|
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();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -16,4 +16,6 @@ public interface IOrder
|
||||||
void StartOrder(string orderId, Node[] nodes, Edge[] edges);
|
void StartOrder(string orderId, Node[] nodes, Edge[] edges);
|
||||||
void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
|
void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges);
|
||||||
void StopOrder();
|
void StopOrder();
|
||||||
|
void PauseOrder();
|
||||||
|
void ResumeOrder();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
285
RobotApp/Protos/xloc.proto
Normal file
285
RobotApp/Protos/xloc.proto
Normal 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);
|
||||||
|
}
|
||||||
|
|
@ -1,36 +1,45 @@
|
||||||
<Project Sdk="Microsoft.NET.Sdk.Web">
|
<Project Sdk="Microsoft.NET.Sdk.Web">
|
||||||
|
|
||||||
<PropertyGroup>
|
<PropertyGroup>
|
||||||
<TargetFramework>net9.0</TargetFramework>
|
<TargetFramework>net9.0</TargetFramework>
|
||||||
<Nullable>enable</Nullable>
|
<Nullable>enable</Nullable>
|
||||||
<ImplicitUsings>enable</ImplicitUsings>
|
<ImplicitUsings>enable</ImplicitUsings>
|
||||||
<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
|
<UserSecretsId>aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea</UserSecretsId>
|
||||||
</PropertyGroup>
|
</PropertyGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<Compile Remove="Layout\**" />
|
<Compile Remove="Layout\**" />
|
||||||
<Content Remove="Layout\**" />
|
<Content Remove="Layout\**" />
|
||||||
<EmbeddedResource Remove="Layout\**" />
|
<EmbeddedResource Remove="Layout\**" />
|
||||||
<None Remove="Layout\**" />
|
<None Remove="Layout\**" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ProjectReference Include="..\RobotApp.Client\RobotApp.Client.csproj" />
|
<ProjectReference Include="..\RobotApp.Client\RobotApp.Client.csproj" />
|
||||||
<ProjectReference Include="..\RobotApp.VDA5050\RobotApp.VDA5050.csproj" />
|
<ProjectReference Include="..\RobotApp.VDA5050\RobotApp.VDA5050.csproj" />
|
||||||
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Server" Version="9.0.9" />
|
<PackageReference Include="Google.Protobuf" Version="3.33.0" />
|
||||||
<PackageReference Include="Microsoft.AspNetCore.Diagnostics.EntityFrameworkCore" Version="9.0.9" />
|
<PackageReference Include="Grpc.Net.Client" Version="2.71.0" />
|
||||||
<PackageReference Include="Microsoft.AspNetCore.Identity.EntityFrameworkCore" Version="9.0.9" />
|
<PackageReference Include="Grpc.Tools" Version="2.72.0">
|
||||||
<PackageReference Include="Microsoft.EntityFrameworkCore.Sqlite" Version="9.0.9" />
|
<PrivateAssets>all</PrivateAssets>
|
||||||
<PackageReference Include="Microsoft.EntityFrameworkCore.SqlServer" Version="9.0.9" />
|
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
||||||
<PackageReference Include="Microsoft.EntityFrameworkCore.Tools" Version="9.0.9">
|
</PackageReference>
|
||||||
<PrivateAssets>all</PrivateAssets>
|
<PackageReference Include="Microsoft.AspNetCore.Components.WebAssembly.Server" Version="9.0.9" />
|
||||||
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
<PackageReference Include="Microsoft.AspNetCore.Diagnostics.EntityFrameworkCore" Version="9.0.9" />
|
||||||
</PackageReference>
|
<PackageReference Include="Microsoft.AspNetCore.Identity.EntityFrameworkCore" Version="9.0.9" />
|
||||||
<PackageReference Include="MQTTnet" Version="5.0.1.1416" />
|
<PackageReference Include="Microsoft.EntityFrameworkCore.Sqlite" Version="9.0.9" />
|
||||||
</ItemGroup>
|
<PackageReference Include="Microsoft.EntityFrameworkCore.SqlServer" Version="9.0.9" />
|
||||||
|
<PackageReference Include="Microsoft.EntityFrameworkCore.Tools" Version="9.0.9">
|
||||||
<ItemGroup>
|
<PrivateAssets>all</PrivateAssets>
|
||||||
<Folder Include="Tests\" />
|
<IncludeAssets>runtime; build; native; contentfiles; analyzers; buildtransitive</IncludeAssets>
|
||||||
</ItemGroup>
|
</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>
|
</Project>
|
||||||
|
|
|
||||||
191
RobotApp/Services/Robot/Actions/RobotAction.cs
Normal file
191
RobotApp/Services/Robot/Actions/RobotAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotDockToAction.cs
Normal 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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -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
14
RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs
Normal 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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -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
14
RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotRotateAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs
Normal 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
23
RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs
Normal 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
14
RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs
Normal 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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -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
118
RobotApp/Services/Robot/RobotActionController.cs
Normal 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
33
RobotApp/Services/Robot/RobotActionStorage.cs
Normal 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,
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
@ -22,4 +22,6 @@ public class RobotConfiguration
|
||||||
public byte PLCUnitId { get; set; } = 1;
|
public byte PLCUnitId { get; set; } = 1;
|
||||||
public bool IsSimulation { get; set; } = true;
|
public bool IsSimulation { get; set; } = true;
|
||||||
public SimulationModel SimulationModel { get; set; } = new();
|
public SimulationModel SimulationModel { get; set; } = new();
|
||||||
|
|
||||||
|
public string XlocAddress { get; set; } = "";
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -54,7 +54,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration,
|
||||||
|
|
||||||
public async Task StartConnection(CancellationToken cancellationToken)
|
public async Task StartConnection(CancellationToken cancellationToken)
|
||||||
{
|
{
|
||||||
MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger);
|
MqttClient = new MQTTClient(RobotConfiguration.SerialNumber, VDA5050Setting, MQTTClientLogger);
|
||||||
MqttClient.OrderChanged += OrderChanged;
|
MqttClient.OrderChanged += OrderChanged;
|
||||||
MqttClient.InstanceActionsChanged += InstanceActionsChanged;
|
MqttClient.InstanceActionsChanged += InstanceActionsChanged;
|
||||||
await MqttClient.ConnectAsync(cancellationToken);
|
await MqttClient.ConnectAsync(cancellationToken);
|
||||||
|
|
|
||||||
|
|
@ -26,10 +26,10 @@ public partial class RobotController(IOrder OrderManager,
|
||||||
await InitializationingHandler(stoppingToken);
|
await InitializationingHandler(stoppingToken);
|
||||||
}
|
}
|
||||||
|
|
||||||
public override async Task StopAsync(CancellationToken cancellationToken)
|
public override Task StopAsync(CancellationToken cancellationToken)
|
||||||
{
|
{
|
||||||
StopHandler();
|
StopHandler();
|
||||||
await base.StopAsync(cancellationToken);
|
return base.StopAsync(cancellationToken);
|
||||||
}
|
}
|
||||||
|
|
||||||
public void NewOrderUpdated(OrderMsg order)
|
public void NewOrderUpdated(OrderMsg order)
|
||||||
|
|
@ -90,4 +90,14 @@ public partial class RobotController(IOrder OrderManager,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void Pause()
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Resume()
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,34 +1,348 @@
|
||||||
|
using RobotApp.Common.Shares;
|
||||||
using RobotApp.Common.Shares;
|
|
||||||
using RobotApp.VDA5050;
|
using RobotApp.VDA5050;
|
||||||
using RobotApp.VDA5050.Factsheet;
|
using RobotApp.VDA5050.Factsheet;
|
||||||
|
using RobotApp.VDA5050.InstantAction;
|
||||||
|
using RobotApp.VDA5050.Type;
|
||||||
using System.Text.Json;
|
using System.Text.Json;
|
||||||
|
|
||||||
namespace RobotApp.Services.Robot;
|
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()
|
public async Task PubFactsheet()
|
||||||
{
|
{
|
||||||
if (!RobotConnection.IsConnected) return;
|
if (!RobotConnection.IsConnected) return;
|
||||||
FactSheetMsg factSheet = new()
|
FactSheetMsg factSheet = new()
|
||||||
{
|
{
|
||||||
SerialNumber = RobotConnection.SerialNumber,
|
SerialNumber = RobotConfiguration.SerialNumber,
|
||||||
ProtocolFeatures = new()
|
ProtocolFeatures = new()
|
||||||
{
|
{
|
||||||
AgvActions = [],
|
AgvActions = [..AgvActions.Values],
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
string factSheetJson = JsonSerializer.Serialize(factSheet, JsonOptionExtends.Write);
|
string factSheetJson = JsonSerializer.Serialize(factSheet, JsonOptionExtends.Write);
|
||||||
await RobotConnection.Publish(VDA5050Topic.FACTSHEET.ToTopicString(), factSheetJson);
|
await RobotConnection.Publish(VDA5050Topic.FACTSHEET.ToTopicString(), factSheetJson);
|
||||||
}
|
}
|
||||||
|
|
||||||
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
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;
|
if (RobotConnection.IsConnected) break;
|
||||||
|
await Task.Delay(1000);
|
||||||
}
|
}
|
||||||
await PubFactsheet();
|
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()],
|
||||||
|
};
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -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.Interfaces;
|
||||||
using RobotApp.Services.Robot.Simulation;
|
using RobotApp.Services.Robot.Simulation;
|
||||||
|
using System;
|
||||||
|
using Xloc;
|
||||||
|
using static Xloc.XlocDiagnostics.Types;
|
||||||
|
|
||||||
namespace RobotApp.Services.Robot;
|
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 X { get; set; }
|
||||||
public double Y { get; set; }
|
public double Y { get; set; }
|
||||||
public double Theta { 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 SlamStateDetail { get; set; } = string.Empty;
|
||||||
public string CurrentActiveMap { get; set; } = string.Empty;
|
public string CurrentActiveMap { get; set; } = string.Empty;
|
||||||
public double Reliability { get; set; }
|
public double Reliability { get; set; }
|
||||||
public double MatchingScore { 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;
|
private readonly XlocData XlocData = new();
|
||||||
|
|
||||||
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 bool IsSimulation = RobotConfiguration.IsSimulation;
|
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)
|
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();
|
try
|
||||||
}
|
{
|
||||||
|
var response = XlocClient.StartMapping(new());
|
||||||
public MessageResult StartLocalization()
|
if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true);
|
||||||
{
|
else
|
||||||
throw new NotImplementedException();
|
{
|
||||||
}
|
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");
|
||||||
public MessageResult StartMapping(double resolution)
|
}
|
||||||
{
|
}
|
||||||
throw new NotImplementedException();
|
catch (Exception ex)
|
||||||
}
|
{
|
||||||
|
Logger.Warning($"Bắt đầu quá trình Mapping thất bại: {ex.Message}");
|
||||||
public MessageResult StopLocalization()
|
return new(false, $"Bắt đầu quá trình Mapping thất bại");
|
||||||
{
|
}
|
||||||
throw new NotImplementedException();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
public MessageResult StopMapping(string mapName)
|
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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -154,6 +154,16 @@ public class RobotOrderController(INavigation NavigationManager, IInstantActions
|
||||||
EdgeStates = [];
|
EdgeStates = [];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void PauseOrder()
|
||||||
|
{
|
||||||
|
throw new NotImplementedException();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ResumeOrder()
|
||||||
|
{
|
||||||
|
throw new NotImplementedException();
|
||||||
|
}
|
||||||
|
|
||||||
private void HandleNavigationStart()
|
private void HandleNavigationStart()
|
||||||
{
|
{
|
||||||
OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
|
OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
|
||||||
|
|
|
||||||
|
|
@ -18,12 +18,13 @@ public static class RobotExtensions
|
||||||
services.AddInterfaceServiceSingleton<IDriver, RobotDriver>();
|
services.AddInterfaceServiceSingleton<IDriver, RobotDriver>();
|
||||||
services.AddInterfaceServiceSingleton<IError, RobotErrors>();
|
services.AddInterfaceServiceSingleton<IError, RobotErrors>();
|
||||||
services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
|
services.AddInterfaceServiceSingleton<IInfomation, RobotInfomations>();
|
||||||
services.AddInterfaceServiceSingleton<IInstantActions, RobotAction>();
|
services.AddInterfaceServiceSingleton<IInstantActions, RobotActionController>();
|
||||||
services.AddInterfaceServiceSingleton<ILocalization, RobotLocalization>();
|
|
||||||
services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
|
services.AddInterfaceServiceSingleton<INavigation, RobotNavigation>();
|
||||||
services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
|
|
||||||
services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
|
services.AddInterfaceServiceSingleton<IOrder, RobotOrderController>();
|
||||||
|
|
||||||
|
services.AddHostedInterfaceServiceSingleton<IPeripheral, ISafety, RobotPeripheral>();
|
||||||
|
services.AddHostedInterfaceServiceSingleton<ILocalization, RobotLocalization>();
|
||||||
|
|
||||||
services.AddHostedServiceSingleton<RobotController>();
|
services.AddHostedServiceSingleton<RobotController>();
|
||||||
services.AddHostedServiceSingleton<RobotVisualization>();
|
services.AddHostedServiceSingleton<RobotVisualization>();
|
||||||
return services;
|
return services;
|
||||||
|
|
|
||||||
|
|
@ -14,7 +14,6 @@ public abstract class RobotState<T>(Enum name, IRobotState? superState = null) :
|
||||||
|
|
||||||
public event Action<IRobotState>? OnEnter;
|
public event Action<IRobotState>? OnEnter;
|
||||||
public event Action<IRobotState>? OnExit;
|
public event Action<IRobotState>? OnExit;
|
||||||
public event Action<IRobotState, IRobotState>? OnTransition;
|
|
||||||
|
|
||||||
public virtual void Enter()
|
public virtual void Enter()
|
||||||
{
|
{
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue
Block a user