68 lines
2.1 KiB
C#
68 lines
2.1 KiB
C#
using RobotNet.VDA5050.Visualization;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Robot.Connection;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public class RobotVisualization(ILocalization Localization,
|
|
INavigation Navigation,
|
|
IConnectionConfig ConnectionConfig,
|
|
IRobotConnectionsService RobotConnectionsService,
|
|
ILogger<RobotVisualization> Logger)
|
|
{
|
|
private uint HeaderId;
|
|
|
|
private WatchThread<RobotVisualization>? UpdateTimer;
|
|
private const int UpdateInterval = 100;
|
|
private VisualizationMsg GetVisualizationMsg()
|
|
{
|
|
var vdaConfig = ConnectionConfig.GetVDA5050Config();
|
|
return new VisualizationMsg()
|
|
{
|
|
HeaderId = HeaderId++,
|
|
Manufacturer = vdaConfig.Manufacturer,
|
|
Version = vdaConfig.Version,
|
|
SerialNumber = vdaConfig.SerialNumber,
|
|
AgvPosition = new AgvPosition()
|
|
{
|
|
X = Localization.X,
|
|
Y = Localization.Y,
|
|
Theta = Localization.Theta
|
|
},
|
|
Velocity = new Velocity()
|
|
{
|
|
Vx = Navigation.VelocityX,
|
|
Vy = Navigation.VelocityY,
|
|
Omega = Navigation.Omega
|
|
}
|
|
};
|
|
}
|
|
|
|
private void UpdateHandler()
|
|
{
|
|
try
|
|
{
|
|
if (!RobotConnectionsService.IsConnected) return;
|
|
var publish = RobotConnectionsService.PublishVisualizationAsync(GetVisualizationMsg());
|
|
publish.ConfigureAwait(false);
|
|
}
|
|
catch { }
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
if (UpdateTimer is not null) Stop();
|
|
|
|
UpdateTimer = new(UpdateInterval, UpdateHandler, Logger, ThreadPriority.Normal);
|
|
UpdateTimer.Start();
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
UpdateTimer?.Dispose();
|
|
UpdateTimer = null;
|
|
}
|
|
}
|