525 lines
20 KiB
C#
525 lines
20 KiB
C#
|
||
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using MDCSToolBox.Commons.Controllers;
|
||
using MyParking.Shared;
|
||
using MultiWheelC.StateEstimation;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
/// <summary>
|
||
/// 将四个舵轮准备到自转姿态并按世界航向闭环旋转，正常完成后等待舵轮回正。
|
||
/// </summary>
|
||
public class MultiWheelRotateInPlace : MovementDefinition
|
||
{
|
||
/// <summary>
|
||
/// 旋转目标角度
|
||
/// </summary>
|
||
public float AngleTarget;
|
||
|
||
// 留作标定或单元测试时显式替换；为空时使用配置化Detour与电机反馈组合状态源。
|
||
public Func<float> ThetaReader;
|
||
|
||
public IVehicleStateProvider StateProvider;
|
||
|
||
public MultiWheelChassis Chassis =
|
||
PilotDefinition.Chassis as MultiWheelChassis;
|
||
|
||
/// <summary>
|
||
/// 获取或设置本次动作的PID参数读取覆盖；为空时读取车辆配置。
|
||
/// </summary>
|
||
public Func<PIDParams> PidparamsRead;
|
||
|
||
public PIDController thPid;
|
||
|
||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||
public Action<float> CommandAngularSpeedObserver;
|
||
|
||
// 自转前舵轮实际角度允许误差覆盖值，单位deg；为空时读取车辆配置。
|
||
public float? WheelAlignmentToleranceDegrees;
|
||
|
||
// 自转舵轮连续保持到位的时间，单位s。
|
||
public float WheelAlignmentStableSeconds = 0.3f;
|
||
|
||
// 自转舵轮准备超时时间，单位s。
|
||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||
|
||
// 航向尚未到位时允许下发的最小有效角速度覆盖值，单位deg/s；为空时读取车辆配置。
|
||
public float? MinimumAngularSpeedDegreesPerSecond;
|
||
|
||
// 舵轮到位后执行航向闭环允许的最长时间覆盖值，单位s；为空时读取车辆配置。
|
||
public float? RotationTimeoutSeconds;
|
||
|
||
/// <summary>
|
||
/// 读取一次有效配置，闭环旋转到目标航向并在正常完成后等待舵轮稳定回正。
|
||
/// </summary>
|
||
public override IEnumerable<bool> Get()
|
||
{
|
||
if (Chassis == null)
|
||
throw new InvalidOperationException(
|
||
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
|
||
|
||
var config = PilotDefinition.Conf;
|
||
var pidParameters =
|
||
PidparamsRead == null
|
||
? new PIDParams
|
||
{
|
||
Kp = config.InPlaceRotateKp,
|
||
Ki = config.InPlaceRotateKi,
|
||
Kd = config.InPlaceRotateKd,
|
||
MaxI = config.InPlaceRotateMaxI,
|
||
DeadZone = config.InPlaceRotateArriveDeg,
|
||
SpeedAccPerSec = config.InPlaceRotateAcc,
|
||
OutputUpperThreshold =
|
||
config.InPlaceRotateMaxSpeed
|
||
}
|
||
: PidparamsRead();
|
||
var wheelAlignmentToleranceDegrees =
|
||
WheelAlignmentToleranceDegrees ??
|
||
config.InPlaceRotateWheelAlignDeg;
|
||
var minimumAngularSpeedDegreesPerSecond =
|
||
MinimumAngularSpeedDegreesPerSecond ??
|
||
config.InPlaceRotateMinimumSpeed;
|
||
var rotationTimeoutSeconds =
|
||
RotationTimeoutSeconds ??
|
||
config.InPlaceRotateTimeoutSec;
|
||
var stateProvider =
|
||
StateProvider ??
|
||
ParkingVehicleStateProviderFactory.Create(
|
||
Chassis,
|
||
config);
|
||
|
||
ValidateParameters(
|
||
pidParameters,
|
||
wheelAlignmentToleranceDegrees,
|
||
minimumAngularSpeedDegreesPerSecond,
|
||
rotationTimeoutSeconds);
|
||
|
||
var adapter = new MultiWheelChassisAdapter(
|
||
Chassis,
|
||
PilotDefinition.Self.CarNum);
|
||
adapter.ResetToBodyFrame();
|
||
|
||
try
|
||
{
|
||
var alignmentStarted = DateTime.Now;
|
||
DateTime? alignedSince = null;
|
||
while (true)
|
||
{
|
||
if (!adapter.PrepareSpin(
|
||
alignmentToleranceDegrees:
|
||
wheelAlignmentToleranceDegrees))
|
||
throw new InvalidOperationException(
|
||
"无法生成原地自转舵轮目标：" +
|
||
adapter.LastFailureReason);
|
||
|
||
if (adapter.AreSpinWheelsAligned)
|
||
{
|
||
if (alignedSince == null)
|
||
alignedSince = DateTime.Now;
|
||
|
||
if ((DateTime.Now - alignedSince.Value)
|
||
.TotalSeconds >=
|
||
WheelAlignmentStableSeconds)
|
||
break;
|
||
}
|
||
else
|
||
{
|
||
alignedSince = null;
|
||
}
|
||
|
||
if ((DateTime.Now - alignmentStarted)
|
||
.TotalSeconds >
|
||
WheelAlignmentTimeoutSeconds)
|
||
throw new TimeoutException(
|
||
"原地自转舵轮在限定时间内未稳定到位。");
|
||
|
||
yield return true;
|
||
}
|
||
|
||
var alignmentToleranceRadians =
|
||
AngleMath.DegreesToRadians(
|
||
wheelAlignmentToleranceDegrees);
|
||
if (!adapter.AdoptPreparedSpinForXYTh(
|
||
alignmentToleranceRadians))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法将已到位的自转舵角交接给XYTh：" +
|
||
adapter.LastFailureReason);
|
||
}
|
||
|
||
var targetAngle =
|
||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||
var currentAngle =
|
||
ReadCurrentAngleDegrees(stateProvider);
|
||
var cachedCurrentAngle = currentAngle;
|
||
thPid = new PIDController(
|
||
() => cachedCurrentAngle,
|
||
pidParameters.Kp);
|
||
thPid.ChangeParameters(
|
||
pidParameters.Kp,
|
||
pidParameters.Ki,
|
||
pidParameters.Kd,
|
||
pidParameters.MaxI,
|
||
pidParameters.DeadZone,
|
||
pidParameters.OutputUpperThreshold,
|
||
pidParameters.SpeedAccPerSec);
|
||
var lastCommandTime = DateTime.Now;
|
||
var rotationStarted = DateTime.Now;
|
||
|
||
while (true)
|
||
{
|
||
if ((DateTime.Now - rotationStarted)
|
||
.TotalSeconds >
|
||
rotationTimeoutSeconds)
|
||
{
|
||
throw new TimeoutException(
|
||
$"原地自转超过{rotationTimeoutSeconds:F1}s仍未到位。");
|
||
}
|
||
|
||
currentAngle =
|
||
ReadCurrentAngleDegrees(stateProvider);
|
||
cachedCurrentAngle = currentAngle;
|
||
var s = thPid.GetResponse(targetAngle, true);
|
||
var angleErrorDegrees =
|
||
(float)AngleMath
|
||
.ShortestDifferenceDegrees(
|
||
targetAngle,
|
||
currentAngle);
|
||
|
||
// PID进入到位死区后等待其0.3s稳定确认；等待期间
|
||
// 只清零驱动速度，不清除已经准备好的自转舵角状态。
|
||
if (Math.Abs(angleErrorDegrees) <=
|
||
pidParameters.DeadZone)
|
||
{
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter
|
||
.StopXYThDrivePreserveSteeringState();
|
||
|
||
// 到位稳定只依赖已经单独校验的航向。
|
||
// 位置候选留到停车后处理，避免位置抖动中断航向闭环。
|
||
if (thPid.IsArrived())
|
||
break;
|
||
|
||
yield return true;
|
||
continue;
|
||
}
|
||
|
||
// PID输出低于底盘有效轮速范围时提高到最小可执行值，
|
||
// 避免接近目标时反复出现微小命令但车辆实际不动。
|
||
if (Math.Abs(s) > 1e-6f &&
|
||
Math.Abs(s) <
|
||
minimumAngularSpeedDegreesPerSecond)
|
||
{
|
||
s = Math.Sign(angleErrorDegrees) *
|
||
minimumAngularSpeedDegreesPerSecond;
|
||
}
|
||
|
||
// PID加速限制在首周期可能暂时输出零；此时保留
|
||
// 已交接的自转状态，等待下一周期产生有效角速度。
|
||
if (Math.Abs(s) <= 1e-6f)
|
||
{
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter
|
||
.StopXYThDrivePreserveSteeringState();
|
||
yield return true;
|
||
continue;
|
||
}
|
||
|
||
CommandAngularSpeedObserver?.Invoke(s);
|
||
var now = DateTime.Now;
|
||
var interval = now - lastCommandTime;
|
||
lastCommandTime = now;
|
||
|
||
// PID输出s为deg/s，Shared统一使用车体坐标系Twist2D和rad/s。
|
||
var omegaRadiansPerSecond =
|
||
(float)AngleMath.DegreesToRadians(s);
|
||
if (!adapter.SendBodyTwist(
|
||
new Twist2D(
|
||
0.0,
|
||
0.0,
|
||
omegaRadiansPerSecond),
|
||
interval))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"安全XYTh原地旋转底盘解算失败：" +
|
||
adapter.LastFailureReason);
|
||
}
|
||
yield return true;
|
||
}
|
||
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter.StopXYThDrivePreserveSteeringState();
|
||
|
||
if (IsLocalizationRecoveryPending(
|
||
stateProvider))
|
||
{
|
||
BeginPostRotationPositionRecovery(
|
||
stateProvider);
|
||
var recoveryStarted = DateTime.Now;
|
||
|
||
while (true)
|
||
{
|
||
adapter
|
||
.StopXYThDrivePreserveSteeringState();
|
||
|
||
if (stateProvider.TryGetState(out _) &&
|
||
!IsLocalizationRecoveryPending(
|
||
stateProvider))
|
||
{
|
||
break;
|
||
}
|
||
|
||
if ((DateTime.Now - recoveryStarted)
|
||
.TotalSeconds >
|
||
config
|
||
.ParkingDetourJumpConfirmationTimeoutSeconds)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"原地自转完成后Detour位置在限定时间内未恢复。" +
|
||
GetStateProviderFailureReason(
|
||
stateProvider));
|
||
}
|
||
|
||
yield return true;
|
||
}
|
||
}
|
||
|
||
// 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
|
||
var wheelPreparation =
|
||
new PrepareWheelsForward();
|
||
foreach (var keepRunning in wheelPreparation.Get())
|
||
{
|
||
if (!keepRunning)
|
||
{
|
||
break;
|
||
}
|
||
|
||
yield return true;
|
||
}
|
||
|
||
if (!wheelPreparation.Completed)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"原地自转完成后舵轮未能稳定回到车头方向。");
|
||
}
|
||
|
||
Console.WriteLine(
|
||
$"final rotate to {targetAngle}, wheels forward");
|
||
}
|
||
finally
|
||
{
|
||
CommandAngularSpeedObserver?.Invoke(0f);
|
||
adapter.StopImmediately();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查原地自转的舵轮准备、最小速度和超时参数是否可执行。
|
||
/// </summary>
|
||
private void ValidateParameters(
|
||
PIDParams pidParameters,
|
||
float wheelAlignmentToleranceDegrees,
|
||
float minimumAngularSpeedDegreesPerSecond,
|
||
float rotationTimeoutSeconds)
|
||
{
|
||
EnsureFinitePositive(
|
||
wheelAlignmentToleranceDegrees,
|
||
nameof(WheelAlignmentToleranceDegrees),
|
||
allowZero: true);
|
||
EnsureFinitePositive(
|
||
WheelAlignmentStableSeconds,
|
||
nameof(WheelAlignmentStableSeconds),
|
||
allowZero: true);
|
||
EnsureFinitePositive(
|
||
WheelAlignmentTimeoutSeconds,
|
||
nameof(WheelAlignmentTimeoutSeconds));
|
||
EnsureFinitePositive(
|
||
minimumAngularSpeedDegreesPerSecond,
|
||
nameof(MinimumAngularSpeedDegreesPerSecond));
|
||
EnsureFinitePositive(
|
||
rotationTimeoutSeconds,
|
||
nameof(RotationTimeoutSeconds));
|
||
|
||
if (pidParameters == null)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"原地自转PID参数读取结果为空。");
|
||
}
|
||
|
||
EnsureFinitePositive(
|
||
pidParameters.DeadZone,
|
||
"PidparamsRead.DeadZone");
|
||
EnsureFinitePositive(
|
||
pidParameters.OutputUpperThreshold,
|
||
"PidparamsRead.OutputUpperThreshold");
|
||
EnsureFinitePositive(
|
||
pidParameters.SpeedAccPerSec,
|
||
"PidparamsRead.SpeedAccPerSec");
|
||
EnsureFinitePositive(
|
||
pidParameters.Kp,
|
||
"PidparamsRead.Kp");
|
||
|
||
if (minimumAngularSpeedDegreesPerSecond >
|
||
pidParameters.OutputUpperThreshold)
|
||
{
|
||
throw new InvalidOperationException(
|
||
"原地自转最小有效角速度不能大于最大角速度。");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 读取经过状态源校验的世界航向，显式设置ThetaReader时优先使用替代读数。
|
||
/// </summary>
|
||
private float ReadCurrentAngleDegrees(
|
||
IVehicleStateProvider stateProvider)
|
||
{
|
||
if (ThetaReader != null)
|
||
{
|
||
var angleDegrees = ThetaReader();
|
||
if (float.IsNaN(angleDegrees) ||
|
||
float.IsInfinity(angleDegrees))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"自定义航向读取结果不是有效角度。");
|
||
}
|
||
|
||
return (float)AngleMath.NormalizeDegrees(
|
||
angleDegrees);
|
||
}
|
||
|
||
if (stateProvider is
|
||
WheelFeedbackVehicleStateProvider wheelProvider)
|
||
{
|
||
if (!wheelProvider.TryGetHeadingRadians(
|
||
out var wheelHeadingRadians))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法从Detour状态源读取有效车辆航向。" +
|
||
wheelProvider.LastHeadingFailureReason);
|
||
}
|
||
|
||
return (float)AngleMath.RadiansToDegrees(
|
||
wheelHeadingRadians);
|
||
}
|
||
|
||
if (stateProvider is
|
||
DetourVehicleStateProvider detourProvider)
|
||
{
|
||
if (!detourProvider.TryGetHeadingRadians(
|
||
out var detourHeadingRadians))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法从Detour状态源读取有效车辆航向。" +
|
||
detourProvider.LastHeadingFailureReason);
|
||
}
|
||
|
||
return (float)AngleMath.RadiansToDegrees(
|
||
detourHeadingRadians);
|
||
}
|
||
|
||
if (stateProvider == null ||
|
||
!stateProvider.TryGetState(out var state))
|
||
{
|
||
throw new InvalidOperationException(
|
||
"无法从Detour状态源读取有效车辆航向。" +
|
||
GetStateProviderFailureReason(
|
||
stateProvider));
|
||
}
|
||
|
||
return (float)AngleMath.RadiansToDegrees(
|
||
state.PoseInWorld.YawRadians);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 通知配置化状态源：车辆已经停车，可以重新确认旋转期间的位置候选。
|
||
/// </summary>
|
||
private static void BeginPostRotationPositionRecovery(
|
||
IVehicleStateProvider stateProvider)
|
||
{
|
||
if (stateProvider is
|
||
WheelFeedbackVehicleStateProvider wheelProvider)
|
||
{
|
||
wheelProvider.BeginPostRotationPositionRecovery();
|
||
return;
|
||
}
|
||
|
||
if (stateProvider is
|
||
DetourVehicleStateProvider detourProvider)
|
||
{
|
||
detourProvider.BeginPostRotationPositionRecovery();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
|
||
/// </summary>
|
||
private static string GetStateProviderFailureReason(
|
||
IVehicleStateProvider stateProvider)
|
||
{
|
||
if (stateProvider is
|
||
WheelFeedbackVehicleStateProvider wheelProvider)
|
||
{
|
||
return wheelProvider.LastFailureReason;
|
||
}
|
||
|
||
if (stateProvider is
|
||
DetourVehicleStateProvider detourProvider)
|
||
{
|
||
return detourProvider.LastFailureReason;
|
||
}
|
||
|
||
return string.Empty;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断Detour是否仍在使用轮组预测确认疑似位姿不连续。
|
||
/// </summary>
|
||
private static bool IsLocalizationRecoveryPending(
|
||
IVehicleStateProvider stateProvider)
|
||
{
|
||
if (stateProvider is
|
||
WheelFeedbackVehicleStateProvider wheelProvider)
|
||
{
|
||
return wheelProvider.TryGetLatestDetourDiagnostics(
|
||
out var jumpCandidateActive,
|
||
out _,
|
||
out _,
|
||
out _,
|
||
out _,
|
||
out _,
|
||
out _) &&
|
||
jumpCandidateActive;
|
||
}
|
||
|
||
return stateProvider is
|
||
DetourVehicleStateProvider detourProvider &&
|
||
detourProvider.IsJumpCandidateActive;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
|
||
/// </summary>
|
||
private static void EnsureFinitePositive(
|
||
float value,
|
||
string parameterName,
|
||
bool allowZero = false)
|
||
{
|
||
if (float.IsNaN(value) ||
|
||
float.IsInfinity(value) ||
|
||
(allowZero
|
||
? value < 0f
|
||
: value <= 0f))
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
parameterName,
|
||
"原地自转参数必须是有效的正数。");
|
||
}
|
||
}
|
||
}
|
||
}
|