431 lines
14 KiB
C#
431 lines
14 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.Fleet;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Tests
|
|
{
|
|
internal static class FleetStateEstimatorTests
|
|
{
|
|
private const double Tolerance = 1e-9;
|
|
|
|
public static void Run()
|
|
{
|
|
VerifyRigidStateIsRecovered();
|
|
VerifyOlderSamplesAreAligned();
|
|
VerifyYawWrapAroundIsAveraged();
|
|
VerifySmallLayoutErrorIsReported();
|
|
VerifyInconsistentCentersAreRejected();
|
|
VerifyMissingMemberIsRejected();
|
|
VerifyInvalidVelocityRemainsExplicit();
|
|
|
|
Console.WriteLine(
|
|
"FleetStateEstimator车队状态估计测试通过。共7个场景。");
|
|
}
|
|
|
|
private static void VerifyRigidStateIsRecovered()
|
|
{
|
|
var layout = CreateLayout();
|
|
var fleetPose = new Pose2D(4.0, -2.0, 0.4);
|
|
var fleetTwist = new Twist2D(0.3, -0.1, 0.2);
|
|
var result = CreateEstimator().Estimate(
|
|
layout,
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
fleetPose,
|
|
fleetTwist,
|
|
sampleTimestampSeconds: 5.0,
|
|
hasValidVelocityEstimate: true),
|
|
targetTimestampSeconds: 5.0);
|
|
|
|
var state = RequireState(result, "刚体状态还原");
|
|
AssertPose(
|
|
state.FleetPoseInWorld,
|
|
fleetPose,
|
|
"刚体状态还原");
|
|
AssertTwist(
|
|
state.TwistAtFleetOriginInWorld,
|
|
fleetTwist,
|
|
"刚体速度还原");
|
|
|
|
for (var index = 0;
|
|
index < result.MemberErrors.Count;
|
|
index++)
|
|
{
|
|
AssertPose(
|
|
result.MemberErrors[index]
|
|
.ActualPoseInExpectedVehicleFrame,
|
|
Pose2D.Identity,
|
|
"刚体布局误差");
|
|
}
|
|
}
|
|
|
|
private static void VerifyOlderSamplesAreAligned()
|
|
{
|
|
var layout = CreateLayout();
|
|
var sampleFleetPose =
|
|
new Pose2D(1.0, 2.0, 0.3);
|
|
var fleetTwist =
|
|
new Twist2D(0.4, -0.2, 0.0);
|
|
var result = CreateEstimator().Estimate(
|
|
layout,
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
sampleFleetPose,
|
|
fleetTwist,
|
|
sampleTimestampSeconds: 0.9,
|
|
hasValidVelocityEstimate: true),
|
|
targetTimestampSeconds: 1.0);
|
|
|
|
var state = RequireState(result, "成员时间对齐");
|
|
AssertPose(
|
|
state.FleetPoseInWorld,
|
|
new Pose2D(1.04, 1.98, 0.3),
|
|
"成员时间对齐");
|
|
AssertTwist(
|
|
state.TwistAtFleetOriginInWorld,
|
|
fleetTwist,
|
|
"时间对齐后速度");
|
|
}
|
|
|
|
private static void VerifySmallLayoutErrorIsReported()
|
|
{
|
|
var layout = CreateLayout();
|
|
var result = CreateEstimator().Estimate(
|
|
layout,
|
|
new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
new Pose2D(-0.98, 0.0, 0.0),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true),
|
|
CreateMemberState(
|
|
2,
|
|
new Pose2D(0.99, 0.0, Math.PI),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true)
|
|
},
|
|
targetTimestampSeconds: 1.0);
|
|
|
|
var state = RequireState(result, "小范围布局误差");
|
|
AssertNear(
|
|
state.FleetPoseInWorld.XMeters,
|
|
0.005,
|
|
"小范围布局误差中心X");
|
|
AssertNear(
|
|
FindError(result.MemberErrors, 1)
|
|
.ActualPoseInExpectedVehicleFrame.XMeters,
|
|
0.015,
|
|
"车辆1布局误差X");
|
|
AssertNear(
|
|
FindError(result.MemberErrors, 2)
|
|
.ActualPoseInExpectedVehicleFrame.XMeters,
|
|
0.015,
|
|
"车辆2布局误差X");
|
|
}
|
|
|
|
private static void VerifyYawWrapAroundIsAveraged()
|
|
{
|
|
var layout = CreateLayout();
|
|
var firstCandidate = new Pose2D(
|
|
0.0,
|
|
0.0,
|
|
AngleMath.DegreesToRadians(179.0));
|
|
var secondCandidate = new Pose2D(
|
|
0.0,
|
|
0.0,
|
|
AngleMath.DegreesToRadians(-179.0));
|
|
var result = CreateEstimator().Estimate(
|
|
layout,
|
|
new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
FrameTransform2D.Compose(
|
|
firstCandidate,
|
|
layout.Vehicles[0].PoseInFleet),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true),
|
|
CreateMemberState(
|
|
2,
|
|
FrameTransform2D.Compose(
|
|
secondCandidate,
|
|
layout.Vehicles[1].PoseInFleet),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true)
|
|
},
|
|
targetTimestampSeconds: 1.0);
|
|
|
|
var state = RequireState(result, "跨正负π航向平均");
|
|
AssertNear(
|
|
Math.Abs(state.FleetPoseInWorld.YawRadians),
|
|
Math.PI,
|
|
"跨正负π航向平均");
|
|
}
|
|
|
|
private static void VerifyInconsistentCentersAreRejected()
|
|
{
|
|
var result = CreateEstimator().Estimate(
|
|
CreateLayout(),
|
|
new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
new Pose2D(-1.0, 0.0, 0.0),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true),
|
|
CreateMemberState(
|
|
2,
|
|
new Pose2D(1.3, 0.0, Math.PI),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true)
|
|
},
|
|
targetTimestampSeconds: 1.0);
|
|
|
|
AssertUnavailable(result, "候选中心冲突");
|
|
}
|
|
|
|
private static void VerifyMissingMemberIsRejected()
|
|
{
|
|
var result = CreateEstimator().Estimate(
|
|
CreateLayout(),
|
|
new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
new Pose2D(-1.0, 0.0, 0.0),
|
|
Twist2D.Zero,
|
|
1.0,
|
|
true)
|
|
},
|
|
targetTimestampSeconds: 1.0);
|
|
|
|
AssertUnavailable(result, "成员缺失");
|
|
}
|
|
|
|
private static void VerifyInvalidVelocityRemainsExplicit()
|
|
{
|
|
var layout = CreateLayout();
|
|
var result = CreateEstimator().Estimate(
|
|
layout,
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
Pose2D.Identity,
|
|
new Twist2D(0.4, 0.0, 0.0),
|
|
sampleTimestampSeconds: 1.0,
|
|
hasValidVelocityEstimate: false),
|
|
targetTimestampSeconds: 1.0);
|
|
|
|
var state = RequireState(result, "速度未初始化");
|
|
if (state.HasValidVelocityEstimate)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"成员速度无效时车队速度不应标记为有效。");
|
|
}
|
|
|
|
AssertTwist(
|
|
state.TwistAtFleetOriginInWorld,
|
|
Twist2D.Zero,
|
|
"速度未初始化");
|
|
}
|
|
|
|
private static FleetStateEstimator CreateEstimator()
|
|
{
|
|
return new FleetStateEstimator(
|
|
maximumMemberStateAgeSeconds: 0.25,
|
|
maximumPositionDisagreementMeters: 0.1,
|
|
maximumYawDisagreementRadians:
|
|
AngleMath.DegreesToRadians(5.0));
|
|
}
|
|
|
|
private static FleetLayout CreateLayout()
|
|
{
|
|
return new FleetLayout(
|
|
new[]
|
|
{
|
|
new VehicleLayout(
|
|
1,
|
|
new Pose2D(-1.0, 0.0, 0.0)),
|
|
new VehicleLayout(
|
|
2,
|
|
new Pose2D(1.0, 0.0, Math.PI))
|
|
});
|
|
}
|
|
|
|
private static FleetMemberStateSample[]
|
|
CreateRigidMemberStates(
|
|
FleetLayout layout,
|
|
Pose2D fleetPoseInWorld,
|
|
Twist2D twistAtFleetOriginInWorld,
|
|
double sampleTimestampSeconds,
|
|
bool hasValidVelocityEstimate)
|
|
{
|
|
var states =
|
|
new FleetMemberStateSample[layout.VehicleCount];
|
|
|
|
for (var index = 0;
|
|
index < layout.Vehicles.Count;
|
|
index++)
|
|
{
|
|
var vehicle = layout.Vehicles[index];
|
|
var memberPoseInWorld =
|
|
FrameTransform2D.Compose(
|
|
fleetPoseInWorld,
|
|
vehicle.PoseInFleet);
|
|
var xFromFleetOrigin =
|
|
memberPoseInWorld.XMeters -
|
|
fleetPoseInWorld.XMeters;
|
|
var yFromFleetOrigin =
|
|
memberPoseInWorld.YMeters -
|
|
fleetPoseInWorld.YMeters;
|
|
var memberTwistInWorld = new Twist2D(
|
|
twistAtFleetOriginInWorld
|
|
.VxMetersPerSecond -
|
|
twistAtFleetOriginInWorld
|
|
.OmegaRadiansPerSecond *
|
|
yFromFleetOrigin,
|
|
twistAtFleetOriginInWorld
|
|
.VyMetersPerSecond +
|
|
twistAtFleetOriginInWorld
|
|
.OmegaRadiansPerSecond *
|
|
xFromFleetOrigin,
|
|
twistAtFleetOriginInWorld
|
|
.OmegaRadiansPerSecond);
|
|
|
|
states[index] = new FleetMemberStateSample(
|
|
vehicle.VehicleId,
|
|
sampleTimestampSeconds,
|
|
memberPoseInWorld,
|
|
memberTwistInWorld,
|
|
isStateAvailable: true,
|
|
hasValidVelocityEstimate:
|
|
hasValidVelocityEstimate);
|
|
}
|
|
|
|
return states;
|
|
}
|
|
|
|
private static FleetMemberStateSample CreateMemberState(
|
|
int vehicleId,
|
|
Pose2D poseInWorld,
|
|
Twist2D twistInWorld,
|
|
double timestampSeconds,
|
|
bool hasValidVelocityEstimate)
|
|
{
|
|
return new FleetMemberStateSample(
|
|
vehicleId,
|
|
timestampSeconds,
|
|
poseInWorld,
|
|
twistInWorld,
|
|
isStateAvailable: true,
|
|
hasValidVelocityEstimate:
|
|
hasValidVelocityEstimate);
|
|
}
|
|
|
|
private static FleetState RequireState(
|
|
FleetStateEstimateResult result,
|
|
string scenario)
|
|
{
|
|
if (!result.IsAvailable || !result.State.HasValue)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"{scenario}应产生可用状态：" +
|
|
result.UnavailableReason);
|
|
}
|
|
|
|
return result.State.Value;
|
|
}
|
|
|
|
private static void AssertUnavailable(
|
|
FleetStateEstimateResult result,
|
|
string scenario)
|
|
{
|
|
if (result.IsAvailable ||
|
|
string.IsNullOrWhiteSpace(
|
|
result.UnavailableReason))
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"{scenario}应返回带原因的不可用结果。");
|
|
}
|
|
}
|
|
|
|
private static FleetMemberLayoutError FindError(
|
|
IReadOnlyList<FleetMemberLayoutError> errors,
|
|
int vehicleId)
|
|
{
|
|
for (var index = 0;
|
|
index < errors.Count;
|
|
index++)
|
|
{
|
|
if (errors[index].VehicleId == vehicleId)
|
|
{
|
|
return errors[index];
|
|
}
|
|
}
|
|
|
|
throw new InvalidOperationException(
|
|
$"没有找到车辆{vehicleId}的布局误差。");
|
|
}
|
|
|
|
private static void AssertPose(
|
|
Pose2D actual,
|
|
Pose2D expected,
|
|
string scenario)
|
|
{
|
|
AssertNear(
|
|
actual.XMeters,
|
|
expected.XMeters,
|
|
scenario + " X");
|
|
AssertNear(
|
|
actual.YMeters,
|
|
expected.YMeters,
|
|
scenario + " Y");
|
|
AssertNear(
|
|
AngleMath.ShortestDifferenceRadians(
|
|
actual.YawRadians,
|
|
expected.YawRadians),
|
|
0.0,
|
|
scenario + " Yaw");
|
|
}
|
|
|
|
private static void AssertTwist(
|
|
Twist2D actual,
|
|
Twist2D expected,
|
|
string scenario)
|
|
{
|
|
AssertNear(
|
|
actual.VxMetersPerSecond,
|
|
expected.VxMetersPerSecond,
|
|
scenario + " Vx");
|
|
AssertNear(
|
|
actual.VyMetersPerSecond,
|
|
expected.VyMetersPerSecond,
|
|
scenario + " Vy");
|
|
AssertNear(
|
|
actual.OmegaRadiansPerSecond,
|
|
expected.OmegaRadiansPerSecond,
|
|
scenario + " Omega");
|
|
}
|
|
|
|
private static void AssertNear(
|
|
double actual,
|
|
double expected,
|
|
string valueName)
|
|
{
|
|
if (Math.Abs(actual - expected) > Tolerance)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"{valueName}错误：" +
|
|
$"actual={actual:F9}, expected={expected:F9}。");
|
|
}
|
|
}
|
|
}
|
|
}
|