52 lines
2.5 KiB
C#
52 lines
2.5 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class FoundationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
var reverseState = new VehicleMotionState(
|
|
new Pose2D(1.5d, -2d, 0.25d),
|
|
-0.15d,
|
|
-0.03d,
|
|
new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero),
|
|
17L);
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond,
|
|
"reverse signed speed");
|
|
|
|
var point = new EmTrajectoryPoint(
|
|
1.25d,
|
|
-0.75d,
|
|
0.5d,
|
|
-0.12d,
|
|
0.4d,
|
|
-0.2d,
|
|
3,
|
|
0.6d,
|
|
0.8d,
|
|
TravelDirection.Reverse,
|
|
EmBoundaryType.GearSwitchApproach,
|
|
-0.04d,
|
|
0.03d);
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature");
|
|
EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s");
|
|
EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction");
|
|
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
|
|
}
|
|
}
|