增加车队轨迹控制核心与轮组自转模式
This commit is contained in:
@@ -0,0 +1,295 @@
|
||||
using System;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Fleet;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetControllerTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyInactiveControllerStops();
|
||||
VerifyStraightCommand();
|
||||
VerifyInvalidVelocityUsesReferenceSpeed();
|
||||
VerifyCompletionStops();
|
||||
VerifyExcessiveTrackingErrorFaults();
|
||||
VerifyCancelStops();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetController车队中心控制测试通过。共6个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyInactiveControllerStops()
|
||||
{
|
||||
var controller = CreateController();
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(0.5, 0.0, 0.0, true),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.Inactive,
|
||||
"未启动控制器");
|
||||
AssertStop(command, "未启动控制器");
|
||||
}
|
||||
|
||||
private static void VerifyStraightCommand()
|
||||
{
|
||||
var controller = CreateController();
|
||||
controller.Start(CreateStraightTrajectory());
|
||||
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(0.5, 0.0, 0.4, true),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.CommandGenerated,
|
||||
"直线控制");
|
||||
AssertNear(
|
||||
command.ReferencePointInFleet.XMeters,
|
||||
0.0,
|
||||
"直线控制参考点X");
|
||||
AssertNear(
|
||||
command.ReferencePointInFleet.YMeters,
|
||||
0.0,
|
||||
"直线控制参考点Y");
|
||||
AssertTwist(
|
||||
command.TwistAtReferencePoint,
|
||||
0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"直线控制");
|
||||
}
|
||||
|
||||
private static void VerifyInvalidVelocityUsesReferenceSpeed()
|
||||
{
|
||||
var controller = CreateController();
|
||||
controller.Start(CreateStraightTrajectory());
|
||||
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(0.5, 0.0, 0.0, false),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.CommandGenerated,
|
||||
"速度尚未初始化");
|
||||
AssertTwist(
|
||||
command.TwistAtReferencePoint,
|
||||
0.4,
|
||||
0.0,
|
||||
0.0,
|
||||
"速度尚未初始化");
|
||||
}
|
||||
|
||||
private static void VerifyCompletionStops()
|
||||
{
|
||||
var controller = CreateController();
|
||||
controller.Start(CreateStraightTrajectory());
|
||||
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(1.0, 0.0, 0.0, true),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.Completed,
|
||||
"终点完成");
|
||||
AssertStop(command, "终点完成");
|
||||
|
||||
if (controller.IsActive || !controller.IsCompleted)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"终点完成后控制器状态错误。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyExcessiveTrackingErrorFaults()
|
||||
{
|
||||
var controller = CreateController();
|
||||
controller.Start(CreateStraightTrajectory());
|
||||
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(0.5, 0.5, 0.0, true),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.Faulted,
|
||||
"轨迹偏离保护");
|
||||
AssertStop(command, "轨迹偏离保护");
|
||||
|
||||
if (string.IsNullOrWhiteSpace(
|
||||
controller.LastFailureReason))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"轨迹偏离故障没有保存原因。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyCancelStops()
|
||||
{
|
||||
var controller = CreateController();
|
||||
controller.Start(CreateStraightTrajectory());
|
||||
controller.Cancel();
|
||||
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(0.5, 0.0, 0.0, true),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.Inactive,
|
||||
"取消控制");
|
||||
AssertStop(command, "取消控制");
|
||||
}
|
||||
|
||||
private static FleetController CreateController()
|
||||
{
|
||||
return new FleetController(
|
||||
new StraightLateralController(),
|
||||
new ReferenceLongitudinalController(),
|
||||
new GcpCommandAllocator(
|
||||
Math.PI / 4.0),
|
||||
virtualControlPointRadiusMeters: 0.5);
|
||||
}
|
||||
|
||||
private static Trajectory2D CreateStraightTrajectory()
|
||||
{
|
||||
return new Trajectory2D(
|
||||
new[]
|
||||
{
|
||||
new TrajectoryPoint(
|
||||
0.0,
|
||||
Pose2D.Identity,
|
||||
0.0,
|
||||
0.4),
|
||||
new TrajectoryPoint(
|
||||
1.0,
|
||||
new Pose2D(1.0, 0.0, 0.0),
|
||||
0.0,
|
||||
0.4)
|
||||
});
|
||||
}
|
||||
|
||||
private static FleetState CreateState(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double vxMetersPerSecond,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
return new FleetState(
|
||||
sampleTimestampSeconds: 1.0,
|
||||
fleetPoseInWorld: new Pose2D(
|
||||
xMeters,
|
||||
yMeters,
|
||||
0.0),
|
||||
twistAtFleetOriginInWorld: new Twist2D(
|
||||
vxMetersPerSecond,
|
||||
0.0,
|
||||
0.0),
|
||||
hasValidVelocityEstimate:
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
private static void AssertResult(
|
||||
FleetControlCycleResult actual,
|
||||
FleetControlCycleResult expected,
|
||||
string scenario)
|
||||
{
|
||||
if (actual != expected)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}结果错误：" +
|
||||
$"actual={actual}, expected={expected}。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertStop(
|
||||
FleetMotionCommand command,
|
||||
string scenario)
|
||||
{
|
||||
AssertTwist(
|
||||
command.TwistAtReferencePoint,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
scenario);
|
||||
}
|
||||
|
||||
private static void AssertTwist(
|
||||
Twist2D actual,
|
||||
double expectedVx,
|
||||
double expectedVy,
|
||||
double expectedOmega,
|
||||
string scenario)
|
||||
{
|
||||
AssertNear(
|
||||
actual.VxMetersPerSecond,
|
||||
expectedVx,
|
||||
$"{scenario} Vx");
|
||||
AssertNear(
|
||||
actual.VyMetersPerSecond,
|
||||
expectedVy,
|
||||
$"{scenario} Vy");
|
||||
AssertNear(
|
||||
actual.OmegaRadiansPerSecond,
|
||||
expectedOmega,
|
||||
$"{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}。");
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class StraightLateralController :
|
||||
ILateralController
|
||||
{
|
||||
public LateralControlCommand Compute(
|
||||
PathTrackingContext context)
|
||||
{
|
||||
return LateralControlCommand.Straight;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class ReferenceLongitudinalController :
|
||||
ILongitudinalController
|
||||
{
|
||||
public double ComputeSpeedMetersPerSecond(
|
||||
PathTrackingContext context)
|
||||
{
|
||||
return context
|
||||
.ControlReferenceSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user