First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial commit from MyParking project

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-04 10:29:21 +08:00
commit d9432bd529
138 changed files with 16957 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Shared/Chassis/MultiWheelChassisAdapter.cs
+515
View File
@@ -0,0 +1,515 @@
// 将统一命令转换为原 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;
}
}