115 lines
3.5 KiB
C#
115 lines
3.5 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 CircleScenarioTests
|
|
{
|
|
[Fact]
|
|
public void GenerateReferencePath_ShouldGenerateCircularPath()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateCircleScenario(2.0);
|
|
|
|
// Act
|
|
var path = scenario.GenerateReferencePath();
|
|
|
|
// Assert
|
|
path.Should().NotBeNull();
|
|
path.Count.Should().BeGreaterThan(0);
|
|
|
|
// First and last points should be close (closed circle)
|
|
var firstPoint = path[0];
|
|
var lastPoint = path[^1];
|
|
var distance = Math.Sqrt(Math.Pow(lastPoint.X - firstPoint.X, 2) +
|
|
Math.Pow(lastPoint.Y - firstPoint.Y, 2));
|
|
distance.Should().BeLessThan(0.5); // Should be close to starting point
|
|
}
|
|
|
|
[Fact]
|
|
public void GenerateReferencePath_ShouldHaveCorrectRadius()
|
|
{
|
|
// Arrange
|
|
const double radius = 2.0;
|
|
var scenario = TestHelpers.CreateCircleScenario(radius);
|
|
|
|
// Act
|
|
var path = scenario.GenerateReferencePath();
|
|
|
|
// Assert
|
|
// Check that points are approximately at the correct radius
|
|
foreach (var point in path)
|
|
{
|
|
var distanceFromCenter = Math.Sqrt(point.X * point.X + point.Y * point.Y);
|
|
distanceFromCenter.Should().BeApproximately(radius, 0.1);
|
|
}
|
|
}
|
|
|
|
[Fact]
|
|
public void IsGoalReached_WithPoseAtGoal_ShouldReturnTrue()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateCircleScenario(2.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.CreateCircleScenario(2.0);
|
|
var poseFarAway = new Pose2D(10.0, 10.0, 0.0);
|
|
|
|
// Act
|
|
var result = scenario.IsGoalReached(poseFarAway);
|
|
|
|
// Assert
|
|
result.Should().BeFalse();
|
|
}
|
|
|
|
[Fact]
|
|
public void GetGoalPose_ShouldReturnStartingPoint()
|
|
{
|
|
// Arrange
|
|
var scenario = TestHelpers.CreateCircleScenario(2.0);
|
|
|
|
// Act
|
|
var goalPose = scenario.GetGoalPose();
|
|
|
|
// Assert
|
|
// Goal should be at starting point (circle is closed)
|
|
goalPose.X.Should().BeApproximately(2.0, 0.1);
|
|
goalPose.Y.Should().BeApproximately(0.0, 0.1);
|
|
}
|
|
|
|
[Fact]
|
|
public void GenerateReferencePath_WithDifferentRadii_ShouldGenerateCorrectPaths()
|
|
{
|
|
// Arrange
|
|
var scenario1 = TestHelpers.CreateCircleScenario(1.0);
|
|
var scenario2 = TestHelpers.CreateCircleScenario(5.0);
|
|
|
|
// Act
|
|
var path1 = scenario1.GenerateReferencePath();
|
|
var path2 = scenario2.GenerateReferencePath();
|
|
|
|
// Assert
|
|
// Check first point radius
|
|
var radius1 = Math.Sqrt(path1[0].X * path1[0].X + path1[0].Y * path1[0].Y);
|
|
var radius2 = Math.Sqrt(path2[0].X * path2[0].X + path2[0].Y * path2[0].Y);
|
|
|
|
radius1.Should().BeApproximately(1.0, 0.1);
|
|
radius2.Should().BeApproximately(5.0, 0.1);
|
|
}
|
|
}
|