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