516 lines
17 KiB
C#
516 lines
17 KiB
C#
// 将统一命令转换为原 Chassis API 调用
|
|
using System;
|
|
using CommonUsage.Chassis;
|
|
|
|
namespace MyParking.Shared
|
|
{
|
|
/// <summary>
|
|
/// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
|
|
/// 车体坐标系固定为X向前、Y向左、逆时针为正。
|
|
/// </summary>
|
|
public sealed class MultiWheelChassisAdapter
|
|
{
|
|
#region 辅助内容
|
|
private const double RadiansToDegrees = 180.0 / Math.PI;
|
|
private const float BiasTolerance = 0.001f;
|
|
private readonly MultiWheelChassis _chassis;
|
|
/// <summary>
|
|
/// 当前适配器对应的车辆编号。
|
|
/// </summary>
|
|
public int VehicleId { get; }
|
|
|
|
/// <summary>
|
|
/// Maximum distance from the body origin to a wheel center, in metres.
|
|
/// </summary>
|
|
public double MaximumWheelRadiusMeters { get; }
|
|
|
|
/// <summary>
|
|
/// Maximum longitudinal wheel offset from the body origin, in metres.
|
|
/// For a symmetric four-wheel-steering chassis this is half the wheelbase.
|
|
/// </summary>
|
|
public double HalfWheelBaseMeters { get; }
|
|
|
|
/// <summary>
|
|
/// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
|
|
/// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
|
|
/// </summary>
|
|
public double HalfTrackWidthMeters { get; }
|
|
|
|
/// <summary>
|
|
/// Width of the steering-alignment speed gate, in degrees.
|
|
/// </summary>
|
|
public double SteeringAlignmentSigmaDegrees
|
|
{
|
|
get => _chassis.SteeringAlignmentSigmaDegrees;
|
|
set
|
|
{
|
|
if (double.IsNaN(value) ||
|
|
double.IsInfinity(value) ||
|
|
value <= 0.0 ||
|
|
value > float.MaxValue)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(value),
|
|
"Steering alignment sigma must be a positive finite value.");
|
|
}
|
|
|
|
_chassis.SteeringAlignmentSigmaDegrees =
|
|
(float)value;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
|
|
/// </summary>
|
|
private void EnsureBodyFrameIsActive()
|
|
{
|
|
EnsureMotionFrameIsActive(0.0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查旧底盘当前是否处于指定的运动坐标系。
|
|
/// motionDirectionRadians表示该运动系X轴在真实车体坐标系中的方向。
|
|
/// </summary>
|
|
private void EnsureMotionFrameIsActive(
|
|
double motionDirectionRadians)
|
|
{
|
|
ValidateFinite(
|
|
motionDirectionRadians,
|
|
nameof(motionDirectionRadians));
|
|
|
|
var expectedBiasDegrees =
|
|
(float)(
|
|
-FrameTransform2D.NormalizeAngle(
|
|
motionDirectionRadians) *
|
|
RadiansToDegrees);
|
|
var bias = _chassis.GetOriginBias();
|
|
var angleErrorDegrees =
|
|
NormalizeDegrees(
|
|
bias.Z - expectedBiasDegrees);
|
|
|
|
if (Math.Abs(bias.X) <= BiasTolerance &&
|
|
Math.Abs(bias.Y) <= BiasTolerance &&
|
|
Math.Abs(angleErrorDegrees) <=
|
|
BiasTolerance)
|
|
{
|
|
return;
|
|
}
|
|
|
|
throw new InvalidOperationException(
|
|
"MultiWheelChassis当前运动坐标系与命令不一致。" +
|
|
$"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°，" +
|
|
$"期望Th={expectedBiasDegrees}°。");
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将角度归一化到[-180°,180°]附近。
|
|
/// </summary>
|
|
private static float NormalizeDegrees(float degrees)
|
|
{
|
|
return (float)(
|
|
degrees -
|
|
Math.Round(degrees / 360.0) * 360.0);
|
|
}
|
|
/// <summary>
|
|
/// 检查底盘命令是否包含无效数值。
|
|
/// </summary>
|
|
private static void ValidateTwist(Twist2D twist)
|
|
{
|
|
ValidateFinite(
|
|
twist.VxMetersPerSecond,
|
|
nameof(twist.VxMetersPerSecond));
|
|
|
|
ValidateFinite(
|
|
twist.VyMetersPerSecond,
|
|
nameof(twist.VyMetersPerSecond));
|
|
|
|
ValidateFinite(
|
|
twist.OmegaRadiansPerSecond,
|
|
nameof(twist.OmegaRadiansPerSecond));
|
|
}
|
|
/// <summary>
|
|
/// 检查数值是否为有限值。
|
|
/// </summary>
|
|
private static void ValidateFinite(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
if (double.IsNaN(value) ||
|
|
double.IsInfinity(value))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"底盘速度命令不能是NaN或无穷大。");
|
|
}
|
|
|
|
if (value > float.MaxValue ||
|
|
value < -float.MaxValue)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"底盘速度命令超过float可表示范围。");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取最近一次底盘运动分解失败原因。
|
|
/// </summary>
|
|
public string LastFailureReason =>
|
|
_chassis.LastMotionDecomposeFailureReason;
|
|
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
|
|
/// </summary>
|
|
public void ResetToBodyFrame()
|
|
{
|
|
ActivateMotionFrame(0.0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 激活指定运动方向对应的SendMotion坐标系。
|
|
/// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
|
|
/// </summary>
|
|
public void ActivateMotionFrame(
|
|
double motionDirectionRadians)
|
|
{
|
|
ValidateFinite(
|
|
motionDirectionRadians,
|
|
nameof(motionDirectionRadians));
|
|
|
|
var biasDegrees =
|
|
(float)(
|
|
-FrameTransform2D.NormalizeAngle(
|
|
motionDirectionRadians) *
|
|
RadiansToDegrees);
|
|
var currentBias =
|
|
_chassis.GetOriginBias();
|
|
|
|
if (Math.Abs(currentBias.X) <=
|
|
BiasTolerance &&
|
|
Math.Abs(currentBias.Y) <=
|
|
BiasTolerance &&
|
|
Math.Abs(
|
|
NormalizeDegrees(
|
|
currentBias.Z -
|
|
biasDegrees)) <=
|
|
BiasTolerance)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_chassis.SetOriginBias(
|
|
x: 0.0f,
|
|
y: 0.0f,
|
|
th: biasDegrees);
|
|
}
|
|
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
|
|
{
|
|
_chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
|
|
if (vehicleId <= 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(vehicleId),
|
|
"车辆编号必须大于零。");
|
|
}
|
|
VehicleId = vehicleId;
|
|
#pragma warning disable CS0612, CS0618
|
|
var wheels = _chassis.GetSteerWheels();
|
|
#pragma warning restore CS0612, CS0618
|
|
|
|
if (wheels.Count == 0)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"MultiWheelChassis尚未完成舵轮初始化，" +
|
|
"不能创建底盘适配器。");
|
|
}
|
|
// 禁用旧版DirectionAngle/ZeroDirection坐标偏置，
|
|
// 保证SendXYThSpeed直接使用真实车体坐标系。
|
|
var maximumWheelRadiusMillimeters = 0.0;
|
|
var maximumLongitudinalOffsetMillimeters = 0.0;
|
|
var maximumLateralOffsetMillimeters = 0.0;
|
|
foreach (var wheel in wheels)
|
|
{
|
|
maximumWheelRadiusMillimeters = Math.Max(
|
|
maximumWheelRadiusMillimeters,
|
|
wheel.PhysicalPosition.Length());
|
|
|
|
maximumLongitudinalOffsetMillimeters = Math.Max(
|
|
maximumLongitudinalOffsetMillimeters,
|
|
Math.Abs(wheel.PhysicalPosition.X));
|
|
|
|
maximumLateralOffsetMillimeters = Math.Max(
|
|
maximumLateralOffsetMillimeters,
|
|
Math.Abs(wheel.PhysicalPosition.Y));
|
|
}
|
|
|
|
MaximumWheelRadiusMeters =
|
|
maximumWheelRadiusMillimeters / 1000.0;
|
|
HalfWheelBaseMeters =
|
|
maximumLongitudinalOffsetMillimeters / 1000.0;
|
|
HalfTrackWidthMeters =
|
|
maximumLateralOffsetMillimeters / 1000.0;
|
|
|
|
if (MaximumWheelRadiusMeters <= 0.0 ||
|
|
HalfWheelBaseMeters <= 0.0 ||
|
|
HalfTrackWidthMeters <= 0.0)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"Wheel positions cannot produce valid chassis dimensions.");
|
|
}
|
|
|
|
// 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
|
|
_chassis.PreferMinimumSteeringTravel = true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将车体坐标系速度命令发送给多舵轮底盘。
|
|
/// </summary>
|
|
public bool Send(ChassisCommand command, TimeSpan? interval = null)
|
|
{
|
|
if (command.VehicleId != VehicleId)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"命令车辆编号{command.VehicleId}与适配器车辆编号" +
|
|
$"{VehicleId}不一致。");
|
|
}
|
|
ValidateTwist(command.BodyTwist);
|
|
// 防止其他旧逻辑再次调用DirectionAngle或
|
|
// SetOriginBias改变底盘坐标语义。
|
|
EnsureBodyFrameIsActive();
|
|
var vxMetersPerSecond =
|
|
(float)command.BodyTwist.VxMetersPerSecond;
|
|
var vyMetersPerSecond =
|
|
(float)command.BodyTwist.VyMetersPerSecond;
|
|
var omegaDegreesPerSecond =
|
|
(float)(
|
|
command.BodyTwist.OmegaRadiansPerSecond *
|
|
RadiansToDegrees);
|
|
var success = _chassis.SendXYThSpeed(
|
|
vxMetersPerSecond,
|
|
vyMetersPerSecond,
|
|
omegaDegreesPerSecond,
|
|
interval);
|
|
if (!success)
|
|
{
|
|
// 防止分解失败后继续执行上一条运动命令。
|
|
_chassis.PredefinedDriveStop();
|
|
}
|
|
return success;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
|
|
/// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
|
|
/// </summary>
|
|
public bool SendVirtualAckermannMotion(
|
|
double motionDirectionRadians,
|
|
double speedMetersPerSecond,
|
|
double steeringRadians,
|
|
TimeSpan? interval = null)
|
|
{
|
|
ValidateFinite(
|
|
motionDirectionRadians,
|
|
nameof(motionDirectionRadians));
|
|
ValidateFinite(
|
|
speedMetersPerSecond,
|
|
nameof(speedMetersPerSecond));
|
|
ValidateFinite(
|
|
steeringRadians,
|
|
nameof(steeringRadians));
|
|
EnsureMotionFrameIsActive(
|
|
motionDirectionRadians);
|
|
|
|
if (Math.Abs(steeringRadians) >=
|
|
Math.PI / 2.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(steeringRadians),
|
|
"虚拟阿克曼转向角必须位于正负90度以内。");
|
|
}
|
|
|
|
var steeringDegrees =
|
|
(float)(
|
|
steeringRadians *
|
|
RadiansToDegrees);
|
|
var success =
|
|
_chassis.SendMotion(
|
|
(float)speedMetersPerSecond,
|
|
steeringDegrees,
|
|
-steeringDegrees,
|
|
interval);
|
|
|
|
if (!success)
|
|
{
|
|
_chassis.PredefinedDriveStop();
|
|
}
|
|
|
|
return success;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 立即将所有驱动轮速度下发为零。
|
|
/// </summary>
|
|
public void StopImmediately()
|
|
{
|
|
_chassis.PredefinedDriveStop();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 清零XYTh驱动速度，但保留已经准备好的自转舵角和轮速方向。
|
|
/// </summary>
|
|
public void StopXYThDrivePreserveSteeringState()
|
|
{
|
|
_chassis.StopXYThDrivePreserveSteeringState();
|
|
}
|
|
|
|
/// <summary>
|
|
/// 停车并将所有舵轮转到指定的车体角度。
|
|
/// 只调整舵轮角度，不产生车辆线速度。
|
|
/// </summary>
|
|
public bool PrepareParallelDirection(
|
|
double directionRadians)
|
|
{
|
|
EnsureBodyFrameIsActive();
|
|
var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
|
|
RadiansToDegrees);
|
|
|
|
#pragma warning disable CS0612, CS0618
|
|
var wheels = _chassis.GetSteerWheels();
|
|
#pragma warning restore CS0612, CS0618
|
|
|
|
// 没有舵轮时不能认为预对齐成功。
|
|
if (wheels.Count == 0)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// 先检查所有舵轮能否到达目标机械角度。
|
|
foreach (var wheel in wheels)
|
|
{
|
|
if (targetDegrees < wheel.AngleLowerLimit ||
|
|
targetDegrees > wheel.AngleUpperLimit)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// 模式切换前立即停止驱动轮。
|
|
_chassis.PredefinedDriveStop();
|
|
// 检查完成后再统一下发，避免只转动一部分舵轮。
|
|
foreach (var wheel in wheels)
|
|
{
|
|
wheel.WriteAngle(targetDegrees);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查所有舵轮是否已经对准给定方向。
|
|
/// </summary>
|
|
public bool AreParallelWheelsAligned(
|
|
double directionRadians,
|
|
double toleranceRadians)
|
|
{
|
|
if (double.IsNaN(toleranceRadians) ||
|
|
double.IsInfinity(toleranceRadians) ||
|
|
toleranceRadians < 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(toleranceRadians),
|
|
"舵轮到位容差必须是非负有限值。");
|
|
}
|
|
|
|
EnsureBodyFrameIsActive();
|
|
var targetDegrees = (float)(
|
|
FrameTransform2D.NormalizeAngle(directionRadians) *
|
|
180.0 / Math.PI);
|
|
|
|
var toleranceDegrees = (float)(
|
|
Math.Abs(toleranceRadians) *
|
|
180.0 / Math.PI);
|
|
|
|
#pragma warning disable CS0612, CS0618
|
|
var wheels = _chassis.GetSteerWheels();
|
|
#pragma warning restore CS0612, CS0618
|
|
|
|
foreach (var wheel in wheels)
|
|
{
|
|
var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
|
|
|
|
if (Math.Abs(angleErrorDegrees) >
|
|
toleranceDegrees)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 停车并将舵轮预对齐到原地自转方向。
|
|
/// 返回是否成功生成舵轮目标。
|
|
/// </summary>
|
|
public bool PrepareSpin(
|
|
TimeSpan? interval = null)
|
|
{
|
|
EnsureBodyFrameIsActive();
|
|
|
|
var success =
|
|
_chassis.PrepareRotateWheels(
|
|
alignmentToleranceDegrees: 2.0f);
|
|
|
|
if (!success)
|
|
{
|
|
_chassis.PredefinedDriveStop();
|
|
}
|
|
return success;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将已到位的自转舵角和轮速方向一次性交接给XYTh，
|
|
/// 防止普通SendXYThSpeed正式运动首帧重新初始化运动状态。
|
|
/// </summary>
|
|
public bool AdoptPreparedSpinForXYTh(
|
|
double toleranceRadians =
|
|
2.0 * Math.PI / 180.0)
|
|
{
|
|
if (double.IsNaN(toleranceRadians) ||
|
|
double.IsInfinity(toleranceRadians) ||
|
|
toleranceRadians < 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(toleranceRadians),
|
|
"自转状态交接容差必须是非负有限值。");
|
|
}
|
|
|
|
EnsureBodyFrameIsActive();
|
|
|
|
var success =
|
|
_chassis
|
|
.AdoptPreparedRotateWheelsForXYTh(
|
|
(float)(
|
|
toleranceRadians *
|
|
RadiansToDegrees));
|
|
|
|
if (!success)
|
|
{
|
|
_chassis.PredefinedDriveStop();
|
|
}
|
|
|
|
return success;
|
|
}
|
|
/// <summary>
|
|
/// 所有舵轮是否已对齐到原地自转方向。
|
|
/// </summary>
|
|
public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
|
|
|
|
|
|
|
|
}
|
|
}
|