feat: observe EM planning across gear segments

This commit is contained in:
梁薄云
2026-08-06 11:10:35 +08:00
parent e141a76fdb
commit 51f744bc00
3 changed files with 249 additions and 7 deletions
@@ -1,5 +1,6 @@
using System;
using System.Collections.Generic;
using System.IO;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
@@ -11,12 +12,23 @@ internal static class TrajectoryObservationSegmentChecks
{
public static void Run()
{
RejectsHardcodedActiveSegmentIndex();
AdvancesOnlyAfterContinuousStopAndStableNextDirection();
RejectsNonTerminalOrMismatchedGearTrajectory();
ResetsConfirmationForInvalidDirectionEvidence();
CompletesOneSegmentWithoutIndexingPastTheEnd();
}
private static void RejectsHardcodedActiveSegmentIndex()
{
string pipelinePath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
"ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPipeline.cs");
string source = File.ReadAllText(pipelinePath);
Verification.True(source.IndexOf("segmentIndex = 0", StringComparison.Ordinal) < 0,
"active observation segment is not hardcoded to zero");
}
private static void AdvancesOnlyAfterContinuousStopAndStableNextDirection()
{
DateTimeOffset t0 = new DateTimeOffset(2026, 8, 6, 0, 0, 0, TimeSpan.Zero);