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/TrajectoryObservationDiagnostics.cs
+74 -1
View File
@@ -1,6 +1,7 @@
using System;
using System.Collections.Generic;
using System.Globalization;
using System.Text;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -18,10 +19,69 @@ public sealed class TrajectoryObservationDiagnostic
public static class TrajectoryObservationDiagnostics
{
public static TrajectoryObservationDiagnostic CreateCurveReport(TrajectoryObservationCharts charts,
EmTrajectory trajectory)
{
if (charts == null || trajectory == null)
return new TrajectoryObservationDiagnostic("curve report unavailable");
IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
var text = new StringBuilder();
text.AppendLine("LS_CURVE");
text.AppendLine("index,referenceS(m),lateralOffset(m)");
for (int index = 0; index < charts.LsSamples.Count; index++)
{
TrajectoryObservationLsSample sample = charts.LsSamples[index];
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
sample.PathS.ToString("R", CultureInfo.InvariantCulture) + "," +
sample.LateralOffset.ToString("R", CultureInfo.InvariantCulture));
}
text.AppendLine("ST_CURVE");
text.AppendLine("index,time(s),pathS(m)");
for (int index = 0; index < charts.StSamples.Count; index++)
{
TrajectoryObservationStSample sample = charts.StSamples[index];
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
sample.PathS.ToString("R", CultureInfo.InvariantCulture));
}
text.AppendLine("SPEED_CURVE");
text.AppendLine("index,time(s),signedSpeed(m/s)");
for (int index = 0; index < charts.SpeedSamples.Count; index++)
{
TrajectoryObservationSpeedSample sample = charts.SpeedSamples[index];
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
sample.SignedLongitudinalVelocity.ToString("R", CultureInfo.InvariantCulture));
}
text.AppendLine("ACCELERATION_CURVE");
text.AppendLine("index,time(s),acceleration(m/s2)");
for (int index = 0; index < points.Count; index++)
{
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
points[index].LongitudinalAcceleration.ToString("R", CultureInfo.InvariantCulture));
}
text.AppendLine("JERK_CURVE");
text.AppendLine("index,time(s),jerk(m/s3)");
for (int index = 0; index + 1 < points.Count; index++)
{
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
points[index].LongitudinalJerk.ToString("R", CultureInfo.InvariantCulture));
}
return new TrajectoryObservationDiagnostic(text.ToString());
}
public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
{
if (configuration == null || configuration.Scheduling == null || configuration.Solver == null ||
configuration.Longitudinal == null)
configuration.Longitudinal == null || configuration.Validation == null)
throw new ArgumentNullException(nameof(configuration));
double horizon = configuration.Scheduling.TimeHorizonSeconds;
@@ -57,6 +117,19 @@ public static class TrajectoryObservationDiagnostics
"maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
"solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
"replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s\n" +
"kinematicTolerance=" + Format(configuration.Validation.KinematicTolerance, "G17") + "\n" +
"stopSpeedToleranceMetersPerSecond=" + Format(longitudinal.StopSpeedToleranceMetersPerSecond, "G17") + "\n" +
"terminalPositionToleranceMeters=" + Format(configuration.Validation.TerminalPositionToleranceMeters, "G17") + "\n" +
"terminalYawToleranceRadians=" + Format(configuration.Validation.TerminalYawToleranceRadians, "G17") + "\n" +
"maximumOptimizationTimeStepSeconds=" + Format(configuration.Scheduling.MaximumOptimizationTimeStepSeconds, "F3") + "\n" +
"maximumOptimizationSpatialStepMeters=" + Format(configuration.Scheduling.MaximumOptimizationSpatialStepMeters, "F3") + "\n" +
"maximumOptimizationKnotCount=" + configuration.Scheduling.MaximumOptimizationKnotCount.ToString(CultureInfo.InvariantCulture) + "\n" +
"maximumPublishedSampleCount=" + configuration.Scheduling.MaximumPublishedSampleCount.ToString(CultureInfo.InvariantCulture) + "\n" +
"maximumForwardSpeedMetersPerSecond=" + Format(longitudinal.MaximumForwardSpeedMetersPerSecond, "F3") + "\n" +
"maximumReverseSpeedMetersPerSecond=" + Format(longitudinal.MaximumReverseSpeedMetersPerSecond, "F3") + "\n" +
"maximumAccelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumAccelerationMetersPerSecondSquared, "F3") + "\n" +
"maximumDecelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumDecelerationMetersPerSecondSquared, "F3") + "\n" +
"maximumJerkMetersPerSecondCubed=" + Format(longitudinal.MaximumJerkMetersPerSecondCubed, "F3") + "\n" +
"maximumJerkLimitedStopDistance=" + Format(worstStop.DistanceMeters, "F3") + "m\n" +
"maximumJerkLimitedStopDuration=" + Format(worstStop.DurationSeconds, "F3") + "s\n" +
"requiredDistanceHorizon=" + Format(requiredDistanceHorizon, "F3") + "m");