feat: add observation test map inputs
This commit is contained in:
梁薄云
+212
@@ -0,0 +1,212 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
public sealed class TrajectoryObservationSettings
|
||||
{
|
||||
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 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 void Validate()
|
||||
{
|
||||
EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
|
||||
EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters));
|
||||
EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds));
|
||||
EnsurePositiveFinite(ObserverPeriodSeconds, nameof(ObserverPeriodSeconds));
|
||||
EnsurePositiveFinite(VehicleLengthMeters, nameof(VehicleLengthMeters));
|
||||
EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters));
|
||||
EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters));
|
||||
EnsurePositiveFinite(MaximumCurvaturePerMeter, nameof(MaximumCurvaturePerMeter));
|
||||
}
|
||||
|
||||
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.");
|
||||
}
|
||||
}
|
||||
@@ -13,9 +13,9 @@ internal static class Program
|
||||
args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
|
||||
args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
|
||||
args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" &&
|
||||
args[0] != "executor" && args[0] != "plugin-package" && args[0] != "em-all"))
|
||||
args[0] != "executor" && args[0] != "plugin-package" && args[0] != "trajectory-observation" && args[0] != "em-all"))
|
||||
{
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration|em-core-all|coordinator|executor|plugin-package|em-all");
|
||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration|em-core-all|coordinator|executor|plugin-package|trajectory-observation|em-all");
|
||||
return 2;
|
||||
}
|
||||
|
||||
@@ -124,6 +124,11 @@ internal static class Program
|
||||
PluginPackagingChecks.Run();
|
||||
Console.WriteLine("PASS plugin-package");
|
||||
}
|
||||
if (args[0] == "trajectory-observation" || args[0] == "em-all")
|
||||
{
|
||||
TrajectoryObservationChecks.Run();
|
||||
Console.WriteLine("PASS trajectory-observation");
|
||||
}
|
||||
if (args[0] == "em-all")
|
||||
{
|
||||
CoordinatorChecks.RunRollingEndToEnd();
|
||||
|
||||
@@ -0,0 +1,47 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||||
|
||||
namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class TrajectoryObservationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
|
||||
RejectsObstacleOutsideConfiguredBounds();
|
||||
}
|
||||
|
||||
private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding()
|
||||
{
|
||||
var settings = new TrajectoryObservationSettings
|
||||
{
|
||||
MapPaddingMeters = 2d,
|
||||
MapResolutionMillimeters = 50f,
|
||||
};
|
||||
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||
new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings,
|
||||
Array.Empty<TrajectoryObservationObstacle>(), 17L);
|
||||
|
||||
Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min");
|
||||
Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max");
|
||||
Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min");
|
||||
Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max");
|
||||
Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution");
|
||||
}
|
||||
|
||||
private static void RejectsObstacleOutsideConfiguredBounds()
|
||||
{
|
||||
Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob(
|
||||
new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(),
|
||||
new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)),
|
||||
"observer obstacle outside configured bounds");
|
||||
}
|
||||
|
||||
private static bool Throws(Action action)
|
||||
{
|
||||
try { action(); return false; }
|
||||
catch (ArgumentOutOfRangeException) { return true; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user