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

feat: show EM observation failure diagnostics

Browse Source
This commit is contained in:
梁薄云
2026-08-05 09:59:52 +08:00
parent 6262a4573a
commit 0be35d73d4
3 changed files with 44 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs
+8 -6
View File
@@ -248,7 +248,8 @@ internal static class TrajectoryObservationMovementTestRunner
bootstrapTimer.Stop();
if (!bootstrap.Succeeded)
{
DrawIfCurrent(sessionId, bootstrap, null, null, null);
DrawIfCurrent(sessionId, bootstrap, null, null, null,
new TrajectoryObservationDiagnostic("bootstrap failed: " + bootstrap.FailureReason));
LogIfCurrent(sessionId, "Planning bootstrap failed after " +
bootstrapTimer.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
" ms: " + bootstrap.FailureReason);
@@ -280,7 +281,7 @@ internal static class TrajectoryObservationMovementTestRunner
settings.MapPaddingMeters);
TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create(
now, observation.PublishedTrajectory);
DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState);
DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
{
LogIfCurrent(sessionId, runtimeState.WorldNotice);
@@ -365,14 +366,15 @@ internal static class TrajectoryObservationMovementTestRunner
private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap,
TrajectoryObservationObservation observation, TrajectoryObservationCharts charts,
TrajectoryObservationRuntimeState runtimeState)
TrajectoryObservationRuntimeState runtimeState, TrajectoryObservationDiagnostic diagnostic)
{
lock (SessionSync)
{
if (activeSessionId != sessionId) return;
Presentation.DrawWorld(bootstrap, observation, runtimeState);
Presentation.DrawLs(charts);
Presentation.DrawSt(charts);
string diagnosticText = diagnostic == null ? string.Empty : diagnostic.Text;
Presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText);
Presentation.DrawLs(charts, diagnosticText);
Presentation.DrawSt(charts, diagnosticText);
}
}
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs
+19 -6
View File
@@ -88,7 +88,8 @@ public sealed class TrajectoryObservationPresentation
private readonly Painter stPainter = UI.GetPainter("TrajectoryObserver.ST", true);
public void DrawWorld(TrajectoryObservationBootstrapResult bootstrap,
TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState)
TrajectoryObservationObservation observation, TrajectoryObservationRuntimeState runtimeState,
string diagnosticText)
{
worldPainter.Clear();
if (bootstrap == null)
@@ -114,18 +115,22 @@ public sealed class TrajectoryObservationPresentation
EmTrajectory trajectory = observation == null ? null : observation.PublishedTrajectory;
if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
{
worldPainter.DrawText(Color.LightYellow, "No published trajectory available.", 0f, 0f);
float diagnosticX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
float diagnosticY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMin + 300f;
worldPainter.DrawText(Color.LightYellow,
EmptyChartMessage("No published trajectory available.\n", diagnosticText), diagnosticX, diagnosticY);
return;
}
DrawEmPath(trajectory.Points);
}
public void DrawLs(TrajectoryObservationCharts charts)
public void DrawLs(TrajectoryObservationCharts charts, string diagnosticText)
{
lsPainter.Clear();
if (charts == null || charts.LsSamples == null || charts.LsSamples.Count == 0)
{
lsPainter.DrawText(Color.LightYellow, "No published trajectory available for L-S chart.", 0f, 0f);
lsPainter.DrawText(Color.LightYellow,
EmptyChartMessage("No published trajectory available for L-S chart.\n", diagnosticText), 0f, 0f);
return;
}
@@ -147,12 +152,13 @@ public sealed class TrajectoryObservationPresentation
}
}
public void DrawSt(TrajectoryObservationCharts charts)
public void DrawSt(TrajectoryObservationCharts charts, string diagnosticText)
{
stPainter.Clear();
if (charts == null || charts.StSamples == null || charts.StSamples.Count == 0)
{
stPainter.DrawText(Color.LightYellow, "No published trajectory available for T-S/T-V charts.", 0f, 0f);
stPainter.DrawText(Color.LightYellow,
EmptyChartMessage("No published trajectory available for T-S/T-V charts.\n", diagnosticText), 0f, 0f);
return;
}
@@ -179,6 +185,13 @@ public sealed class TrajectoryObservationPresentation
stPainter.Clear();
}
private static string EmptyChartMessage(string label, string diagnosticText)
{
return string.IsNullOrWhiteSpace(diagnosticText)
? label
: label + diagnosticText;
}
private void DrawMap(PlanningGridMap map)
{
if (map == null) return;