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

fix: stabilize stopped-start EM planning and add txt diagnostics

Browse Source
This commit is contained in:
梁薄云
2026-08-07 15:51:31 +08:00
parent f2970d0aff
commit d378ad6257
13 changed files with 373 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs
+62 -2
View File
@@ -204,6 +204,7 @@ internal static class TrajectoryObservationMovementTestRunner
private static Task activeTask;
private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
private static TrajectoryObservationPresentation activePresentation;
private static TrajectoryObservationReportWriter activeReportWriter;
private static long nextSessionId;
private static long activeSessionId;
private static long stateSequence;
@@ -218,6 +219,7 @@ internal static class TrajectoryObservationMovementTestRunner
var cancellation = new CancellationTokenSource();
CancellationTokenSource previousCancellation;
long sessionId;
TrajectoryObservationReportWriter reportWriter = null;
lock (SessionSync)
{
previousCancellation = activeCancellation;
@@ -227,6 +229,8 @@ internal static class TrajectoryObservationMovementTestRunner
activeSessionId = sessionId;
StopActiveVisualizationNoLock();
activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null;
reportWriter = new TrajectoryObservationReportWriter(sessionId);
activeReportWriter = reportWriter;
PrintStatus("Starting frozen observation session " +
sessionId.ToString(CultureInfo.InvariantCulture) + ".");
}
@@ -239,7 +243,7 @@ internal static class TrajectoryObservationMovementTestRunner
{
if (activeSessionId == sessionId) activeTask = task;
}
_ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed), TaskScheduler.Default);
_ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed, reportWriter), TaskScheduler.Default);
}
internal static void Stop()
@@ -271,6 +275,13 @@ internal static class TrajectoryObservationMovementTestRunner
{
token.ThrowIfCancellationRequested();
VehicleMotionState initialState = ReadVehicleState();
LogIfCurrent(sessionId, "initial state: pose=(" + Format(initialState.Pose.X) + ", " +
Format(initialState.Pose.Y) + ", " + Format(initialState.Pose.Heading) + ") m/rad, signed speed=" +
initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) + " m/s, acceleration=" +
(initialState.LongitudinalAccelerationMetersPerSecondSquared.HasValue
? Format(initialState.LongitudinalAccelerationMetersPerSecondSquared.Value)
: "unavailable") + " m/s2, stateSequence=" +
initialState.SequenceId.ToString(CultureInfo.InvariantCulture));
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
@@ -294,6 +305,15 @@ internal static class TrajectoryObservationMovementTestRunner
new EmPlanningService(new OsqpNativeSolver()),
"trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture));
LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
LogIfCurrent(sessionId, CreateSettingsDiagnostic(settings));
EmPlannerConfiguration effectiveConfiguration = controller.CreateEffectiveConfigurationSnapshot();
double stopSpeedTolerance = effectiveConfiguration.Longitudinal.StopSpeedToleranceMetersPerSecond;
LogIfCurrent(sessionId, "static start check: initialSignedSpeed=" +
initialState.SignedLongitudinalSpeedMetersPerSecond.ToString("R", CultureInfo.InvariantCulture) +
", stopSpeedTolerance=" + stopSpeedTolerance.ToString("R", CultureInfo.InvariantCulture) +
", treatedAsStopped=" +
(Math.Abs(initialState.SignedLongitudinalSpeedMetersPerSecond) <= stopSpeedTolerance)
.ToString(CultureInfo.InvariantCulture));
var observationLoop = new TrajectoryObservationLoop(controller);
TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled(
sessionId, bootstrap, controller, settings, obstacleSnapshotVersion);
@@ -332,7 +352,14 @@ internal static class TrajectoryObservationMovementTestRunner
if (tick.PlanningStarted)
LogIfCurrent(sessionId, "planning status=pending");
if (tick.PlanningCompleted)
{
LogIfCurrent(sessionId, diagnostic.Text);
if (observation.PublishedTrajectory != null && charts != null)
{
AppendReportIfCurrent(sessionId,
TrajectoryObservationDiagnostics.CreateCurveReport(charts, observation.PublishedTrajectory).Text);
}
}
}
}
@@ -490,14 +517,25 @@ internal static class TrajectoryObservationMovementTestRunner
}
}
private static void AppendReportIfCurrent(long sessionId, string text)
{
lock (SessionSync)
{
if (activeSessionId != sessionId) return;
activeReportWriter?.Append(text);
}
}
private static void PrintStatus(string message)
{
string text = ObserveOnlyNotice + "\n" + message;
Hedingben.ToastText(text, StatusChannel);
Console.WriteLine("[TrajectoryObserver] " + text);
activeReportWriter?.Append(message);
}
private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed)
private static void Finish(long sessionId, CancellationTokenSource cancellation, Task completed,
TrajectoryObservationReportWriter reportWriter)
{
try
{
@@ -514,16 +552,38 @@ internal static class TrajectoryObservationMovementTestRunner
{
lock (SessionSync)
{
if (ReferenceEquals(activeReportWriter, reportWriter))
activeReportWriter = null;
if (activeSessionId == sessionId)
{
activeTask = null;
activeCancellation = null;
}
}
try
{
string reportPath = reportWriter.Save();
Console.WriteLine("[TrajectoryObserver] report=" + reportPath);
}
catch (Exception exception)
{
Console.WriteLine("[TrajectoryObserver] report write failed: " + exception.Message);
}
cancellation.Dispose();
}
}
private static string CreateSettingsDiagnostic(TrajectoryObservationSettings settings)
{
return "observation settings: planningScope=" + settings.PlanningScope +
", webVisualization=" + settings.EnableWebVisualization +
", nativePainter=" + settings.EnableNativePainterVisualization +
", outputTimeStep=" + Format(settings.OutputTimeStepSeconds) + "s" +
", observerPeriod=" + Format(settings.ObserverPeriodSeconds) + "s" +
", solverTimeout=" + Format(settings.SolverTimeoutSeconds) + "s" +
", maximumOsqpIterations=" + settings.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture);
}
private static void ClearAndLogRuntimeFaultIfCurrent(long sessionId, Exception exception)
{
lock (SessionSync)