110 lines
3.2 KiB
C#
110 lines
3.2 KiB
C#
using FluentAssertions;
|
|
using Xunit;
|
|
using RobotNet10.NavigationTune.Shared.Models;
|
|
using RobotNet10.NavigationTune.Scenarios;
|
|
using RobotNet10.NavigationTune.Test.Helpers;
|
|
|
|
namespace RobotNet10.NavigationTune.Test.Scenarios;
|
|
|
|
public class StraightLineScenarioTests
|
|
{
|
|
[Fact]
|
|
public void GenerateReferencePath_ShouldGenerateStraightPath()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateStraightLineScenario(10.0);
|
|
|
|
// Act
|
|
var path = scenario.GenerateReferencePath();
|
|
|
|
// Assert
|
|
path.Should().NotBeNull();
|
|
path.Count.Should().BeGreaterThan(0);
|
|
|
|
// First point should be at origin
|
|
path[0].X.Should().BeApproximately(0.0, 0.01);
|
|
path[0].Y.Should().BeApproximately(0.0, 0.01);
|
|
|
|
// Last point should be at length
|
|
path[^1].X.Should().BeApproximately(10.0, 0.01);
|
|
path[^1].Y.Should().BeApproximately(0.0, 0.01);
|
|
}
|
|
|
|
[Fact]
|
|
public void GenerateReferencePath_ShouldHaveCorrectDirection()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateStraightLineScenario(10.0);
|
|
|
|
// Act
|
|
var path = scenario.GenerateReferencePath();
|
|
|
|
// Assert
|
|
// All points should have FORWARD direction (straight line)
|
|
foreach (var point in path)
|
|
{
|
|
point.Direction.Should().Be(RobotDirection.FORWARD);
|
|
}
|
|
}
|
|
|
|
[Fact]
|
|
public void IsGoalReached_WithPoseAtGoal_ShouldReturnTrue()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateStraightLineScenario(10.0);
|
|
var goalPose = scenario.GetGoalPose();
|
|
var poseAtGoal = new Pose2D(goalPose.X, goalPose.Y, goalPose.Theta);
|
|
|
|
// Act
|
|
var result = scenario.IsGoalReached(poseAtGoal);
|
|
|
|
// Assert
|
|
result.Should().BeTrue();
|
|
}
|
|
|
|
[Fact]
|
|
public void IsGoalReached_WithPoseFarFromGoal_ShouldReturnFalse()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateStraightLineScenario(10.0);
|
|
var poseFarAway = new Pose2D(0.0, 0.0, 0.0);
|
|
|
|
// Act
|
|
var result = scenario.IsGoalReached(poseFarAway);
|
|
|
|
// Assert
|
|
result.Should().BeFalse();
|
|
}
|
|
|
|
[Fact]
|
|
public void GetGoalPose_ShouldReturnCorrectGoal()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateStraightLineScenario(10.0);
|
|
|
|
// Act
|
|
var goalPose = scenario.GetGoalPose();
|
|
|
|
// Assert
|
|
goalPose.X.Should().BeApproximately(10.0, 0.01);
|
|
goalPose.Y.Should().BeApproximately(0.0, 0.01);
|
|
goalPose.Theta.Should().BeApproximately(0.0, 0.01);
|
|
}
|
|
|
|
[Fact]
|
|
public void GenerateReferencePath_WithDifferentLengths_ShouldGenerateCorrectPaths()
|
|
{
|
|
// Arrange
|
|
var scenario1 = TestHelpers.CreateStraightLineScenario(5.0);
|
|
var scenario2 = TestHelpers.CreateStraightLineScenario(20.0);
|
|
|
|
// Act
|
|
var path1 = scenario1.GenerateReferencePath();
|
|
var path2 = scenario2.GenerateReferencePath();
|
|
|
|
// Assert
|
|
path1[^1].X.Should().BeApproximately(5.0, 0.01);
|
|
path2[^1].X.Should().BeApproximately(20.0, 0.01);
|
|
}
|
|
}
|