48 lines
1.9 KiB
C#
48 lines
1.9 KiB
C#
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; }
|
|
}
|
|
}
|