143 lines
5.5 KiB
C#
143 lines
5.5 KiB
C#
using RobotNet.VDA5050;
|
|
using RobotNet.VDA5050.State;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Navigation;
|
|
using RobotNet10.RobotApp.Services.Robot.Connection;
|
|
using RobotNet10.RobotApp.Services.State;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public class RobotStates(IConnectionConfig ConnectionConfig,
|
|
IRobotConnectionsService RobotConnectionsService,
|
|
RobotStateMachine StateManager,
|
|
ILogger<RobotStates> Logger,
|
|
IOrder OrderManager,
|
|
IAction ActionManager,
|
|
IPlcController PeripheralManager,
|
|
IInfomation InfoManager,
|
|
IError ErrorManager,
|
|
ILocalization LocalizationManager,
|
|
IDeviceProvider DeviceProvider,
|
|
ILoad LoadManager,
|
|
INavigation NavigationManager,
|
|
IVelocityController VelocityController) : IState
|
|
{
|
|
private uint HeaderId = 0;
|
|
|
|
private WatchTimerAsync<RobotStates>? UpdateStateTimer;
|
|
private const int UpdateStateInterval = 1000;
|
|
|
|
public async Task PubState()
|
|
{
|
|
try
|
|
{
|
|
if (!RobotConnectionsService.IsConnected) return;
|
|
await RobotConnectionsService.PublishStateAsync(GetStateMsg());
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
private StateMsg GetStateMsg()
|
|
{
|
|
var vdaConfig = ConnectionConfig.GetVDA5050Config();
|
|
var batteryDevice = DeviceProvider.GetDevice("battery-varta-001");
|
|
RobotNet10.Shared.Sensor.BatteryState batteryState = new();
|
|
if (batteryDevice is IBattery battery && battery.CurrentBatteryState.HasValue && battery.CurrentBatteryState is RobotNet10.Shared.Sensor.BatteryState state)
|
|
{
|
|
batteryState = state;
|
|
}
|
|
return new StateMsg
|
|
{
|
|
HeaderId = HeaderId++,
|
|
Manufacturer = vdaConfig.Manufacturer,
|
|
Version = vdaConfig.Version,
|
|
SerialNumber = vdaConfig.SerialNumber,
|
|
Maps = [],
|
|
OrderId = OrderManager.OrderId,
|
|
OrderUpdateId = OrderManager.OrderUpdateId,
|
|
ZoneSetId = LocalizationManager.CurrentActiveMap,
|
|
LastNodeId = OrderManager.LastNodeId,
|
|
LastNodeSequenceId = OrderManager.LastNodeSequenceId,
|
|
Driving = NavigationManager.Driving,
|
|
Paused = OrderManager.IsPaused,
|
|
NewBaseRequest = OrderManager.NewBaseRequest,
|
|
DistanceSinceLastNode = OrderManager.DistanceSinceLastNode,
|
|
OperatingMode = PeripheralManager.PeripheralMode.ToString(),
|
|
NodeStates = OrderManager.NodeStates,
|
|
EdgeStates = OrderManager.EdgeStates,
|
|
ActionStates = ActionManager.ActionStates,
|
|
Information = [General, .. InfoManager.InformationState],
|
|
Errors = ErrorManager.ErrorsState,
|
|
AgvPosition = new()
|
|
{
|
|
X = LocalizationManager.X,
|
|
Y = LocalizationManager.Y,
|
|
Theta = LocalizationManager.Theta,
|
|
LocalizationScore = LocalizationManager.LocalizationScore,
|
|
MapId = LocalizationManager.CurrentActiveMap,
|
|
DeviationRange = LocalizationManager.DeviationRange,
|
|
PositionInitialized = LocalizationManager.PositionInitialized,
|
|
},
|
|
BatteryState = new()
|
|
{
|
|
Charging = batteryState.Current > 0,
|
|
BatteryHealth = 100,
|
|
Reach = 0,
|
|
BatteryVoltage = batteryState.Voltage is double.NaN ? 0 : batteryState.Voltage,
|
|
BatteryCharge = batteryState.Percentage is double.NaN ? 0 : batteryState.Percentage,
|
|
},
|
|
Loads = LoadManager.Load,
|
|
Velocity = new()
|
|
{
|
|
Vx = VelocityController.ActualVelocity.Linear,
|
|
Vy = 0,
|
|
Omega = VelocityController.ActualVelocity.Angular,
|
|
},
|
|
SafetyState = new()
|
|
{
|
|
FieldViolation = !PeripheralManager.LidarBackProtectField || !PeripheralManager.LidarFrontProtectField || PeripheralManager.LidarFrontTimProtectField,
|
|
EStop = PeripheralManager.Emergency || PeripheralManager.Bumper ? EStop.AUTOACK : EStop.NONE,
|
|
}
|
|
};
|
|
}
|
|
|
|
private Information General => new()
|
|
{
|
|
InfoType = InformationType.GENERAL.ToJsonString(),
|
|
InfoDescription = "Thông tin chung của robot",
|
|
InfoLevel = InfoLevel.INFO,
|
|
InfoReferences =
|
|
[
|
|
new InfomationReference
|
|
{
|
|
ReferenceKey = InformationReferencesKey.STATE.ToJsonString(),
|
|
ReferenceValue = StateManager.CurrentState.ToString(),
|
|
},
|
|
],
|
|
};
|
|
|
|
private async Task UpdateStateHandler()
|
|
{
|
|
await PubState();
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
if (UpdateStateTimer is not null) Stop();
|
|
|
|
UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
|
|
UpdateStateTimer.Start();
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
UpdateStateTimer?.Dispose();
|
|
UpdateStateTimer = null;
|
|
}
|
|
}
|