Initial commit

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Scenarios/StraightLineScenario.cs

@@ -0,0 +1,102 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Scenarios;
+
+/// <summary>
+/// Straight line test scenario
+/// </summary>
+public class StraightLineScenario : TestScenario
+{
+    public double Length { get; set; } = 10.0; // meters
+    public double StartX { get; set; } = 0.0;
+    public double StartY { get; set; } = 0.0;
+    public double StartTheta { get; set; } = 0.0; // radians
+    public double Resolution { get; set; } = 0.05; // meters between points
+
+    public StraightLineScenario()
+    {
+        Name = "Straight Line 10m";
+        Description = "Robot moves in a straight line for 10 meters";
+        Type = TrajectoryType.StraightLine;
+    }
+
+    public override List<PathPoint> GenerateReferencePath()
+    {
+        var points = new List<PathPoint>();
+        double absLength = Math.Abs(Length);
+        bool isBackward = Length < 0;
+        
+        // For backward movement, reverse the direction
+        double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
+        
+        // Normalize directionTheta to [-π, π]
+        while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
+        while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
+
+        var direction = isBackward ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+
+        // Start point
+        points.Add(new PathPoint
+        {
+            X = StartX,
+            Y = StartY,
+            Direction = direction,
+            DistanceFromStart = 0.0
+        });
+
+        // Generate intermediate points
+        int numPoints = (int)(absLength / Resolution);
+        for (int i = 1; i <= numPoints; i++)
+        {
+            double distance = i * Resolution;
+            if (distance > absLength) distance = absLength;
+
+            points.Add(new PathPoint
+            {
+                X = StartX + distance * Math.Cos(directionTheta),
+                Y = StartY + distance * Math.Sin(directionTheta),
+                Direction = direction,
+                DistanceFromStart = distance
+            });
+        }
+
+        // Ensure end point is exactly at absLength
+        if (points[^1].DistanceFromStart < absLength)
+        {
+            points.Add(new PathPoint
+            {
+                X = StartX + absLength * Math.Cos(directionTheta),
+                Y = StartY + absLength * Math.Sin(directionTheta),
+                Direction = direction,
+                DistanceFromStart = absLength
+            });
+        }
+
+        return points;
+    }
+
+    public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
+    {
+        var goal = GetGoalPose();
+        double distance = Pose2D.Distance(currentPose, goal);
+        if (distance <= tolerance) Console.WriteLine($"Robot is goal reached. Current pose: [{currentPose.X} - {currentPose.Y}], Goal: [{goal.X} - {goal.Y}], Distance: {distance}");
+        return distance <= tolerance;
+    }
+
+    public override Pose2D GetGoalPose()
+    {
+        double absLength = Math.Abs(Length);
+        bool isBackward = Length < 0;
+        double directionTheta = isBackward ? StartTheta + Math.PI : StartTheta;
+        
+        // Normalize directionTheta to [-π, π]
+        while (directionTheta > Math.PI) directionTheta -= 2 * Math.PI;
+        while (directionTheta < -Math.PI) directionTheta += 2 * Math.PI;
+        
+        return new Pose2D(
+            StartX + absLength * Math.Cos(directionTheta),
+            StartY + absLength * Math.Sin(directionTheta),
+            directionTheta
+        );
+    }
+}