285 lines
14 KiB
C#
285 lines
14 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
public sealed class TrajectoryObservationSettings
|
|
{
|
|
public EmPlanningScope PlanningScope { get; set; } = EmPlanningScope.FullDirectionSegment;
|
|
public double MapPaddingMeters { get; set; } = 2d;
|
|
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; } = 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; } = true;
|
|
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()
|
|
{
|
|
var snapshot = new TrajectoryObservationSettings
|
|
{
|
|
PlanningScope = PlanningScope,
|
|
MapPaddingMeters = MapPaddingMeters,
|
|
MapResolutionMillimeters = MapResolutionMillimeters,
|
|
ReplanPeriodSeconds = ReplanPeriodSeconds,
|
|
ObserverPeriodSeconds = ObserverPeriodSeconds,
|
|
SolverTimeoutSeconds = SolverTimeoutSeconds,
|
|
MaximumOsqpIterations = MaximumOsqpIterations,
|
|
TimeHorizonSeconds = TimeHorizonSeconds,
|
|
OutputTimeStepSeconds = OutputTimeStepSeconds,
|
|
VehicleLengthMeters = VehicleLengthMeters,
|
|
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;
|
|
}
|
|
|
|
public void Validate()
|
|
{
|
|
if (!Enum.IsDefined(typeof(EmPlanningScope), PlanningScope))
|
|
throw new ArgumentOutOfRangeException(nameof(PlanningScope), "Value must be a defined planning scope.");
|
|
EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
|
|
EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters));
|
|
EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds));
|
|
EnsurePositiveFinite(ObserverPeriodSeconds, nameof(ObserverPeriodSeconds));
|
|
EnsurePositiveFinite(SolverTimeoutSeconds, nameof(SolverTimeoutSeconds));
|
|
if (MaximumOsqpIterations <= 0)
|
|
throw new ArgumentOutOfRangeException(nameof(MaximumOsqpIterations), "Value must be positive.");
|
|
EnsurePositiveFinite(TimeHorizonSeconds, nameof(TimeHorizonSeconds));
|
|
EnsurePositiveFinite(OutputTimeStepSeconds, nameof(OutputTimeStepSeconds));
|
|
if (OutputTimeStepSeconds > TimeHorizonSeconds)
|
|
throw new ArgumentOutOfRangeException(nameof(OutputTimeStepSeconds),
|
|
"The trajectory timestamp spacing cannot exceed the ST time horizon.");
|
|
EnsurePositiveFinite(VehicleLengthMeters, nameof(VehicleLengthMeters));
|
|
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()
|
|
{
|
|
Validate();
|
|
return new VehicleParameters
|
|
{
|
|
LengthMeters = VehicleLengthMeters,
|
|
WidthMeters = VehicleWidthMeters,
|
|
SafetyMarginMeters = SafetyMarginMeters,
|
|
MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
|
|
};
|
|
}
|
|
|
|
private static void EnsurePositiveFinite(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d)
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite and positive.");
|
|
}
|
|
}
|
|
|
|
public sealed class TrajectoryObservationObstacle
|
|
{
|
|
private readonly bool _isCircle;
|
|
private readonly double _first;
|
|
private readonly double _second;
|
|
private readonly double _third;
|
|
private readonly double _fourth;
|
|
|
|
private TrajectoryObservationObstacle(bool isCircle, double first, double second, double third, double fourth)
|
|
{
|
|
_isCircle = isCircle;
|
|
_first = first;
|
|
_second = second;
|
|
_third = third;
|
|
_fourth = fourth;
|
|
}
|
|
|
|
public static TrajectoryObservationObstacle Circle(double centerXMillimeters, double centerYMillimeters,
|
|
double radiusMillimeters)
|
|
{
|
|
EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
|
|
EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
|
|
EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
|
|
return new TrajectoryObservationObstacle(true, centerXMillimeters, centerYMillimeters, radiusMillimeters, 0d);
|
|
}
|
|
|
|
public static TrajectoryObservationObstacle Rectangle(double xMinMillimeters, double xMaxMillimeters,
|
|
double yMinMillimeters, double yMaxMillimeters)
|
|
{
|
|
EnsureFinite(xMinMillimeters, nameof(xMinMillimeters));
|
|
EnsureFinite(xMaxMillimeters, nameof(xMaxMillimeters));
|
|
EnsureFinite(yMinMillimeters, nameof(yMinMillimeters));
|
|
EnsureFinite(yMaxMillimeters, nameof(yMaxMillimeters));
|
|
if (xMaxMillimeters <= xMinMillimeters || yMaxMillimeters <= yMinMillimeters)
|
|
throw new ArgumentOutOfRangeException(nameof(xMaxMillimeters), "Rectangle bounds must be non-degenerate.");
|
|
return new TrajectoryObservationObstacle(false, xMinMillimeters, xMaxMillimeters, yMinMillimeters, yMaxMillimeters);
|
|
}
|
|
|
|
public MapBoundsMm GetBounds()
|
|
{
|
|
return _isCircle
|
|
? new MapBoundsMm(ToFiniteFloat(_first - _third), ToFiniteFloat(_first + _third),
|
|
ToFiniteFloat(_second - _third), ToFiniteFloat(_second + _third))
|
|
: new MapBoundsMm(ToFiniteFloat(_first), ToFiniteFloat(_second), ToFiniteFloat(_third), ToFiniteFloat(_fourth));
|
|
}
|
|
|
|
public IMapObstacle ToMapObstacle()
|
|
{
|
|
return _isCircle
|
|
? new CircleObstacle(ToFiniteFloat(_first), ToFiniteFloat(_second), ToFiniteFloat(_third))
|
|
: new AxisAlignedRectangleObstacle(ToFiniteFloat(_first), ToFiniteFloat(_second),
|
|
ToFiniteFloat(_third), ToFiniteFloat(_fourth));
|
|
}
|
|
|
|
private static float ToFiniteFloat(double value)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
|
|
throw new ArgumentOutOfRangeException(nameof(value), "Value cannot be represented as a finite millimeter coordinate.");
|
|
return (float)value;
|
|
}
|
|
|
|
private static void EnsureFinite(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value))
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
|
|
}
|
|
|
|
private static void EnsurePositiveFinite(double value, string parameterName)
|
|
{
|
|
EnsureFinite(value, parameterName);
|
|
if (value <= 0d) throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
|
|
}
|
|
}
|
|
|
|
public static class TrajectoryObservationSetupFactory
|
|
{
|
|
public static CoarsePathPlanningJob CreateBootstrapJob(Pose2D start, Pose2D goal,
|
|
TrajectoryObservationSettings settings, IReadOnlyList<TrajectoryObservationObstacle> obstacles,
|
|
long obstacleSnapshotVersion)
|
|
{
|
|
if (start == null) throw new ArgumentNullException(nameof(start));
|
|
if (goal == null) throw new ArgumentNullException(nameof(goal));
|
|
if (settings == null) throw new ArgumentNullException(nameof(settings));
|
|
if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
|
|
ValidatePose(start, nameof(start));
|
|
ValidatePose(goal, nameof(goal));
|
|
settings.Validate();
|
|
|
|
double padMm = settings.MapPaddingMeters * 1000d;
|
|
var bounds = new MapBoundsMm(
|
|
ToGridLower(Math.Min(start.X, goal.X) * 1000d - padMm, settings.MapResolutionMillimeters),
|
|
ToGridUpper(Math.Max(start.X, goal.X) * 1000d + padMm, settings.MapResolutionMillimeters),
|
|
ToGridLower(Math.Min(start.Y, goal.Y) * 1000d - padMm, settings.MapResolutionMillimeters),
|
|
ToGridUpper(Math.Max(start.Y, goal.Y) * 1000d + padMm, settings.MapResolutionMillimeters));
|
|
|
|
var mapObstacles = new List<IMapObstacle>(obstacles.Count);
|
|
for (int index = 0; index < obstacles.Count; index++)
|
|
{
|
|
TrajectoryObservationObstacle obstacle = obstacles[index] ?? throw new ArgumentNullException(nameof(obstacles));
|
|
MapBoundsMm obstacleBounds = obstacle.GetBounds();
|
|
if (obstacleBounds.XMin < bounds.XMin || obstacleBounds.XMax > bounds.XMax ||
|
|
obstacleBounds.YMin < bounds.YMin || obstacleBounds.YMax > bounds.YMax)
|
|
throw new ArgumentOutOfRangeException(nameof(obstacles), "Obstacle envelope must fit within map bounds.");
|
|
mapObstacles.Add(obstacle.ToMapObstacle());
|
|
}
|
|
|
|
IReadOnlyList<IMapObstacleSource> sources;
|
|
bool allowExplicitEmptyMap = mapObstacles.Count == 0;
|
|
if (allowExplicitEmptyMap)
|
|
sources = Array.Empty<IMapObstacleSource>();
|
|
else
|
|
{
|
|
if (obstacleSnapshotVersion <= 0L)
|
|
throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
|
|
sources = new IMapObstacleSource[]
|
|
{
|
|
new ManualObstacleSource("trajectory-observer-manual", obstacleSnapshotVersion, true, mapObstacles),
|
|
};
|
|
}
|
|
|
|
return new CoarsePathPlanningJob
|
|
{
|
|
MapRequest = new PlanningMapRequest
|
|
{
|
|
Bounds = bounds,
|
|
ResolutionMm = settings.MapResolutionMillimeters,
|
|
ObstacleSources = sources,
|
|
AllowExplicitEmptyMap = allowExplicitEmptyMap,
|
|
},
|
|
Start = start,
|
|
Goal = goal,
|
|
Vehicle = settings.CreateVehicle(),
|
|
Configuration = new HybridAStarConfiguration(),
|
|
StartDirection = null,
|
|
GoalDirection = GoalDirectionConstraint.Any,
|
|
};
|
|
}
|
|
|
|
private static float ToGridLower(double millimeters, float resolutionMm)
|
|
{
|
|
return ToFiniteFloat(Math.Floor(millimeters / resolutionMm) * resolutionMm, nameof(millimeters));
|
|
}
|
|
|
|
private static float ToGridUpper(double millimeters, float resolutionMm)
|
|
{
|
|
return ToFiniteFloat(Math.Ceiling(millimeters / resolutionMm) * resolutionMm, nameof(millimeters));
|
|
}
|
|
|
|
private static float ToFiniteFloat(double value, string parameterName)
|
|
{
|
|
if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
|
|
throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
|
|
return (float)value;
|
|
}
|
|
|
|
private static void ValidatePose(Pose2D pose, string parameterName)
|
|
{
|
|
if (double.IsNaN(pose.X) || double.IsInfinity(pose.X) || double.IsNaN(pose.Y) || double.IsInfinity(pose.Y) ||
|
|
double.IsNaN(pose.Heading) || double.IsInfinity(pose.Heading))
|
|
throw new ArgumentOutOfRangeException(parameterName, "Pose values must be finite.");
|
|
}
|
|
}
|