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

feat: keep rolling speed envelopes open

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:33:50 +08:00
parent 105ec4bdad
commit 94a9be9c02
3 changed files with 132 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs
+14 -7
View File
@@ -11,7 +11,7 @@ public sealed class PathSpeedLimit
internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond, bool hasStopBoundary)
{
PathS = CopyStrictStations(pathS, nameof(pathS));
MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
@@ -22,13 +22,15 @@ public sealed class PathSpeedLimit
StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit));
if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d)
throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond));
if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d)
if (hasStopBoundary &&
(MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d))
{
throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
throw new ArgumentException("A real stop boundary must have an exact zero speed limit.");
}
DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
HasStopBoundary = hasStopBoundary;
}
public IReadOnlyList<double> PathS { get; }
@@ -43,7 +45,12 @@ public sealed class PathSpeedLimit
public double DirectionMaximumSpeedMetersPerSecond { get; }
public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
public bool HasStopBoundary { get; }
public double PathUpperBoundS { get { return PathS[PathS.Count - 1]; } }
[Obsolete("Use PathUpperBoundS.")]
public double TerminalPathS { get { return PathUpperBoundS; } }
public double MaximumSpeedAt(double pathS)
{
@@ -67,11 +74,11 @@ public sealed class PathSpeedLimit
private double Interpolate(IReadOnlyList<double> values, double pathS)
{
if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > PathUpperBoundS + StationTolerance)
throw new ArgumentOutOfRangeException(nameof(pathS));
if (pathS <= PathS[0])
return values[0];
if (pathS >= TerminalPathS)
if (pathS >= PathUpperBoundS)
return values[values.Count - 1];
for (int index = 1; index < PathS.Count; index++)