Initial commit

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

@@ -0,0 +1,182 @@
+using RobotNet10.Common;
+using RobotNet10.Common.Models;
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Scenarios;
+
+/// <summary>
+/// Custom path scenario with user-defined edges
+/// </summary>
+public class CustomPathScenario : TestScenario
+{
+    /// <summary>
+    /// List of edges defining the path
+    /// </summary>
+    public List<PathEdge> Edges { get; set; } = [];
+    
+    /// <summary>
+    /// Resolution for splitting edges into points (meters)
+    /// </summary>
+    public double Resolution { get; set; } = 0.05; // meters between points
+
+    public CustomPathScenario()
+    {
+        Name = "Custom Path";
+        Description = "User-defined path with custom edges";
+        Type = TrajectoryType.Custom;
+    }
+
+    public override List<PathPoint> GenerateReferencePath()
+    {
+        var points = new List<PathPoint>();
+        
+        if (Edges.Count == 0)
+            return points;
+
+        // Process each edge
+        foreach (var edge in Edges)
+        {
+            var edgePoints = SplitEdge(edge, Resolution);
+
+            if (edgePoints.Count == 0)
+                continue;
+
+            double cumulativeDistance;
+            // Adjust cumulative distance for first point
+            if (points.Count > 0)
+            {
+                // Calculate distance from last point to first point of this edge
+                double dx = edgePoints[0].X - points[^1].X;
+                double dy = edgePoints[0].Y - points[^1].Y;
+                double connectionDistance = Math.Sqrt(dx * dx + dy * dy);
+                cumulativeDistance = points[^1].DistanceFromStart + connectionDistance;
+            }
+            else
+            {
+                cumulativeDistance = 0.0;
+            }
+
+            // Add points from this edge
+            for (int i = 0; i < edgePoints.Count; i++)
+            {
+                if (i == 0 && points.Count > 0)
+                {
+                    // Skip first point if it's the same as last point (edge connection)
+                    double dx = edgePoints[i].X - points[^1].X;
+                    double dy = edgePoints[i].Y - points[^1].Y;
+                    if (Math.Sqrt(dx * dx + dy * dy) < 0.001)
+                        continue;
+                }
+
+                if (i > 0)
+                {
+                    // Calculate distance from previous point
+                    double dx = edgePoints[i].X - edgePoints[i - 1].X;
+                    double dy = edgePoints[i].Y - edgePoints[i - 1].Y;
+                    double segmentDistance = Math.Sqrt(dx * dx + dy * dy);
+                    cumulativeDistance += segmentDistance;
+                }
+
+                points.Add(new PathPoint
+                {
+                    X = edgePoints[i].X,
+                    Y = edgePoints[i].Y,
+                    Direction = edge.Direction,
+                    DistanceFromStart = cumulativeDistance
+                });
+            }
+        }
+
+        return points;
+    }
+
+    /// <summary>
+    /// Split an edge into points based on resolution
+    /// </summary>
+    private static List<(double X, double Y)> SplitEdge(PathEdge edge, double resolution)
+    {
+        var points = new List<(double X, double Y)>();
+
+        // Calculate edge length
+        SpaceEdge spaceEdge = new()
+        {
+            StartX = edge.StartX,
+            StartY = edge.StartY,
+            EndX = edge.EndX,
+            EndY = edge.EndY,
+            Degree = edge.Degree,
+            ControlPoint1X = edge.ControlPoint1X ?? 0,
+            ControlPoint1Y = edge.ControlPoint1Y ?? 0,
+            ControlPoint2X = edge.ControlPoint2X ?? 0,
+            ControlPoint2Y = edge.ControlPoint2Y ?? 0,
+        };
+        double edgeLength = SpaceCompute.GetEdgeLength(spaceEdge, 0.1);
+        
+        if (edgeLength <= 0)
+        {
+            points.Add((edge.StartX, edge.StartY));
+            return points;
+        }
+
+        // Calculate number of points based on resolution
+        int numPoints = Math.Max(1, (int)(edgeLength / resolution));
+        
+        for (int i = 0; i <= numPoints; i++)
+        {
+            double t = numPoints > 0 ? i * 1.0 / numPoints : 0.0;
+            var point = SpaceCompute.BezierPoint(t, spaceEdge);
+            
+            points.Add((point.X, point.Y));
+        }
+
+        return points;
+    }
+
+
+
+    public override bool IsGoalReached(Pose2D currentPose, double tolerance = 0.05f)
+    {
+        if (Edges.Count == 0)
+            return false;
+
+        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()
+    {
+        if (Edges.Count == 0)
+            return new Pose2D(0, 0, 0);
+
+        var lastEdge = Edges[^1];
+        // Calculate theta from direction (FORWARD = 0, BACKWARD = PI)
+        double goalTheta = 0;
+        if (Edges.Count > 0)
+        {
+            // Calculate direction from last edge
+            if (Edges.Count > 1)
+            {
+                var prevEdge = Edges[^2];
+                double dx = lastEdge.EndX - prevEdge.EndX;
+                double dy = lastEdge.EndY - prevEdge.EndY;
+                goalTheta = Math.Atan2(dy, dx);
+            }
+            else
+            {
+                double dx = lastEdge.EndX - lastEdge.StartX;
+                double dy = lastEdge.EndY - lastEdge.StartY;
+                goalTheta = Math.Atan2(dy, dx);
+            }
+        }
+        return new Pose2D(lastEdge.EndX, lastEdge.EndY, goalTheta);
+    }
+
+    private static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2 * Math.PI;
+        while (angle < -Math.PI) angle += 2 * Math.PI;
+        return angle;
+    }
+}