Initial commit
This commit is contained in:
@@ -0,0 +1,428 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user