429 lines
16 KiB
C#
429 lines
16 KiB
C#
using Microsoft.AspNetCore.SignalR;
|
|
using Microsoft.Extensions.Logging;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Hubs;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.Navigation;
|
|
using RobotNet10.RobotApp.Services.Robot.Modules;
|
|
using RobotNet10.RobotApp.Services.Simulation;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.RobotApp.Shared.NavigationMonitor;
|
|
|
|
namespace RobotNet10.RobotApp.Services.NavigationMonitor;
|
|
|
|
/// <summary>
|
|
/// Independent monitoring service for CSharpNavigation.
|
|
/// Periodically reads robot state and broadcasts telemetry via SignalR.
|
|
/// Can be enabled/disabled at runtime without affecting navigation.
|
|
/// </summary>
|
|
public class NavigationMonitorService : IHostedService, IDisposable
|
|
{
|
|
private const int DefaultIntervalMs = 100; // 10Hz internal tick (safety checks)
|
|
private const int BroadcastEveryNTicks = 5; // Broadcast telemetry every 5 ticks = 2Hz
|
|
|
|
private readonly ILocalization _localization;
|
|
private readonly IVelocityController _velocityController;
|
|
private readonly RobotNavigation _navigation;
|
|
private readonly IHubContext<NavigationMonitorHub> _hubContext;
|
|
private readonly ILogger<NavigationMonitorService> _logger;
|
|
|
|
private WatchTimer<NavigationMonitorService>? _timer;
|
|
private volatile bool _telemetryEnabled;
|
|
private volatile bool _safetyStopEnabled;
|
|
private NavigationSafetyConfigDto _safetyConfig = new();
|
|
private readonly Lock _configLock = new();
|
|
|
|
// Safety stop latch state
|
|
private volatile bool _safetyStopLatched;
|
|
private string _safetyStopReason = "";
|
|
private int _safetyStopGraceTicks;
|
|
private const int SafetyStopGraceCount = 20; // 2s at 10Hz
|
|
|
|
// State for acceleration calculation
|
|
private double _lastLinearVel;
|
|
private double _lastAngularVel;
|
|
private long _lastTimestampMs;
|
|
|
|
// Broadcast throttle counter
|
|
private int _broadcastTickCounter;
|
|
|
|
public NavigationMonitorService(
|
|
ILocalization localization,
|
|
IVelocityController velocityController,
|
|
RobotNavigation navigation,
|
|
IHubContext<NavigationMonitorHub> hubContext,
|
|
ILogger<NavigationMonitorService> logger)
|
|
{
|
|
_localization = localization;
|
|
_velocityController = velocityController;
|
|
_navigation = navigation;
|
|
_hubContext = hubContext;
|
|
_logger = logger;
|
|
}
|
|
|
|
public Task StartAsync(CancellationToken cancellationToken)
|
|
{
|
|
_logger.LogInformation("NavigationMonitorService started (telemetry disabled by default)");
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
public Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
StopTimer();
|
|
_logger.LogInformation("NavigationMonitorService stopped");
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
public void SetTelemetryEnabled(bool enabled)
|
|
{
|
|
_telemetryEnabled = enabled;
|
|
if (enabled)
|
|
StartTimer();
|
|
else
|
|
StopTimer();
|
|
|
|
_logger.LogInformation("Navigation telemetry {State}", enabled ? "enabled" : "disabled");
|
|
BroadcastStateAsync();
|
|
}
|
|
|
|
public void SetSafetyStopEnabled(bool enabled)
|
|
{
|
|
_safetyStopEnabled = enabled;
|
|
_logger.LogInformation("Navigation safety stop {State}", enabled ? "enabled" : "disabled");
|
|
BroadcastStateAsync();
|
|
}
|
|
|
|
public void UpdateSafetyConfig(NavigationSafetyConfigDto config)
|
|
{
|
|
lock (_configLock)
|
|
{
|
|
_safetyConfig = config;
|
|
}
|
|
_logger.LogInformation("Navigation safety config updated");
|
|
BroadcastStateAsync();
|
|
}
|
|
|
|
public NavigationMonitorStateDto GetState()
|
|
{
|
|
NavigationSafetyConfigDto configCopy;
|
|
lock (_configLock)
|
|
{
|
|
configCopy = new NavigationSafetyConfigDto
|
|
{
|
|
MaxLinearVelocity = _safetyConfig.MaxLinearVelocity,
|
|
MaxAngularVelocity = _safetyConfig.MaxAngularVelocity,
|
|
MaxLinearAcceleration = _safetyConfig.MaxLinearAcceleration,
|
|
MaxCrossTrackError = _safetyConfig.MaxCrossTrackError,
|
|
MaxHeadingError = _safetyConfig.MaxHeadingError
|
|
};
|
|
}
|
|
|
|
return new NavigationMonitorStateDto
|
|
{
|
|
TelemetryEnabled = _telemetryEnabled,
|
|
SafetyStopEnabled = _safetyStopEnabled,
|
|
SafetyStopLatched = _safetyStopLatched,
|
|
SafetyStopReason = _safetyStopReason,
|
|
SafetyConfig = configCopy,
|
|
UpdateFrequencyHz = 1000.0 / (DefaultIntervalMs * BroadcastEveryNTicks)
|
|
};
|
|
}
|
|
|
|
public void ReleaseSafetyStop()
|
|
{
|
|
if (!_safetyStopLatched) return;
|
|
_safetyStopLatched = false;
|
|
_safetyStopReason = "";
|
|
_safetyStopGraceTicks = SafetyStopGraceCount;
|
|
_navigation.Refresh();
|
|
_logger.LogInformation("Safety stop released by user");
|
|
BroadcastStateAsync();
|
|
}
|
|
|
|
private void StartTimer()
|
|
{
|
|
StopTimer();
|
|
_lastTimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
_lastLinearVel = 0;
|
|
_lastAngularVel = 0;
|
|
_broadcastTickCounter = 0;
|
|
_timer = new WatchTimer<NavigationMonitorService>(DefaultIntervalMs, OnTick, _logger);
|
|
_timer.Start();
|
|
}
|
|
|
|
private void StopTimer()
|
|
{
|
|
_timer?.Dispose();
|
|
_timer = null;
|
|
}
|
|
|
|
private void OnTick()
|
|
{
|
|
try
|
|
{
|
|
var now = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
var dtSeconds = (now - _lastTimestampMs) / 1000.0;
|
|
if (dtSeconds <= 0) dtSeconds = DefaultIntervalMs / 1000.0;
|
|
|
|
// Read robot state
|
|
var x = _localization.X;
|
|
var y = _localization.Y;
|
|
var theta = _localization.Theta;
|
|
var (linearVel, angularVel) = _velocityController.ActualVelocity;
|
|
var modelConfidence = _velocityController.GetModelConfidence();
|
|
var navState = _navigation.State;
|
|
var driving = _navigation.Driving;
|
|
|
|
// Calculate acceleration
|
|
var linearAccel = (linearVel - _lastLinearVel) / dtSeconds;
|
|
var angularAccel = (angularVel - _lastAngularVel) / dtSeconds;
|
|
_lastLinearVel = linearVel;
|
|
_lastAngularVel = angularVel;
|
|
_lastTimestampMs = now;
|
|
|
|
// Calculate CTE and heading error if navigating with path
|
|
double cte = 0;
|
|
double headingError = 0;
|
|
double distanceToGoal = 0;
|
|
|
|
var path = _navigation.CurrentPath; // capture once (volatile)
|
|
if (path is { Count: >= 2 } && driving)
|
|
{
|
|
// Skip heading error check during Rotating/FinePositioning:
|
|
// - Rotating: robot intentionally turning in place, heading diverges from path tangent
|
|
// - FinePositioning: robot re-aligning to dock goal, heading changes rapidly
|
|
bool skipHeadingCheck = navState is NavigationState.Rotating or NavigationState.FinePositioning;
|
|
(cte, headingError) = CalculatePathErrors(x, y, theta, path, skipHeadingCheck);
|
|
var goal = path[^1];
|
|
distanceToGoal = Math.Sqrt((x - goal.X) * (x - goal.X) + (y - goal.Y) * (y - goal.Y));
|
|
}
|
|
|
|
// DockTo-specific telemetry
|
|
DockToTelemetryDto? dockToTelemetry = null;
|
|
if (_navigation.IsDockingActive)
|
|
{
|
|
var dockGoal = _navigation.DockGoal;
|
|
var dockStart = _navigation.DockStartNode;
|
|
dockToTelemetry = new DockToTelemetryDto
|
|
{
|
|
Phase = _navigation.DockPhase,
|
|
Direction = _navigation.DockDirection,
|
|
RetryCount = _navigation.DockRetryCount,
|
|
MaxRetries = _navigation.DockMaxRetries,
|
|
GoalX = dockGoal?.X ?? 0,
|
|
GoalY = dockGoal?.Y ?? 0,
|
|
GoalTheta = dockGoal?.Theta ?? 0,
|
|
TotalWaypoints = _navigation.DockWaypointCount,
|
|
StartX = dockStart?.X ?? 0,
|
|
StartY = dockStart?.Y ?? 0,
|
|
StartTheta = dockStart?.Theta ?? 0,
|
|
Waypoints = DownsampleWaypoints(_navigation.DockWaypoints, 30)
|
|
};
|
|
}
|
|
|
|
// Downsample current path waypoints for client visualization
|
|
var waypoints = path is { Count: >= 2 } && driving
|
|
? DownsampleNavigationWaypoints(path, 50)
|
|
: [];
|
|
|
|
var telemetry = new NavigationTelemetryDto
|
|
{
|
|
TimestampMs = now,
|
|
X = x,
|
|
Y = y,
|
|
Theta = theta,
|
|
LinearVelocity = linearVel,
|
|
AngularVelocity = angularVel,
|
|
LinearAcceleration = double.IsFinite(linearAccel) ? linearAccel : 0,
|
|
AngularAcceleration = double.IsFinite(angularAccel) ? angularAccel : 0,
|
|
NavigationState = navState.ToString(),
|
|
Driving = driving,
|
|
CrossTrackError = cte,
|
|
HeadingError = headingError,
|
|
DistanceToGoal = distanceToGoal,
|
|
ModelConfidence = modelConfidence,
|
|
DockTo = dockToTelemetry,
|
|
Waypoints = waypoints
|
|
};
|
|
|
|
// Broadcast telemetry at reduced rate (2Hz) to save client resources
|
|
_broadcastTickCounter++;
|
|
if (_broadcastTickCounter >= BroadcastEveryNTicks)
|
|
{
|
|
_broadcastTickCounter = 0;
|
|
_ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveTelemetry", telemetry);
|
|
}
|
|
|
|
// Auto-release latch if navigation ended while latched
|
|
if (_safetyStopLatched && !driving)
|
|
{
|
|
_safetyStopLatched = false;
|
|
_safetyStopReason = "";
|
|
_navigation.Refresh();
|
|
_logger.LogInformation("Safety stop auto-released: navigation ended");
|
|
BroadcastStateAsync();
|
|
}
|
|
|
|
// Grace period countdown after user release
|
|
if (_safetyStopGraceTicks > 0) _safetyStopGraceTicks--;
|
|
|
|
// Safety checks
|
|
NavigationSafetyConfigDto configSnapshot;
|
|
lock (_configLock)
|
|
{
|
|
configSnapshot = new NavigationSafetyConfigDto
|
|
{
|
|
MaxLinearVelocity = _safetyConfig.MaxLinearVelocity,
|
|
MaxAngularVelocity = _safetyConfig.MaxAngularVelocity,
|
|
MaxLinearAcceleration = _safetyConfig.MaxLinearAcceleration,
|
|
MaxCrossTrackError = _safetyConfig.MaxCrossTrackError,
|
|
MaxHeadingError = _safetyConfig.MaxHeadingError
|
|
};
|
|
}
|
|
|
|
var violations = NavigationSafetyChecker.Check(telemetry, configSnapshot);
|
|
if (violations.Count > 0)
|
|
{
|
|
foreach (var v in violations)
|
|
{
|
|
_ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveSafetyViolation", v);
|
|
}
|
|
|
|
// Latch safety stop on critical violations: Pause navigation + hold
|
|
if (_safetyStopEnabled && !_safetyStopLatched
|
|
&& _safetyStopGraceTicks == 0
|
|
&& violations.Exists(v => v.Severity == SafetyViolationSeverity.Critical))
|
|
{
|
|
_safetyStopLatched = true;
|
|
_safetyStopReason = violations.First(v => v.Severity == SafetyViolationSeverity.Critical).Message;
|
|
_navigation.SafetyStop();
|
|
_logger.LogWarning("Safety stop latched: {Reason}", _safetyStopReason);
|
|
BroadcastStateAsync();
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in NavigationMonitor tick");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate cross-track error and heading error from current position to closest path point.
|
|
/// </summary>
|
|
private static (double cte, double headingError) CalculatePathErrors(
|
|
double x, double y, double theta,
|
|
IReadOnlyList<NavigationNode> path,
|
|
bool skipHeadingCheck = false)
|
|
{
|
|
// Find closest point on path
|
|
int closestIndex = 0;
|
|
double minDist = double.MaxValue;
|
|
|
|
for (int i = 0; i < path.Count; i++)
|
|
{
|
|
var dx = x - path[i].X;
|
|
var dy = y - path[i].Y;
|
|
var dist = dx * dx + dy * dy;
|
|
if (dist < minDist)
|
|
{
|
|
minDist = dist;
|
|
closestIndex = i;
|
|
}
|
|
}
|
|
|
|
double cte = Math.Sqrt(minDist);
|
|
|
|
if (skipHeadingCheck)
|
|
return (cte, 0);
|
|
|
|
// Calculate heading error using path tangent, accounting for Direction
|
|
double headingError = 0;
|
|
double refTheta;
|
|
if (closestIndex < path.Count - 1)
|
|
{
|
|
var dx = path[closestIndex + 1].X - path[closestIndex].X;
|
|
var dy = path[closestIndex + 1].Y - path[closestIndex].Y;
|
|
refTheta = Math.Atan2(dy, dx);
|
|
}
|
|
else if (closestIndex > 0)
|
|
{
|
|
var dx = path[closestIndex].X - path[closestIndex - 1].X;
|
|
var dy = path[closestIndex].Y - path[closestIndex - 1].Y;
|
|
refTheta = Math.Atan2(dy, dx);
|
|
}
|
|
else
|
|
{
|
|
return (cte, 0);
|
|
}
|
|
|
|
// When path segment is BACKWARD, robot faces opposite to path tangent
|
|
if (path[closestIndex].Direction == RobotDirection.BACKWARD)
|
|
refTheta = NormalizeAngle(refTheta + Math.PI);
|
|
|
|
headingError = NormalizeAngle(theta - refTheta);
|
|
|
|
return (cte, headingError);
|
|
}
|
|
|
|
private static double NormalizeAngle(double angle)
|
|
{
|
|
while (angle > Math.PI) angle -= 2 * Math.PI;
|
|
while (angle < -Math.PI) angle += 2 * Math.PI;
|
|
return angle;
|
|
}
|
|
|
|
private static List<DockWaypointDto> DownsampleWaypoints(IReadOnlyList<NavigationNode>? waypoints, int maxPoints)
|
|
{
|
|
if (waypoints is null or { Count: 0 }) return [];
|
|
if (waypoints.Count <= maxPoints)
|
|
return waypoints.Select(w => new DockWaypointDto { X = w.X, Y = w.Y }).ToList();
|
|
|
|
var result = new List<DockWaypointDto>(maxPoints);
|
|
result.Add(new DockWaypointDto { X = waypoints[0].X, Y = waypoints[0].Y });
|
|
|
|
double step = (double)(waypoints.Count - 1) / (maxPoints - 1);
|
|
for (int i = 1; i < maxPoints - 1; i++)
|
|
{
|
|
int idx = (int)Math.Round(i * step);
|
|
result.Add(new DockWaypointDto { X = waypoints[idx].X, Y = waypoints[idx].Y });
|
|
}
|
|
|
|
var last = waypoints[^1];
|
|
result.Add(new DockWaypointDto { X = last.X, Y = last.Y });
|
|
return result;
|
|
}
|
|
|
|
private static List<WaypointDto> DownsampleNavigationWaypoints(IReadOnlyList<NavigationNode> waypoints, int maxPoints)
|
|
{
|
|
if (waypoints.Count <= maxPoints)
|
|
return waypoints.Select(w => new WaypointDto { X = w.X, Y = w.Y, Direction = w.Direction.ToString() }).ToList();
|
|
|
|
var result = new List<WaypointDto>(maxPoints);
|
|
result.Add(new WaypointDto { X = waypoints[0].X, Y = waypoints[0].Y, Direction = waypoints[0].Direction.ToString() });
|
|
|
|
double step = (double)(waypoints.Count - 1) / (maxPoints - 1);
|
|
for (int i = 1; i < maxPoints - 1; i++)
|
|
{
|
|
int idx = (int)Math.Round(i * step);
|
|
result.Add(new WaypointDto { X = waypoints[idx].X, Y = waypoints[idx].Y, Direction = waypoints[idx].Direction.ToString() });
|
|
}
|
|
|
|
var last = waypoints[^1];
|
|
result.Add(new WaypointDto { X = last.X, Y = last.Y, Direction = last.Direction.ToString() });
|
|
return result;
|
|
}
|
|
|
|
private void BroadcastStateAsync()
|
|
{
|
|
var state = GetState();
|
|
_ = _hubContext.Clients.Group("monitor").SendAsync("ReceiveMonitorState", state);
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
StopTimer();
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|