1036 lines
42 KiB
C#
1036 lines
42 KiB
C#
using FluentAssertions;
|
|
using Xunit;
|
|
using Xunit.Abstractions;
|
|
using RobotNet10.NavigationTune.Shared.Models;
|
|
using RobotNet10.NavigationTune.Services;
|
|
using RobotNet10.NavigationTune.Test.Helpers;
|
|
|
|
namespace RobotNet10.NavigationTune.Test.Services;
|
|
|
|
public class TuningAdvisorTests
|
|
{
|
|
private readonly TuningAdvisor _advisor;
|
|
private readonly ITestOutputHelper _output;
|
|
|
|
public TuningAdvisorTests(ITestOutputHelper output)
|
|
{
|
|
_advisor = new TuningAdvisor();
|
|
_output = output;
|
|
}
|
|
|
|
#region Test Data Helpers
|
|
|
|
/// <summary>
|
|
/// Create telemetry with explicit phase, timestamp, and configurable errors.
|
|
/// </summary>
|
|
private static TelemetryData CreatePhasedTelemetry(
|
|
TelemetryPhase phase,
|
|
long timestampMs,
|
|
double x = 0, double y = 0, double theta = 0,
|
|
double linearVel = 1.0, double angularVel = 0.0,
|
|
double cte = 0.0, double headingError = 0.0,
|
|
double distanceToGoal = 5.0,
|
|
double modelConfidence = 1.0,
|
|
double commandLinearVel = 1.0)
|
|
{
|
|
return new TelemetryData
|
|
{
|
|
TimestampMs = timestampMs,
|
|
Phase = phase,
|
|
RobotPose = new Pose2D(x, y, theta),
|
|
RobotTwist = new Twist2D(linearVel, angularVel),
|
|
CommandTwist = new Twist2D(commandLinearVel, angularVel),
|
|
ReferencePose = new Pose2D(x, 0, theta),
|
|
CrossTrackError = cte,
|
|
HeadingError = headingError,
|
|
LookaheadDistance = 1.0,
|
|
ModelConfidence = modelConfidence,
|
|
DistanceToGoal = distanceToGoal
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create telemetry for PathFollowing phase with good tracking (small CTE, small heading error).
|
|
/// </summary>
|
|
private static List<TelemetryData> CreateGoodPathFollowingTelemetry(int count = 50, long startMs = 0)
|
|
{
|
|
var data = new List<TelemetryData>();
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
data.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
startMs + i * 300, // ~3.3 Hz
|
|
x: i * 0.1, cte: 0.02, headingError: 0.02,
|
|
distanceToGoal: 10.0 - i * 0.1));
|
|
}
|
|
return data;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create telemetry for FinalApproach phase with configurable CTE and heading.
|
|
/// </summary>
|
|
private static List<TelemetryData> CreateFinalApproachTelemetry(
|
|
double cte, double headingErrorRad, int count = 20, long startMs = 15000)
|
|
{
|
|
var data = new List<TelemetryData>();
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
data.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalApproach,
|
|
startMs + i * 300,
|
|
x: 8.0 + i * 0.05, cte: cte, headingError: headingErrorRad,
|
|
linearVel: 0.3, distanceToGoal: 0.5 - i * 0.02));
|
|
}
|
|
return data;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create telemetry for FinalRotation phase.
|
|
/// </summary>
|
|
private static List<TelemetryData> CreateFinalRotationTelemetry(
|
|
double headingErrorRad, int count = 10, long startMs = 21000)
|
|
{
|
|
var data = new List<TelemetryData>();
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
data.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalRotation,
|
|
startMs + i * 300,
|
|
x: 9.0, cte: 0.01, headingError: headingErrorRad * (1.0 - i * 0.08),
|
|
linearVel: 0.0, angularVel: 0.5,
|
|
distanceToGoal: 0.01));
|
|
}
|
|
return data;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create telemetry for InitialRotation phase.
|
|
/// </summary>
|
|
private static List<TelemetryData> CreateInitialRotationTelemetry(
|
|
int count = 10, long startMs = 0)
|
|
{
|
|
var data = new List<TelemetryData>();
|
|
for (int i = 0; i < count; i++)
|
|
{
|
|
data.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.InitialRotation,
|
|
startMs + i * 300,
|
|
x: 0, y: 0, theta: i * 0.1,
|
|
linearVel: 0.0, angularVel: 1.5,
|
|
cte: 0.0, headingError: 0.5 - i * 0.05,
|
|
distanceToGoal: 10.0));
|
|
}
|
|
return data;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create default TestMetrics for test scenarios.
|
|
/// </summary>
|
|
private static TestMetrics CreateDefaultMetrics(
|
|
double cteRms = 0.05, double ctePeak = 0.10, double cteMean = 0.04,
|
|
double headingRms = 0.05, double goalPosError = 0.02, double goalHeadingError = 0.03,
|
|
double velStdDev = 0.05, double accelStdDev = 0.2,
|
|
double pathLengthRatio = 1.05, double completionTime = 10.0,
|
|
double overallScore = 80.0, bool passedCriteria = true)
|
|
{
|
|
return new TestMetrics
|
|
{
|
|
CrossTrackErrorRMS = cteRms,
|
|
CrossTrackErrorPeak = ctePeak,
|
|
CrossTrackErrorMean = cteMean,
|
|
HeadingErrorRMS = headingRms,
|
|
HeadingErrorPeak = headingRms * 1.5,
|
|
GoalPositionError = goalPosError,
|
|
GoalHeadingError = goalHeadingError,
|
|
VelocityStdDev = velStdDev,
|
|
AccelerationStdDev = accelStdDev,
|
|
PathLengthRatio = pathLengthRatio,
|
|
CompletionTime = completionTime,
|
|
AverageSpeed = 1.0,
|
|
MaxSpeed = 1.5,
|
|
OverallScore = overallScore,
|
|
TrackingScore = 80.0,
|
|
SmoothnessScore = 85.0,
|
|
EfficiencyScore = 90.0,
|
|
PassedCriteria = passedCriteria
|
|
};
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Phase Segmentation Tests
|
|
|
|
[Fact]
|
|
public void Analyze_WithPhasedTelemetry_ShouldPopulatePhaseSampleCounts()
|
|
{
|
|
// Arrange
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateInitialRotationTelemetry(10));
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50, startMs: 3000));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.02, 0.02, 20, startMs: 18000));
|
|
telemetry.AddRange(CreateFinalRotationTelemetry(0.03, 10, startMs: 24000));
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert
|
|
report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.InitialRotation);
|
|
report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.PathFollowing);
|
|
report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.FinalApproach);
|
|
report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.FinalRotation);
|
|
report.PhaseSampleCounts[TelemetryPhase.InitialRotation].Should().Be(10);
|
|
report.PhaseSampleCounts[TelemetryPhase.PathFollowing].Should().Be(50);
|
|
report.PhaseSampleCounts[TelemetryPhase.FinalApproach].Should().Be(20);
|
|
report.PhaseSampleCounts[TelemetryPhase.FinalRotation].Should().Be(10);
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_WithPhasedTelemetry_ShouldPopulatePhaseMetricsMap()
|
|
{
|
|
// Arrange
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.05, 0.05, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert
|
|
report.PhaseMetricsMap.Should().ContainKey(TelemetryPhase.PathFollowing);
|
|
report.PhaseMetricsMap.Should().ContainKey(TelemetryPhase.FinalApproach);
|
|
|
|
var pfMetrics = report.PhaseMetricsMap[TelemetryPhase.PathFollowing];
|
|
pfMetrics.SampleCount.Should().Be(50);
|
|
pfMetrics.CrossTrackErrorRMS.Should().BeGreaterThan(0);
|
|
|
|
var faMetrics = report.PhaseMetricsMap[TelemetryPhase.FinalApproach];
|
|
faMetrics.SampleCount.Should().Be(20);
|
|
faMetrics.CrossTrackErrorRMS.Should().BeApproximately(0.05, 0.001);
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_WithLegacyTelemetry_ShouldTreatAsPathFollowing()
|
|
{
|
|
// Arrange — telemetry without Phase set (null)
|
|
var telemetry = TestHelpers.CreateTelemetryHistory(50);
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — all samples should be grouped as PathFollowing
|
|
report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.PathFollowing);
|
|
report.PhaseSampleCounts[TelemetryPhase.PathFollowing].Should().Be(50);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Pattern Detection Phase-Aware Tests
|
|
|
|
[Fact]
|
|
public void Analyze_WithGoodTracking_ShouldNotDetectLargeCTE()
|
|
{
|
|
// Arrange — CTE well below thresholds for both phases
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50)); // CTE = 0.02
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.01, 0.01, 20, startMs: 15000)); // CTE = 0.01
|
|
|
|
var metrics = CreateDefaultMetrics(cteRms: 0.02, ctePeak: 0.03, cteMean: 0.02);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — no LargeCTE patterns
|
|
report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.LargeCTE)
|
|
.Should().BeEmpty();
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_FinalApproach_WithCTEAbove002_ShouldDetectLargeCTE()
|
|
{
|
|
// Arrange — PathFollowing CTE good (0.05 < 0.10), FinalApproach CTE bad (0.04 > 0.02)
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50)); // CTE = 0.02 (good for PF)
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 15000)); // CTE = 0.04 (bad for FA, > 0.02)
|
|
|
|
var metrics = CreateDefaultMetrics(cteRms: 0.03);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — should detect LargeCTE only in FinalApproach
|
|
var faCtePattern = report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.FinalApproach)
|
|
.ToList();
|
|
faCtePattern.Should().HaveCount(1);
|
|
faCtePattern[0].Evidence.Should().ContainKey("Threshold");
|
|
faCtePattern[0].Evidence["Threshold"].Should().BeApproximately(0.02, 0.001);
|
|
|
|
// PathFollowing should NOT have LargeCTE (0.02 < 0.10 threshold)
|
|
report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.PathFollowing)
|
|
.Should().BeEmpty();
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_FinalApproach_WithHeadingAbove2Deg_ShouldDetectLargeHeadingError()
|
|
{
|
|
// Arrange — FinalApproach heading = 5° > 2° threshold
|
|
double headingRad = 5.0 * Math.PI / 180.0;
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.01, headingRad, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — heading error detected in FinalApproach (target: 2°)
|
|
var faHeadingPattern = report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.GoalHeadingError && p.DetectedInPhase == TelemetryPhase.FinalApproach)
|
|
.ToList();
|
|
faHeadingPattern.Should().HaveCount(1);
|
|
faHeadingPattern[0].Evidence.Should().ContainKey("ThresholdDeg");
|
|
faHeadingPattern[0].Evidence["ThresholdDeg"].Should().BeApproximately(2.0, 0.1);
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_PathFollowing_WithOscillation_ShouldDetectInCorrectPhase()
|
|
{
|
|
// Arrange — oscillating angular velocity in PathFollowing
|
|
var telemetry = new List<TelemetryData>();
|
|
for (int i = 0; i < 60; i++)
|
|
{
|
|
double angVel = Math.Sin(i * 2.0) * 1.5; // High frequency, high amplitude
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
i * 100, // 10 Hz → 6 seconds
|
|
x: i * 0.05, angularVel: angVel,
|
|
cte: 0.03, distanceToGoal: 5.0));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert
|
|
var oscillation = report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.Oscillation && p.DetectedInPhase == TelemetryPhase.PathFollowing)
|
|
.ToList();
|
|
oscillation.Should().HaveCount(1);
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_InitialRotation_ShouldNotRunCTEDetector()
|
|
{
|
|
// Arrange — InitialRotation with high CTE (expected, since robot is rotating in place)
|
|
var telemetry = new List<TelemetryData>();
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.InitialRotation,
|
|
i * 300,
|
|
cte: 0.5, // Huge CTE — but should be ignored in InitialRotation
|
|
headingError: 0.3,
|
|
linearVel: 0.0, angularVel: 2.0,
|
|
distanceToGoal: 10.0));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — no LargeCTE for InitialRotation
|
|
report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.InitialRotation)
|
|
.Should().BeEmpty();
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_GoalOvershoot_ShouldDetectInFinalApproachOnly()
|
|
{
|
|
// Arrange — FinalApproach data where distance-to-goal decreases then increases (overshoot)
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
|
|
// FinalApproach: distance decreases to 0.01 then increases to 0.08 (overshoot = 0.07)
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
double dist;
|
|
if (i < 12)
|
|
dist = 0.3 - i * 0.024; // Decreasing to ~0.01
|
|
else
|
|
dist = 0.01 + (i - 12) * 0.01; // Increasing after min
|
|
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalApproach,
|
|
15000 + i * 300,
|
|
cte: 0.02, distanceToGoal: dist, linearVel: 0.3));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert
|
|
var overshoot = report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.GoalOvershoot)
|
|
.ToList();
|
|
overshoot.Should().HaveCount(1);
|
|
overshoot[0].DetectedInPhase.Should().Be(TelemetryPhase.FinalApproach);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Suggestion Generation Phase-Aware Tests
|
|
|
|
[Fact]
|
|
public void Analyze_FinalApproach_LargeCTE_PurePursuit_ShouldSuggestPrecisionParams()
|
|
{
|
|
// Arrange — FinalApproach CTE = 0.04 > 0.02 target, PurePursuit controller
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics(cteRms: 0.03);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
parameters.ControllerType = PathFollowingController.PurePursuit;
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — should suggest FinalApproach-specific PP parameters
|
|
var faSuggestions = report.Suggestions
|
|
.Where(s => s.TargetPhase == TelemetryPhase.FinalApproach)
|
|
.ToList();
|
|
|
|
faSuggestions.Should().NotBeEmpty("FinalApproach CTE exceeds 0.02m target");
|
|
|
|
// Should suggest increasing FinalApproachThreshold or GoalRegionDistance
|
|
var ppFinalParams = faSuggestions
|
|
.Where(s => s.ParameterPath.Contains("FinalApproachThreshold") ||
|
|
s.ParameterPath.Contains("GoalRegionDistance"))
|
|
.ToList();
|
|
ppFinalParams.Should().NotBeEmpty("should suggest PP FinalApproach precision params");
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_FinalApproach_LargeCTE_Stanley_ShouldSuggestGoalParams()
|
|
{
|
|
// Arrange — FinalApproach CTE = 0.04 > 0.02 target, Stanley controller
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics(cteRms: 0.03);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
parameters.ControllerType = PathFollowingController.Stanley;
|
|
parameters.StanleyConfig = new StanleyConfig();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — should suggest Stanley FinalApproach parameters
|
|
var faSuggestions = report.Suggestions
|
|
.Where(s => s.TargetPhase == TelemetryPhase.FinalApproach)
|
|
.ToList();
|
|
|
|
faSuggestions.Should().NotBeEmpty("FinalApproach CTE exceeds 0.02m target");
|
|
|
|
// Should suggest GoalGainMultiplier or GoalApproachDistance
|
|
var stanleyGoalParams = faSuggestions
|
|
.Where(s => s.ParameterPath.Contains("GoalGainMultiplier") ||
|
|
s.ParameterPath.Contains("GoalApproachDistance") ||
|
|
s.ParameterPath.Contains("LowSpeedAngularGain"))
|
|
.ToList();
|
|
stanleyGoalParams.Should().NotBeEmpty("should suggest Stanley goal precision params");
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_AllSuggestions_ShouldHaveTargetPhaseSet()
|
|
{
|
|
// Arrange — mixed-phase telemetry with some issues
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.05, 20, startMs: 15000)); // Bad CTE + heading
|
|
|
|
var metrics = CreateDefaultMetrics(goalHeadingError: 0.2); // ~11° heading error
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — every suggestion should have TargetPhase populated
|
|
foreach (var suggestion in report.Suggestions)
|
|
{
|
|
suggestion.TargetPhase.Should().NotBeNull(
|
|
$"suggestion for {suggestion.ParameterPath} should have TargetPhase set");
|
|
}
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_AllPatterns_ShouldHaveDetectedInPhaseSet()
|
|
{
|
|
// Arrange
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.05, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics(goalHeadingError: 0.2, pathLengthRatio: 1.25);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — every pattern should have DetectedInPhase populated
|
|
foreach (var pattern in report.DetectedPatterns)
|
|
{
|
|
pattern.DetectedInPhase.Should().NotBeNull(
|
|
$"pattern {pattern.Category} should have DetectedInPhase set");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Conflict Resolution Phase-Aware Tests
|
|
|
|
[Fact]
|
|
public void Analyze_SameParamDifferentPhases_ShouldKeepBothSuggestions()
|
|
{
|
|
// Arrange — Create telemetry that triggers different suggestions for the same param
|
|
// PathFollowing oscillation (→ decrease K) + FinalApproach large CTE (→ increase K for Stanley)
|
|
var telemetry = new List<TelemetryData>();
|
|
|
|
// PathFollowing with oscillation
|
|
for (int i = 0; i < 60; i++)
|
|
{
|
|
double angVel = Math.Sin(i * 2.0) * 1.5;
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
i * 100,
|
|
x: i * 0.05, angularVel: angVel,
|
|
cte: 0.03, distanceToGoal: 5.0));
|
|
}
|
|
|
|
// FinalApproach with high CTE
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.02, 20, startMs: 6000));
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
parameters.ControllerType = PathFollowingController.Stanley;
|
|
parameters.StanleyConfig = new StanleyConfig();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — suggestions for different phases should coexist even if same param
|
|
// (conflict resolution groups by ParameterPath + TargetPhase)
|
|
var pfSuggestions = report.Suggestions.Where(s => s.TargetPhase == TelemetryPhase.PathFollowing).ToList();
|
|
var faSuggestions = report.Suggestions.Where(s => s.TargetPhase == TelemetryPhase.FinalApproach).ToList();
|
|
|
|
// Both phases should have suggestions
|
|
pfSuggestions.Should().NotBeEmpty("PathFollowing oscillation should generate suggestions");
|
|
faSuggestions.Should().NotBeEmpty("FinalApproach CTE should generate suggestions");
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Assessment Generation Tests
|
|
|
|
[Fact]
|
|
public void Analyze_Assessment_ShouldIncludePerPhaseInfo()
|
|
{
|
|
// Arrange
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.04, 0.05, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — assessment should mention phase names
|
|
report.OverallAssessment.Should().NotBeNullOrEmpty();
|
|
// If there are patterns, the assessment should mention phases
|
|
if (report.DetectedPatterns.Count > 0)
|
|
{
|
|
// Should contain phase-specific info (PathFollowing or FinalApproach)
|
|
bool hasPhaseInfo = report.OverallAssessment.Contains("PathFollowing") ||
|
|
report.OverallAssessment.Contains("FinalApproach");
|
|
hasPhaseInfo.Should().BeTrue("assessment should include per-phase metrics info");
|
|
}
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_WithNoIssues_ShouldReturnExcellentAssessment()
|
|
{
|
|
// Arrange — perfect tracking in all phases
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.01, 0.01, 20, startMs: 15000));
|
|
|
|
var metrics = CreateDefaultMetrics(overallScore: 95, passedCriteria: true);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert
|
|
if (report.DetectedPatterns.Count == 0)
|
|
{
|
|
report.OverallAssessment.Should().Contain("Excellent");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region ApplySuggestion Tests
|
|
|
|
[Fact]
|
|
public void ApplySuggestion_ShouldModifyParameterValue()
|
|
{
|
|
// Arrange
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
var suggestion = new TuningSuggestion
|
|
{
|
|
ParameterPath = "PurePursuitConfig.LookaheadMin",
|
|
CurrentValue = parameters.PurePursuitConfig.LookaheadMin,
|
|
SuggestedValue = 0.5,
|
|
TargetPhase = TelemetryPhase.PathFollowing
|
|
};
|
|
|
|
// Act
|
|
var result = _advisor.ApplySuggestion(parameters, suggestion);
|
|
|
|
// Assert
|
|
result.PurePursuitConfig.LookaheadMin.Should().BeApproximately(0.5, 0.001);
|
|
// Original should be unchanged
|
|
parameters.PurePursuitConfig.LookaheadMin.Should().NotBe(0.5);
|
|
}
|
|
|
|
[Fact]
|
|
public void ApplyAllSuggestions_ShouldApplyMultiple()
|
|
{
|
|
// Arrange
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
var suggestions = new List<TuningSuggestion>
|
|
{
|
|
new()
|
|
{
|
|
ParameterPath = "PurePursuitConfig.LookaheadMin",
|
|
CurrentValue = parameters.PurePursuitConfig.LookaheadMin,
|
|
SuggestedValue = 0.5,
|
|
TargetPhase = TelemetryPhase.PathFollowing
|
|
},
|
|
new()
|
|
{
|
|
ParameterPath = "PurePursuitConfig.FinalApproachThreshold",
|
|
CurrentValue = parameters.PurePursuitConfig.FinalApproachThreshold,
|
|
SuggestedValue = 0.35,
|
|
TargetPhase = TelemetryPhase.FinalApproach
|
|
}
|
|
};
|
|
|
|
// Act
|
|
var result = _advisor.ApplyAllSuggestions(parameters, suggestions);
|
|
|
|
// Assert
|
|
result.PurePursuitConfig.LookaheadMin.Should().BeApproximately(0.5, 0.001);
|
|
result.PurePursuitConfig.FinalApproachThreshold.Should().BeApproximately(0.35, 0.001);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Edge Case Tests
|
|
|
|
[Fact]
|
|
public void Analyze_WithMinimalTelemetry_ShouldNotThrow()
|
|
{
|
|
// Arrange — only a few samples per phase (below min threshold of 5)
|
|
var telemetry = new List<TelemetryData>();
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(TelemetryPhase.PathFollowing, i * 300,
|
|
cte: 0.5, distanceToGoal: 5.0));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var action = () => _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — should not throw, just return empty patterns
|
|
action.Should().NotThrow();
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
report.Should().NotBeNull();
|
|
report.TelemetrySamplesAnalyzed.Should().Be(3);
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_WithOnlyCompletedPhase_ShouldSkipDetection()
|
|
{
|
|
// Arrange — only Completed phase telemetry
|
|
var telemetry = new List<TelemetryData>();
|
|
for (int i = 0; i < 10; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(TelemetryPhase.Completed, i * 300,
|
|
cte: 0.0, distanceToGoal: 0.0));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics();
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — Completed phase is skipped, only global detectors may fire
|
|
report.PhaseSampleCounts.Should().ContainKey(TelemetryPhase.Completed);
|
|
// Phase-specific patterns should be absent (Completed is skipped)
|
|
report.DetectedPatterns
|
|
.Where(p => p.DetectedInPhase == TelemetryPhase.Completed)
|
|
.Should().BeEmpty();
|
|
}
|
|
|
|
[Fact]
|
|
public void Analyze_SuggestionsPriority_ShouldBeSortedHighestFirst()
|
|
{
|
|
// Arrange — create telemetry with multiple issues
|
|
var telemetry = new List<TelemetryData>();
|
|
telemetry.AddRange(CreateGoodPathFollowingTelemetry(50));
|
|
telemetry.AddRange(CreateFinalApproachTelemetry(0.06, 0.1, 20, startMs: 15000)); // Bad CTE and heading
|
|
|
|
var metrics = CreateDefaultMetrics(goalHeadingError: 0.2);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
// Act
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
|
|
// Assert — sorted by priority descending, then confidence descending
|
|
if (report.Suggestions.Count > 1)
|
|
{
|
|
for (int i = 1; i < report.Suggestions.Count; i++)
|
|
{
|
|
var prev = report.Suggestions[i - 1];
|
|
var curr = report.Suggestions[i];
|
|
|
|
if (prev.Priority == curr.Priority)
|
|
prev.Confidence.Should().BeGreaterThanOrEqualTo(curr.Confidence);
|
|
else
|
|
((int)prev.Priority).Should().BeGreaterThanOrEqualTo((int)curr.Priority);
|
|
}
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Demo Output Tests
|
|
|
|
private void PrintReport(TuningReport report, string scenarioName)
|
|
{
|
|
_output.WriteLine($"{"",0}");
|
|
_output.WriteLine($"{"═════════════════════════════════════════════════════════════",0}");
|
|
_output.WriteLine($" TUNING REPORT: {scenarioName}");
|
|
_output.WriteLine($"{"═════════════════════════════════════════════════════════════",0}");
|
|
_output.WriteLine($" Controller: {report.ControllerType}");
|
|
_output.WriteLine($" Samples Analyzed: {report.TelemetrySamplesAnalyzed}");
|
|
_output.WriteLine($" Has Critical Issues: {report.HasCriticalIssues}");
|
|
_output.WriteLine($" High Priority Count: {report.HighPriorityCount}");
|
|
|
|
// Phase Sample Counts
|
|
_output.WriteLine($"\n ── Phase Sample Counts ──");
|
|
foreach (var kv in report.PhaseSampleCounts)
|
|
_output.WriteLine($" {kv.Key,-20} : {kv.Value} samples");
|
|
|
|
// Phase Metrics
|
|
_output.WriteLine($"\n ── Phase Metrics ──");
|
|
foreach (var kv in report.PhaseMetricsMap)
|
|
{
|
|
var m = kv.Value;
|
|
_output.WriteLine($" [{kv.Key}] ({m.SampleCount} samples, {m.DurationMs:F0}ms)");
|
|
_output.WriteLine($" CTE RMS={m.CrossTrackErrorRMS:F4}m Peak={m.CrossTrackErrorPeak:F4}m Mean={m.CrossTrackErrorMean:F4}m");
|
|
_output.WriteLine($" Heading RMS={m.HeadingErrorRMS * 180 / Math.PI:F2}° Peak={m.HeadingErrorPeak * 180 / Math.PI:F2}°");
|
|
_output.WriteLine($" AngVel StdDev={m.AngularVelocityStdDev:F4} VelStdDev={m.VelocityStdDev:F4} AccelStdDev={m.AccelerationStdDev:F4}");
|
|
if (kv.Key == TelemetryPhase.FinalApproach || kv.Key == TelemetryPhase.FinalRotation)
|
|
_output.WriteLine($" GoalPos={m.GoalPositionError:F4}m GoalHeading={m.GoalHeadingErrorDeg:F2}°");
|
|
}
|
|
|
|
// Detected Patterns
|
|
_output.WriteLine($"\n ── Detected Patterns ({report.DetectedPatterns.Count}) ──");
|
|
foreach (var p in report.DetectedPatterns)
|
|
{
|
|
_output.WriteLine($" [{p.DetectedInPhase}] {p.Category} (severity={p.Severity:F2})");
|
|
_output.WriteLine($" {p.Description}");
|
|
if (p.Evidence.Count > 0)
|
|
{
|
|
var evidence = string.Join(", ", p.Evidence.Select(e => $"{e.Key}={e.Value:F4}"));
|
|
_output.WriteLine($" Evidence: {evidence}");
|
|
}
|
|
}
|
|
|
|
// Suggestions
|
|
_output.WriteLine($"\n ── Suggestions ({report.Suggestions.Count}) ──");
|
|
foreach (var s in report.Suggestions)
|
|
{
|
|
_output.WriteLine($" [{s.Priority}] {s.ParameterDisplayName} (phase={s.TargetPhase})");
|
|
_output.WriteLine($" {s.ParameterPath}: {s.CurrentValue:F4} → {s.SuggestedValue:F4} ({s.ChangePercent:+0.0;-0.0}%)");
|
|
_output.WriteLine($" Confidence={s.Confidence:F2} Reason: {s.Reason}");
|
|
_output.WriteLine($" Impact: {s.ExpectedImpact}");
|
|
}
|
|
|
|
// Overall Assessment
|
|
_output.WriteLine($"\n ── Overall Assessment ──");
|
|
_output.WriteLine($" {report.OverallAssessment}");
|
|
_output.WriteLine($"{"═════════════════════════════════════════════════════════════",0}\n");
|
|
}
|
|
|
|
[Fact]
|
|
public void Demo_Scenario1_GoodTracking_FinalApproachNeedsWork()
|
|
{
|
|
// Scenario: PathFollowing tốt, FinalApproach CTE > 0.02m, Heading > 2°
|
|
var telemetry = new List<TelemetryData>();
|
|
|
|
// InitialRotation: quay tại chỗ 1s
|
|
telemetry.AddRange(CreateInitialRotationTelemetry(8, startMs: 0));
|
|
|
|
// PathFollowing: bám đường tốt 5s (CTE = 0.03m)
|
|
for (int i = 0; i < 50; i++)
|
|
{
|
|
double noise = Math.Sin(i * 0.5) * 0.01;
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
2400 + i * 100,
|
|
x: i * 0.1, y: noise,
|
|
cte: 0.03 + noise, headingError: 0.03 + noise * 0.5,
|
|
linearVel: 1.0 + noise, angularVel: noise * 2,
|
|
distanceToGoal: 8.0 - i * 0.15));
|
|
}
|
|
|
|
// FinalApproach: CTE = 0.04 (vượt ngưỡng 0.02), Heading = 4° (vượt ngưỡng 2°)
|
|
double headingRad4deg = 4.0 * Math.PI / 180.0;
|
|
for (int i = 0; i < 25; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalApproach,
|
|
7400 + i * 200,
|
|
x: 5.0 + i * 0.04, y: 0.04 - i * 0.001,
|
|
cte: 0.04 - i * 0.0005, headingError: headingRad4deg * (1.0 - i * 0.02),
|
|
linearVel: 0.3 - i * 0.008, angularVel: 0.05,
|
|
distanceToGoal: 0.5 - i * 0.02));
|
|
}
|
|
|
|
// FinalRotation: heading OK
|
|
telemetry.AddRange(CreateFinalRotationTelemetry(0.04, 10, startMs: 12400));
|
|
|
|
var metrics = CreateDefaultMetrics(
|
|
cteRms: 0.035, ctePeak: 0.06, cteMean: 0.03,
|
|
headingRms: 0.05, goalPosError: 0.03, goalHeadingError: 0.04,
|
|
overallScore: 75);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
parameters.ControllerType = PathFollowingController.PurePursuit;
|
|
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
PrintReport(report, "Good PathFollowing, FinalApproach needs precision (PurePursuit)");
|
|
|
|
// Verify key behaviors
|
|
report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.LargeCTE && p.DetectedInPhase == TelemetryPhase.FinalApproach)
|
|
.Should().NotBeEmpty("FinalApproach CTE=0.04 > threshold 0.02");
|
|
|
|
report.Suggestions
|
|
.Where(s => s.TargetPhase == TelemetryPhase.FinalApproach)
|
|
.Should().NotBeEmpty("should suggest FinalApproach improvements");
|
|
}
|
|
|
|
[Fact]
|
|
public void Demo_Scenario2_Oscillation_InPathFollowing_Stanley()
|
|
{
|
|
// Scenario: Dao động mạnh ở PathFollowing (Stanley), FinalApproach CTE cao
|
|
var telemetry = new List<TelemetryData>();
|
|
|
|
// PathFollowing: oscillation rõ rệt
|
|
for (int i = 0; i < 80; i++)
|
|
{
|
|
double angVel = Math.Sin(i * 1.5) * 1.2; // dao động lớn
|
|
double cte = 0.06 + Math.Sin(i * 1.5) * 0.04;
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
i * 100,
|
|
x: i * 0.08, y: Math.Sin(i * 0.3) * 0.05,
|
|
cte: cte, headingError: 0.06,
|
|
linearVel: 0.8, angularVel: angVel,
|
|
distanceToGoal: 10.0 - i * 0.12));
|
|
}
|
|
|
|
// FinalApproach: CTE vẫn cao
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalApproach,
|
|
8000 + i * 200,
|
|
x: 6.4 + i * 0.03, y: 0.03,
|
|
cte: 0.05 - i * 0.001, headingError: 0.05,
|
|
linearVel: 0.25, angularVel: 0.1,
|
|
distanceToGoal: 0.4 - i * 0.018));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics(
|
|
cteRms: 0.07, ctePeak: 0.12, cteMean: 0.06,
|
|
headingRms: 0.06, velStdDev: 0.15, accelStdDev: 0.6,
|
|
pathLengthRatio: 1.18, overallScore: 60, passedCriteria: false);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
parameters.ControllerType = PathFollowingController.Stanley;
|
|
parameters.StanleyConfig = new StanleyConfig();
|
|
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
PrintReport(report, "Oscillation + High CTE (Stanley)");
|
|
|
|
report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.Oscillation)
|
|
.Should().NotBeEmpty("strong oscillation in PathFollowing");
|
|
|
|
report.Suggestions.Count.Should().BeGreaterThan(2, "multiple issues → multiple suggestions");
|
|
}
|
|
|
|
[Fact]
|
|
public void Demo_Scenario3_ExcellentPerformance()
|
|
{
|
|
// Scenario: Robot chạy hoàn hảo — không có vấn đề gì
|
|
var telemetry = new List<TelemetryData>();
|
|
|
|
// InitialRotation: nhanh gọn, heading error nhỏ (< 10° threshold)
|
|
for (int i = 0; i < 6; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.InitialRotation, i * 200,
|
|
theta: i * 0.05, linearVel: 0, angularVel: 0.8,
|
|
cte: 0, headingError: 0.1 - i * 0.015, distanceToGoal: 8,
|
|
commandLinearVel: 0));
|
|
}
|
|
|
|
// PathFollowing: CTE rất nhỏ, command = actual velocity
|
|
for (int i = 0; i < 60; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
1200 + i * 100,
|
|
x: i * 0.12, cte: 0.015, headingError: 0.01,
|
|
linearVel: 1.2, angularVel: 0.02,
|
|
distanceToGoal: 8.0 - i * 0.12,
|
|
commandLinearVel: 1.2));
|
|
}
|
|
|
|
// FinalApproach: CTE = 0.01 < 0.02 ✓, Heading < 1° ✓
|
|
for (int i = 0; i < 20; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalApproach,
|
|
7200 + i * 200,
|
|
x: 7.2 + i * 0.04, cte: 0.01, headingError: 0.01,
|
|
linearVel: 0.3, angularVel: 0.01,
|
|
distanceToGoal: 0.4 - i * 0.02,
|
|
commandLinearVel: 0.3));
|
|
}
|
|
|
|
// FinalRotation
|
|
for (int i = 0; i < 8; i++)
|
|
{
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalRotation,
|
|
11200 + i * 200,
|
|
x: 8.0, cte: 0.005, headingError: 0.02 * (1 - i * 0.1),
|
|
linearVel: 0, angularVel: 0.3, distanceToGoal: 0.01,
|
|
commandLinearVel: 0));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics(
|
|
cteRms: 0.015, ctePeak: 0.025, cteMean: 0.013,
|
|
headingRms: 0.015, goalPosError: 0.01, goalHeadingError: 0.01,
|
|
velStdDev: 0.03, accelStdDev: 0.1,
|
|
pathLengthRatio: 1.02, overallScore: 95, passedCriteria: true);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
PrintReport(report, "Excellent Performance — No Issues");
|
|
|
|
report.OverallAssessment.Should().Contain("Excellent");
|
|
report.DetectedPatterns.Count.Should().BeLessThanOrEqualTo(1, "nearly perfect run");
|
|
}
|
|
|
|
[Fact]
|
|
public void Demo_Scenario4_GoalOvershoot_WithJerkyMotion()
|
|
{
|
|
// Scenario: Overshoot tại goal + chuyển động giật
|
|
var telemetry = new List<TelemetryData>();
|
|
|
|
// PathFollowing: có acceleration jitter
|
|
for (int i = 0; i < 50; i++)
|
|
{
|
|
double accelJitter = (i % 3 == 0) ? 0.8 : -0.3;
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.PathFollowing,
|
|
i * 100,
|
|
x: i * 0.1,
|
|
cte: 0.04, headingError: 0.03,
|
|
linearVel: 1.0 + accelJitter * 0.3,
|
|
angularVel: accelJitter * 0.2,
|
|
commandLinearVel: 1.0,
|
|
distanceToGoal: 8.0 - i * 0.15));
|
|
}
|
|
|
|
// FinalApproach: overshoot — distance giảm rồi tăng lại
|
|
for (int i = 0; i < 25; i++)
|
|
{
|
|
double dist;
|
|
if (i < 15)
|
|
dist = 0.4 - i * 0.027; // giảm tới ~0.005
|
|
else
|
|
dist = 0.005 + (i - 15) * 0.008; // tăng lại (overshoot)
|
|
|
|
telemetry.Add(CreatePhasedTelemetry(
|
|
TelemetryPhase.FinalApproach,
|
|
5000 + i * 200,
|
|
x: 5.0 + i * 0.03,
|
|
cte: 0.03, headingError: 0.04,
|
|
linearVel: 0.25, distanceToGoal: dist));
|
|
}
|
|
|
|
var metrics = CreateDefaultMetrics(
|
|
cteRms: 0.04, ctePeak: 0.08, cteMean: 0.035,
|
|
headingRms: 0.04, goalPosError: 0.05,
|
|
velStdDev: 0.2, accelStdDev: 0.7,
|
|
overallScore: 65, passedCriteria: false);
|
|
var parameters = TestHelpers.CreateDefaultParameterSet();
|
|
parameters.ControllerType = PathFollowingController.PurePursuit;
|
|
|
|
var report = _advisor.Analyze(telemetry, metrics, parameters);
|
|
PrintReport(report, "Goal Overshoot + Jerky Motion (PurePursuit)");
|
|
|
|
report.DetectedPatterns
|
|
.Where(p => p.Category == DiagnosticCategory.GoalOvershoot)
|
|
.Should().NotBeEmpty("distance-to-goal increases after minimum");
|
|
}
|
|
|
|
#endregion
|
|
}
|