Initial commit

srcs/RobotNet10/Tests/RobotNet10.GlobalPathPlanner.Test/DifferentialPlannerTests.cs

@@ -0,0 +1,319 @@
+using RobotNet10.GlobalPathPlanner.Differential;
+using RobotNet10.GlobalPathPlanner.Model;
+
+namespace RobotNet10.GlobalPathPlanner.UnitTest;
+
+public class DifferentialPlannerTests
+{
+    private readonly DifferentialPlanner _planner;
+    private readonly (GlobalNode[] Nodes, GlobalEdge[] Edges) _simpleGraph;
+
+    public DifferentialPlannerTests()
+    {
+        _planner = new DifferentialPlanner();
+        _simpleGraph = TestHelpers.CreateSimpleLinearGraph();
+    }
+
+    [Fact]
+    public void SetData_ShouldStoreNodesAndEdges()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+
+        // Act
+        _planner.SetData(nodes, edges);
+
+        // Assert - No exception should be thrown
+        Assert.True(true);
+    }
+
+    [Fact]
+    public void SetData_WithEmptyArrays_ShouldNotThrow()
+    {
+        // Arrange
+        var nodes = Array.Empty<GlobalNode>();
+        var edges = Array.Empty<GlobalEdge>();
+
+        // Act & Assert
+        _planner.SetData(nodes, edges);
+        Assert.True(true);
+    }
+
+    [Fact]
+    public void SetOptions_ShouldUpdateOptions()
+    {
+        // Arrange
+        var options = new PathPlannerOptions
+        {
+            LimitDistanceToEdge = 2.0,
+            LimitDistanceToNode = 0.5,
+            ResolutionSplit = 0.2,
+            TimeOut = TimeSpan.FromSeconds(5),
+            ChangeOrientationAngle = 90.0
+        };
+
+        // Act
+        _planner.SetOptions(options);
+
+        // Assert - No exception should be thrown
+        Assert.True(true);
+    }
+
+    [Fact]
+    public void PathPlanning_WithValidGraph_ShouldReturnPath()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id; // Node3
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanning(startX, startY, startTheta, goalId);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.NotNull(pathEdges);
+        Assert.NotEmpty(pathNodes);
+        Assert.True(pathNodes.Length >= 2); // At least start and goal
+        Assert.Equal(goalId, pathNodes[^1].Id); // Last node should be goal
+    }
+
+    [Fact]
+    public void PathPlanning_WithInvalidGoalId_ShouldThrowException()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var invalidGoalId = Guid.NewGuid();
+
+        // Act & Assert
+        var exception = Assert.Throws<Exception>(() =>
+            _planner.PathPlanning(0.0, 0.0, 0.0, invalidGoalId));
+
+        Assert.Contains("does not exist", exception.Message);
+    }
+
+    [Fact]
+    public void PathPlanning_WithDisconnectedGraph_ShouldThrowException()
+    {
+        // Arrange
+        var (nodes, edges) = TestHelpers.CreateDisconnectedGraph();
+        _planner.SetData(nodes, edges);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id; // Node3 is disconnected
+
+        // Act & Assert
+        var exception = Assert.Throws<Exception>(() =>
+            _planner.PathPlanning(startX, startY, startTheta, goalId));
+
+        Assert.Contains("does not exist", exception.Message);
+    }
+
+    [Fact]
+    public void PathPlanning_FromNodeId_ShouldReturnPath()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startNodeId = nodes[0].Id; // Node1
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id; // Node3
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanning(startNodeId, startTheta, goalId);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.NotNull(pathEdges);
+        Assert.NotEmpty(pathNodes);
+        Assert.Equal(startNodeId, pathNodes[0].Id); // First node should be start
+        Assert.Equal(goalId, pathNodes[^1].Id); // Last node should be goal
+    }
+
+    [Fact]
+    public void PathPlanning_FromInvalidNodeId_ShouldThrowException()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var invalidStartNodeId = Guid.NewGuid();
+        var goalId = nodes[2].Id;
+
+        // Act & Assert
+        var exception = Assert.Throws<Exception>(() =>
+            _planner.PathPlanning(invalidStartNodeId, 0.0, goalId));
+
+        Assert.Contains("does not exist", exception.Message);
+    }
+
+    [Fact]
+    public void PathPlanningWithStartDirection_WithForwardDirection_ShouldReturnPath()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id;
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanningWithStartDirection(
+            startX, startY, startTheta, goalId, Orientation.FORWARD);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.NotEmpty(pathNodes);
+    }
+
+    [Fact]
+    public void PathPlanningWithStartDirection_WithBackwardDirection_ShouldReturnPath()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id;
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanningWithStartDirection(
+            startX, startY, startTheta, goalId, Orientation.BACKWARD);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.NotEmpty(pathNodes);
+    }
+
+    [Fact]
+    public void PathPlanningWithFinalDirection_WithForwardDirection_ShouldReturnPath()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id;
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanningWithFinalDirection(
+            startX, startY, startTheta, goalId, Orientation.FORWARD);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.NotEmpty(pathNodes);
+    }
+
+    [Fact]
+    public void PathPlanningWithAngle_WithValidAngle_ShouldReturnPath()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[2].Id;
+        var goalAngle = 90.0;
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanningWithAngle(
+            startX, startY, startTheta, goalId, goalAngle);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.NotEmpty(pathNodes);
+    }
+
+    [Fact]
+    public void PathPlanning_WithTimeout_ShouldThrowTimeoutException()
+    {
+        // Arrange
+        var (nodes, edges) = TestHelpers.CreateSquareGraph();
+        _planner.SetData(nodes, edges);
+
+        var options = new PathPlannerOptions
+        {
+            LimitDistanceToEdge = 1.0,
+            LimitDistanceToNode = 0.3,
+            ResolutionSplit = 0.1,
+            TimeOut = TimeSpan.FromMilliseconds(1), // Very short timeout
+            ChangeOrientationAngle = 89.0
+        };
+        _planner.SetOptions(options);
+
+        var startX = 0.0;
+        var startY = 0.0;
+        var startTheta = 0.0;
+        var goalId = nodes[3].Id;
+
+        // Act & Assert
+        // Note: This test might not always timeout depending on system performance
+        // It's included as an example of timeout handling
+        try
+        {
+            _planner.PathPlanning(startX, startY, startTheta, goalId);
+        }
+        catch (TimeoutException)
+        {
+            // Expected behavior
+            Assert.True(true);
+        }
+        catch (Exception)
+        {
+            // May not timeout on fast systems
+            Assert.True(true);
+        }
+    }
+
+    [Fact]
+    public void PathPlanning_WithCancellation_ShouldThrowOperationCanceledException()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        using var cts = new CancellationTokenSource();
+        cts.Cancel(); // Cancel immediately
+
+        // Act & Assert
+        Assert.Throws<OperationCanceledException>(() =>
+            _planner.PathPlanning(0.0, 0.0, 0.0, nodes[2].Id, cts.Token));
+    }
+
+    [Fact]
+    public void PathPlanning_WithSameStartAndGoal_ShouldReturnSingleNode()
+    {
+        // Arrange
+        var (nodes, edges) = _simpleGraph;
+        _planner.SetData(nodes, edges);
+
+        var startNodeId = nodes[0].Id;
+        var goalId = nodes[0].Id; // Same as start
+
+        // Act
+        var (pathNodes, pathEdges) = _planner.PathPlanning(startNodeId, 0.0, goalId);
+
+        // Assert
+        Assert.NotNull(pathNodes);
+        Assert.Single(pathNodes);
+        Assert.Empty(pathEdges);
+        Assert.Equal(goalId, pathNodes[0].Id);
+    }
+}