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

feat: configure observation visualization

Browse Source
This commit is contained in:
梁薄云
2026-08-06 10:16:48 +08:00
parent 0bc8df9d5e
commit c5ea4f6d91
5 changed files with 201 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs
+36 -4
View File
@@ -12,16 +12,26 @@ public sealed class TrajectoryObservationSettings
public float MapResolutionMillimeters { get; set; } = 50f;
public double ReplanPeriodSeconds { get; set; } = 0.20d;
public double ObserverPeriodSeconds { get; set; } = 0.05d;
public double SolverTimeoutSeconds { get; set; } = 0.50d;
public int MaximumOsqpIterations { get; set; } = 12000;
/// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；不等于观察循环周期。</summary>
public double TimeHorizonSeconds { get; set; } = 6d;
public double SolverTimeoutSeconds { get; set; } = 5d;
public int MaximumOsqpIterations { get; set; } = 100000;
/// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。</summary>
public double TimeHorizonSeconds { get; set; } = 2d;
/// <summary>发布 Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；不等于观察循环周期。</summary>
public double OutputTimeStepSeconds { get; set; } = 0.10d;
public double VehicleLengthMeters { get; set; } = 0.80d;
public double VehicleWidthMeters { get; set; } = 0.60d;
public double SafetyMarginMeters { get; set; } = 0.05d;
public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;
public bool EnableWebVisualization { get; set; } = false;
public bool AutoOpenWebVisualization { get; set; } = true;
public int WebVisualizationPort { get; set; } = 0;
public double WebRefreshRateHz { get; set; } = 10d;
public int VisualizationHistoryCycleLimit { get; set; } = 60;
public bool EnableNativePainterVisualization { get; set; } = false;
public double DirectionConfirmationSpeedMetersPerSecond { get; set; } = 0.02d;
public int DirectionConfirmationSamples { get; set; } = 3;
public double GearSwitchProjectionToleranceMeters { get; set; } = 0.50d;
public double GearSwitchStopHoldSeconds { get; set; } = 0.20d;
public TrajectoryObservationSettings CreateValidatedSnapshot()
{
@@ -39,6 +49,16 @@ public sealed class TrajectoryObservationSettings
VehicleWidthMeters = VehicleWidthMeters,
SafetyMarginMeters = SafetyMarginMeters,
MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
EnableWebVisualization = EnableWebVisualization,
AutoOpenWebVisualization = AutoOpenWebVisualization,
WebVisualizationPort = WebVisualizationPort,
WebRefreshRateHz = WebRefreshRateHz,
VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
EnableNativePainterVisualization = EnableNativePainterVisualization,
DirectionConfirmationSpeedMetersPerSecond = DirectionConfirmationSpeedMetersPerSecond,
DirectionConfirmationSamples = DirectionConfirmationSamples,
GearSwitchProjectionToleranceMeters = GearSwitchProjectionToleranceMeters,
GearSwitchStopHoldSeconds = GearSwitchStopHoldSeconds,
};
snapshot.Validate();
return snapshot;
@@ -62,6 +82,18 @@ public sealed class TrajectoryObservationSettings
EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters));
EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters));
EnsurePositiveFinite(MaximumCurvaturePerMeter, nameof(MaximumCurvaturePerMeter));
if (WebVisualizationPort != 0 && (WebVisualizationPort < 1024 || WebVisualizationPort > 65535))
throw new ArgumentOutOfRangeException(nameof(WebVisualizationPort),
"Port must be zero or in the inclusive range 1024 through 65535.");
EnsurePositiveFinite(WebRefreshRateHz, nameof(WebRefreshRateHz));
if (VisualizationHistoryCycleLimit <= 0)
throw new ArgumentOutOfRangeException(nameof(VisualizationHistoryCycleLimit), "Value must be positive.");
EnsurePositiveFinite(DirectionConfirmationSpeedMetersPerSecond,
nameof(DirectionConfirmationSpeedMetersPerSecond));
if (DirectionConfirmationSamples <= 0)
throw new ArgumentOutOfRangeException(nameof(DirectionConfirmationSamples), "Value must be positive.");
EnsurePositiveFinite(GearSwitchProjectionToleranceMeters, nameof(GearSwitchProjectionToleranceMeters));
EnsurePositiveFinite(GearSwitchStopHoldSeconds, nameof(GearSwitchStopHoldSeconds));
}
public VehicleParameters CreateVehicle()