92 lines
3.9 KiB
C#
92 lines
3.9 KiB
C#
using RobotNet10.RobotApp.Shared.NavigationMonitor;
|
|
|
|
namespace RobotNet10.RobotApp.Services.NavigationMonitor;
|
|
|
|
/// <summary>
|
|
/// Stateless safety checker: evaluates telemetry against configurable thresholds.
|
|
/// </summary>
|
|
public static class NavigationSafetyChecker
|
|
{
|
|
public static List<NavigationSafetyViolationDto> Check(
|
|
NavigationTelemetryDto telemetry,
|
|
NavigationSafetyConfigDto config)
|
|
{
|
|
var violations = new List<NavigationSafetyViolationDto>();
|
|
var now = telemetry.TimestampMs;
|
|
|
|
// 1. Linear velocity
|
|
if (Math.Abs(telemetry.LinearVelocity) > config.MaxLinearVelocity)
|
|
{
|
|
violations.Add(new NavigationSafetyViolationDto
|
|
{
|
|
TimestampMs = now,
|
|
Type = SafetyViolationType.LinearVelocity,
|
|
Severity = SafetyViolationSeverity.Warning,
|
|
Value = Math.Abs(telemetry.LinearVelocity),
|
|
Threshold = config.MaxLinearVelocity,
|
|
Message = $"Linear velocity {Math.Abs(telemetry.LinearVelocity):F2} m/s exceeds limit {config.MaxLinearVelocity:F2} m/s"
|
|
});
|
|
}
|
|
|
|
// 2. Angular velocity
|
|
if (Math.Abs(telemetry.AngularVelocity) > config.MaxAngularVelocity)
|
|
{
|
|
violations.Add(new NavigationSafetyViolationDto
|
|
{
|
|
TimestampMs = now,
|
|
Type = SafetyViolationType.AngularVelocity,
|
|
Severity = SafetyViolationSeverity.Warning,
|
|
Value = Math.Abs(telemetry.AngularVelocity),
|
|
Threshold = config.MaxAngularVelocity,
|
|
Message = $"Angular velocity {Math.Abs(telemetry.AngularVelocity):F2} rad/s exceeds limit {config.MaxAngularVelocity:F2} rad/s"
|
|
});
|
|
}
|
|
|
|
// 3. Linear acceleration
|
|
if (Math.Abs(telemetry.LinearAcceleration) > config.MaxLinearAcceleration)
|
|
{
|
|
violations.Add(new NavigationSafetyViolationDto
|
|
{
|
|
TimestampMs = now,
|
|
Type = SafetyViolationType.LinearAcceleration,
|
|
Severity = SafetyViolationSeverity.Warning,
|
|
Value = Math.Abs(telemetry.LinearAcceleration),
|
|
Threshold = config.MaxLinearAcceleration,
|
|
Message = $"Linear acceleration {Math.Abs(telemetry.LinearAcceleration):F2} m/s² exceeds limit {config.MaxLinearAcceleration:F2} m/s²"
|
|
});
|
|
}
|
|
|
|
// 4. Cross-track error (only when navigating and path is available)
|
|
double maxCteRadians = config.MaxCrossTrackError;
|
|
if (telemetry.Driving && telemetry.CrossTrackError > maxCteRadians)
|
|
{
|
|
violations.Add(new NavigationSafetyViolationDto
|
|
{
|
|
TimestampMs = now,
|
|
Type = SafetyViolationType.CrossTrackError,
|
|
Severity = SafetyViolationSeverity.Critical,
|
|
Value = telemetry.CrossTrackError,
|
|
Threshold = config.MaxCrossTrackError,
|
|
Message = $"CTE {telemetry.CrossTrackError:F3} m exceeds limit {config.MaxCrossTrackError:F3} m"
|
|
});
|
|
}
|
|
|
|
// 5. Heading error (only when navigating and path is available)
|
|
double maxHeadingRadians = config.MaxHeadingError * Math.PI / 180.0;
|
|
if (telemetry.Driving && Math.Abs(telemetry.HeadingError) > maxHeadingRadians)
|
|
{
|
|
violations.Add(new NavigationSafetyViolationDto
|
|
{
|
|
TimestampMs = now,
|
|
Type = SafetyViolationType.HeadingError,
|
|
Severity = SafetyViolationSeverity.Critical,
|
|
Value = Math.Abs(telemetry.HeadingError) * 180.0 / Math.PI,
|
|
Threshold = config.MaxHeadingError,
|
|
Message = $"Heading error {Math.Abs(telemetry.HeadingError) * 180.0 / Math.PI:F1}° exceeds limit {config.MaxHeadingError:F1}°"
|
|
});
|
|
}
|
|
|
|
return violations;
|
|
}
|
|
}
|