Initial commit

2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
--- a/srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Helpers/TestHelpers.cs
+++ b/srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Helpers/TestHelpers.cs
@@ -0,0 +1,145 @@
+using RobotNet10.NavigationTune.Shared.Models;
+using RobotNet10.NavigationTune.Navigation.Core;
+using NavScenarios = RobotNet10.NavigationTune.Scenarios;
+
+namespace RobotNet10.NavigationTune.Test.Helpers;
+
+/// <summary>
+/// Helper methods for creating test data
+/// </summary>
+public static class TestHelpers
+{
+    /// <summary>
+    /// Create a default NavigationParameterSet for testing
+    /// </summary>
+    public static NavigationParameterSet CreateDefaultParameterSet()
+    {
+        return new NavigationParameterSet
+        {
+            Name = "Test Parameters",
+            Description = "Test parameter set",
+            MovePidConfig = new PIDConfig { Kp = 1.0, Ki = 0.0001, Kd = 0.6 },
+            RotatePidConfig = new PIDConfig { Kp = 10.0, Ki = 0.01, Kd = 0.1 },
+            PurePursuitConfig = new PurePursuitConfig(),
+            EstimatorConfig = new VelocityEstimatorConfig(),
+            SignalConfig = new VelocitySignalProcessingConfig(),
+            MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
+            NavigationConfig = new NavigationConfig()
+        };
+    }
+
+    /// <summary>
+    /// Create a custom NavigationParameterSet with specified values
+    /// </summary>
+    public static NavigationParameterSet CreateCustomParameterSet(
+        double moveKp = 1.0, double moveKi = 0.0001, double moveKd = 0.6,
+        double rotateKp = 10.0, double rotateKi = 0.01, double rotateKd = 0.1,
+        double lookaheadMin = 0.3, double lookaheadMax = 2.0)
+    {
+        return new NavigationParameterSet
+        {
+            Name = "Custom Parameters",
+            MovePidConfig = new PIDConfig { Kp = moveKp, Ki = moveKi, Kd = moveKd },
+            RotatePidConfig = new PIDConfig { Kp = rotateKp, Ki = rotateKi, Kd = rotateKd },
+            PurePursuitConfig = new PurePursuitConfig
+            {
+                LookaheadMin = lookaheadMin,
+                LookaheadMax = lookaheadMax
+            },
+            EstimatorConfig = new VelocityEstimatorConfig(),
+            SignalConfig = new VelocitySignalProcessingConfig(),
+            MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
+            NavigationConfig = new NavigationConfig()
+        };
+    }
+
+    /// <summary>
+    /// Create a straight line scenario for testing
+    /// </summary>
+    public static NavScenarios.StraightLineScenario CreateStraightLineScenario(double length = 10.0)
+    {
+        return new NavScenarios.StraightLineScenario
+        {
+            Id = Guid.NewGuid(),
+            Name = "Test Straight Line",
+            Description = "Test scenario",
+            Length = length
+        };
+    }
+
+    /// <summary>
+    /// Create a circle scenario for testing
+    /// </summary>
+    public static NavScenarios.CircleScenario CreateCircleScenario(double radius = 2.0)
+    {
+        return new NavScenarios.CircleScenario
+        {
+            Id = Guid.NewGuid(),
+            Name = "Test Circle",
+            Description = "Test scenario",
+            Radius = radius
+        };
+    }
+
+    /// <summary>
+    /// Create a simple reference path for testing
+    /// </summary>
+    public static List<PathPoint> CreateSimplePath(int pointCount = 10, double length = 10.0)
+    {
+        var path = new List<PathPoint>();
+        for (int i = 0; i < pointCount; i++)
+        {
+            var distance = (length / (pointCount - 1)) * i;
+            path.Add(new PathPoint
+            {
+                X = distance,
+                Y = 0.0,
+                DistanceFromStart = distance
+            });
+        }
+        return path;
+    }
+
+    /// <summary>
+    /// Create telemetry data for testing
+    /// </summary>
+    public static TelemetryData CreateTelemetryData(
+        double x = 0.0, double y = 0.0, double theta = 0.0,
+        double linearVel = 0.0, double angularVel = 0.0,
+        double cte = 0.0, double headingError = 0.0)
+    {
+        return new TelemetryData
+        {
+            TimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(),
+            RobotPose = new Pose2D(x, y, theta),
+            RobotTwist = new Twist2D(linearVel, angularVel),
+            ReferencePose = new Pose2D(x, y, theta),
+            CrossTrackError = cte,
+            HeadingError = headingError,
+            LookaheadDistance = 1.0,
+            ModelConfidence = 1.0,
+            DistanceToGoal = 0.0
+        };
+    }
+
+    /// <summary>
+    /// Create a list of telemetry data points for testing
+    /// </summary>
+    public static List<TelemetryData> CreateTelemetryHistory(int count = 100)
+    {
+        var history = new List<TelemetryData>();
+        for (int i = 0; i < count; i++)
+        {
+            history.Add(CreateTelemetryData(
+                x: i * 0.1,
+                y: Math.Sin(i * 0.1) * 0.1, // Small sinusoidal deviation
+                theta: 0.0,
+                linearVel: 1.0,
+                angularVel: 0.0,
+                cte: (Math.Abs(Math.Sin(i * 0.1)) * 0.1),
+                headingError: 0.0
+            ));
+        }
+        return history;
+    }
+}