First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: complete EM observation final review

Browse Source
This commit is contained in:
梁薄云
2026-08-04 17:33:14 +08:00
parent d0e673b573
commit bd7170b611
7 changed files with 655 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs
+44 -41
View File
@@ -31,6 +31,10 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
public float MapResolutionMm = 50f;
public double ReplanPeriodSeconds = 0.20d;
public double ObserverPeriodSeconds = 0.05d;
public double VehicleLengthMeters = 0.80d;
public double VehicleWidthMeters = 0.60d;
public double SafetyMarginMeters = 0.05d;
public double MaximumCurvaturePerMeter = 1d / 1.20d;
public override void Test()
{
@@ -55,8 +59,12 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
MapResolutionMillimeters = MapResolutionMm,
ReplanPeriodSeconds = ReplanPeriodSeconds,
ObserverPeriodSeconds = ObserverPeriodSeconds,
VehicleLengthMeters = VehicleLengthMeters,
VehicleWidthMeters = VehicleWidthMeters,
SafetyMarginMeters = SafetyMarginMeters,
MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
};
settings.Validate();
settings = settings.CreateValidatedSnapshot();
TimeSpan observerPeriod = TimeSpan.FromSeconds(settings.ObserverPeriodSeconds);
IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
long obstacleSnapshotVersion = obstacles.Count == 0
@@ -157,9 +165,6 @@ internal static class TrajectoryObservationMovementTestRunner
{
private const string StatusChannel = "TrajectoryObserver";
private const string ObserveOnlyNotice = "OBSERVE_ONLY: no chassis command is sent.";
private const string GearSwitchWaitingNotice =
"等待真实档位/方向确认；观察模式不会推进下一方向段";
private static readonly TimeSpan StatusPeriod = TimeSpan.FromSeconds(1d);
private static readonly object SessionSync = new object();
private static readonly TrajectoryObservationPresentation Presentation =
new TrajectoryObservationPresentation();
@@ -212,7 +217,9 @@ internal static class TrajectoryObservationMovementTestRunner
activeCancellation = null;
activeTask = null;
activeSessionId = 0L;
Presentation.ClearAll();
if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
TrajectoryObservationSessionEndReason.Cancellation))
Presentation.ClearAll();
PrintStatus("Observation stop requested; all observer layers were cleared.");
}
@@ -241,7 +248,7 @@ internal static class TrajectoryObservationMovementTestRunner
bootstrapTimer.Stop();
if (!bootstrap.Succeeded)
{
DrawIfCurrent(sessionId, bootstrap, null, null);
DrawIfCurrent(sessionId, bootstrap, null, null, null);
LogIfCurrent(sessionId, "Planning bootstrap failed after " +
bootstrapTimer.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
" ms: " + bootstrap.FailureReason);
@@ -253,44 +260,33 @@ internal static class TrajectoryObservationMovementTestRunner
var controller = new TrajectoryObservationController(bootstrap, settings,
new EmPlanningService(new OsqpNativeSolver()),
"trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
var observationLoop = new TrajectoryObservationLoop(controller);
DirectionSegmentView segment = bootstrap.Segments[0];
DateTimeOffset nextStatusAt = DateTimeOffset.MinValue;
PlanningCycleResult latestCycle = null;
TimeSpan latestPlanningElapsed = TimeSpan.Zero;
bool gearSwitchWaitingPrinted = false;
while (true)
{
await Task.Delay(observerPeriod, token).ConfigureAwait(false);
DateTimeOffset now = DateTimeOffset.UtcNow;
VehicleMotionState state = ReadVehicleState();
if (controller.ShouldStartCycle(now))
{
var planningTimer = Stopwatch.StartNew();
latestCycle = await controller.StartCycle(now, state, token).ConfigureAwait(false);
planningTimer.Stop();
latestPlanningElapsed = planningTimer.Elapsed;
}
TrajectoryObservationObservation observation = controller.Observe(now, state);
DateTimeOffset now = state.CapturedAtUtc;
TrajectoryObservationLoopTick tick = observationLoop.Tick(now, state, token);
TrajectoryObservationObservation observation = tick.Observation;
TrajectoryObservationCharts charts = observation.PublishedTrajectory == null
? null
: TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment,
settings.MapPaddingMeters);
DrawIfCurrent(sessionId, bootstrap, observation, charts);
bool waitingAtGearSwitch = HasReachedGearSwitchFinal(now, observation.PublishedTrajectory);
if (waitingAtGearSwitch && !gearSwitchWaitingPrinted)
TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
now, observation.PublishedTrajectory);
DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState);
if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
{
LogIfCurrent(sessionId, GearSwitchWaitingNotice);
LogIfCurrent(sessionId, runtimeState.WorldNotice);
gearSwitchWaitingPrinted = true;
}
if (now >= nextStatusAt)
{
if (tick.ShouldLog)
LogIfCurrent(sessionId, CreateTickStatus(sessionId, observation, charts,
latestCycle, latestPlanningElapsed, waitingAtGearSwitch));
nextStatusAt = now + StatusPeriod;
}
tick.LatestCycle, tick.LatestPlanningElapsed, runtimeState.WaitingAtGearSwitch));
}
}
@@ -305,14 +301,6 @@ internal static class TrajectoryObservationMovementTestRunner
speed.Vx, null, DateTimeOffset.UtcNow, Interlocked.Increment(ref stateSequence));
}
private static bool HasReachedGearSwitchFinal(DateTimeOffset now, EmTrajectory trajectory)
{
if (trajectory == null || trajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
return false;
EmTrajectoryPoint finalPoint = trajectory.Points[trajectory.Points.Count - 1];
return (now - trajectory.Metadata.EffectiveAtUtc).TotalSeconds >= finalPoint.TimeFromStart;
}
private static string CreateBootstrapStatus(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
TrajectoryObservationSettings settings, int obstacleCount, TimeSpan elapsed)
{
@@ -356,7 +344,9 @@ internal static class TrajectoryObservationMovementTestRunner
string projectionFailures = charts == null
? "unavailable"
: charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
string waiting = waitingAtGearSwitch ? "\n" + GearSwitchWaitingNotice : string.Empty;
string waiting = waitingAtGearSwitch
? "\n" + TrajectoryObservationRuntimeState.GearSwitchWaitingNotice
: string.Empty;
return "Session " + sessionId.ToString(CultureInfo.InvariantCulture) + ".\n" +
"pose=(" + Format(observation.VehicleState.Pose.X) + ", " +
@@ -370,12 +360,13 @@ internal static class TrajectoryObservationMovementTestRunner
}
private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
TrajectoryObservationRuntimeState runtimeState)
{
lock (SessionSync)
{
if (activeSessionId != sessionId) return;
Presentation.DrawWorld(bootstrap, observation);
Presentation.DrawWorld(bootstrap, observation, runtimeState);
Presentation.DrawLs(charts);
Presentation.DrawSt(charts);
}
@@ -406,8 +397,7 @@ internal static class TrajectoryObservationMovementTestRunner
}
catch (Exception exception)
{
LogIfCurrent(sessionId, "Observation session failed: " +
exception.GetType().Name + ": " + exception.Message);
ClearAndLogRuntimeFaultIfCurrent(sessionId, exception);
}
finally
{
@@ -423,6 +413,19 @@ internal static class TrajectoryObservationMovementTestRunner
}
}
private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
{
lock (SessionSync)
{
if (activeSessionId != sessionId) return;
if (TrajectoryObservationSessionLifecycle.ShouldClearLayers(
TrajectoryObservationSessionEndReason.RuntimeFault))
Presentation.ClearAll();
PrintStatus("Observation session failed; all observer layers were cleared: " +
exception.GetType().Name + ": " + exception.Message);
}
}
private static void CancelWithoutWaiting(CancellationTokenSource cancellation)
{
if (cancellation == null) return;