Initial commit

2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/NavigationMonitor/NavigationSafetyChecker.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/NavigationMonitor/NavigationSafetyChecker.cs
@@ -0,0 +1,91 @@
+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;
+    }
+}