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

备份

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-14 13:55:19 +08:00
parent 7611deefa3
commit a13e345f83
23 changed files with 144 additions and 596 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Shared/Chassis/MultiWheelChassisAdapter.cs
+86 -95
View File
@@ -11,7 +11,6 @@ namespace MyParking.Shared
public sealed class MultiWheelChassisAdapter
{
#region 辅助内容
private const double RadiansToDegrees = 180.0 / Math.PI;
private const float BiasTolerance = 0.001f;
private readonly MultiWheelChassis _chassis;
/// <summary>
@@ -44,15 +43,12 @@ namespace MyParking.Shared
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.");
}
NumericGuard.EnsureFinitePositive(
value,
nameof(value));
EnsureRepresentableAsSingle(
value,
nameof(value));
_chassis.SteeringAlignmentSigmaDegrees =
(float)value;
@@ -74,18 +70,18 @@ namespace MyParking.Shared
private void EnsureMotionFrameIsActive(
double motionDirectionRadians)
{
ValidateFinite(
NumericGuard.EnsureFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
var expectedBiasDegrees =
(float)(
-FrameTransform2D.NormalizeAngle(
motionDirectionRadians) *
RadiansToDegrees);
ConvertRadiansToSingleDegrees(
-AngleMath.NormalizeRadians(
motionDirectionRadians),
nameof(motionDirectionRadians));
var bias = _chassis.GetOriginBias();
var angleErrorDegrees =
NormalizeDegrees(
AngleMath.NormalizeDegrees(
bias.Z - expectedBiasDegrees);
if (Math.Abs(bias.X) <= BiasTolerance &&
@@ -102,46 +98,32 @@ namespace MyParking.Shared
$"期望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(
EnsureRepresentableAsSingle(
twist.VxMetersPerSecond,
nameof(twist.VxMetersPerSecond));
ValidateFinite(
EnsureRepresentableAsSingle(
twist.VyMetersPerSecond,
nameof(twist.VyMetersPerSecond));
ValidateFinite(
twist.OmegaRadiansPerSecond,
EnsureRepresentableAsSingle(
AngleMath.RadiansToDegrees(
twist.OmegaRadiansPerSecond),
nameof(twist.OmegaRadiansPerSecond));
}
/// <summary>
/// 检查数值是否为有限值。
/// 检查数值是否为有限值且可安全转换为float。
/// </summary>
private static void ValidateFinite(
private static void EnsureRepresentableAsSingle(
double value,
string parameterName)
{
if (double.IsNaN(value) ||
double.IsInfinity(value))
{
throw new ArgumentOutOfRangeException(
parameterName,
"底盘速度命令不能是NaN或无穷大。");
}
NumericGuard.EnsureFinite(value, parameterName);
if (value > float.MaxValue ||
value < -float.MaxValue)
@@ -152,6 +134,21 @@ namespace MyParking.Shared
}
}
/// <summary>
/// 将有限弧度值转换为float可表示的角度值。
/// </summary>
private static float ConvertRadiansToSingleDegrees(
double angleRadians,
string parameterName)
{
var angleDegrees =
AngleMath.RadiansToDegrees(angleRadians);
EnsureRepresentableAsSingle(
angleDegrees,
parameterName);
return (float)angleDegrees;
}
/// <summary>
/// 获取最近一次底盘运动分解失败原因。
/// </summary>
@@ -175,15 +172,15 @@ namespace MyParking.Shared
public void ActivateMotionFrame(
double motionDirectionRadians)
{
ValidateFinite(
NumericGuard.EnsureFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
var biasDegrees =
(float)(
-FrameTransform2D.NormalizeAngle(
motionDirectionRadians) *
RadiansToDegrees);
ConvertRadiansToSingleDegrees(
-AngleMath.NormalizeRadians(
motionDirectionRadians),
nameof(motionDirectionRadians));
var currentBias =
_chassis.GetOriginBias();
@@ -192,7 +189,7 @@ namespace MyParking.Shared
Math.Abs(currentBias.Y) <=
BiasTolerance &&
Math.Abs(
NormalizeDegrees(
AngleMath.NormalizeDegrees(
currentBias.Z -
biasDegrees)) <=
BiasTolerance)
@@ -284,9 +281,9 @@ namespace MyParking.Shared
var vyMetersPerSecond =
(float)command.BodyTwist.VyMetersPerSecond;
var omegaDegreesPerSecond =
(float)(
command.BodyTwist.OmegaRadiansPerSecond *
RadiansToDegrees);
ConvertRadiansToSingleDegrees(
command.BodyTwist.OmegaRadiansPerSecond,
nameof(command.BodyTwist.OmegaRadiansPerSecond));
var success = _chassis.SendXYThSpeed(
vxMetersPerSecond,
vyMetersPerSecond,
@@ -312,13 +309,13 @@ namespace MyParking.Shared
double steeringRadians,
TimeSpan? interval = null)
{
ValidateFinite(
NumericGuard.EnsureFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
ValidateFinite(
EnsureRepresentableAsSingle(
speedMetersPerSecond,
nameof(speedMetersPerSecond));
ValidateFinite(
NumericGuard.EnsureFinite(
steeringRadians,
nameof(steeringRadians));
EnsureMotionFrameIsActive(
@@ -333,9 +330,9 @@ namespace MyParking.Shared
}
var steeringDegrees =
(float)(
steeringRadians *
RadiansToDegrees);
ConvertRadiansToSingleDegrees(
steeringRadians,
nameof(steeringRadians));
var success =
_chassis.SendMotion(
(float)speedMetersPerSecond,
@@ -360,13 +357,13 @@ namespace MyParking.Shared
double rearAngleRadians,
TimeSpan? interval = null)
{
ValidateFinite(
EnsureRepresentableAsSingle(
speedMetersPerSecond,
nameof(speedMetersPerSecond));
ValidateFinite(
NumericGuard.EnsureFinite(
frontAngleRadians,
nameof(frontAngleRadians));
ValidateFinite(
NumericGuard.EnsureFinite(
rearAngleRadians,
nameof(rearAngleRadians));
EnsureBodyFrameIsActive();
@@ -383,10 +380,12 @@ namespace MyParking.Shared
var success = _chassis.SendMotion(
(float)speedMetersPerSecond,
(float)(frontAngleRadians *
RadiansToDegrees),
(float)(rearAngleRadians *
RadiansToDegrees),
ConvertRadiansToSingleDegrees(
frontAngleRadians,
nameof(frontAngleRadians)),
ConvertRadiansToSingleDegrees(
rearAngleRadians,
nameof(rearAngleRadians)),
interval);
if (!success)
@@ -422,8 +421,11 @@ namespace MyParking.Shared
double directionRadians)
{
EnsureBodyFrameIsActive();
var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
RadiansToDegrees);
var targetDegrees =
ConvertRadiansToSingleDegrees(
AngleMath.NormalizeRadians(
directionRadians),
nameof(directionRadians));
#pragma warning disable CS0612, CS0618
var wheels = _chassis.GetSteerWheels();
@@ -463,23 +465,21 @@ namespace MyParking.Shared
double directionRadians,
double toleranceRadians)
{
if (double.IsNaN(toleranceRadians) ||
double.IsInfinity(toleranceRadians) ||
toleranceRadians < 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(toleranceRadians),
"舵轮到位容差必须是非负有限值。");
}
NumericGuard.EnsureFiniteNonNegative(
toleranceRadians,
nameof(toleranceRadians));
EnsureBodyFrameIsActive();
var targetDegrees = (float)(
FrameTransform2D.NormalizeAngle(directionRadians) *
180.0 / Math.PI);
var targetDegrees =
ConvertRadiansToSingleDegrees(
AngleMath.NormalizeRadians(
directionRadians),
nameof(directionRadians));
var toleranceDegrees = (float)(
Math.Abs(toleranceRadians) *
180.0 / Math.PI);
var toleranceDegrees =
ConvertRadiansToSingleDegrees(
toleranceRadians,
nameof(toleranceRadians));
#pragma warning disable CS0612, CS0618
var wheels = _chassis.GetSteerWheels();
@@ -507,16 +507,12 @@ namespace MyParking.Shared
TimeSpan? interval = null,
double alignmentToleranceDegrees = 2.0)
{
ValidateFinite(
NumericGuard.EnsureFiniteNonNegative(
alignmentToleranceDegrees,
nameof(alignmentToleranceDegrees));
EnsureRepresentableAsSingle(
alignmentToleranceDegrees,
nameof(alignmentToleranceDegrees));
if (alignmentToleranceDegrees < 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(alignmentToleranceDegrees),
"自转舵轮到位容差必须是非负有限值。");
}
EnsureBodyFrameIsActive();
@@ -540,23 +536,18 @@ namespace MyParking.Shared
double toleranceRadians =
2.0 * Math.PI / 180.0)
{
if (double.IsNaN(toleranceRadians) ||
double.IsInfinity(toleranceRadians) ||
toleranceRadians < 0.0)
{
throw new ArgumentOutOfRangeException(
nameof(toleranceRadians),
"自转状态交接容差必须是非负有限值。");
}
NumericGuard.EnsureFiniteNonNegative(
toleranceRadians,
nameof(toleranceRadians));
EnsureBodyFrameIsActive();
var success =
_chassis
.AdoptPreparedRotateWheelsForXYTh(
(float)(
toleranceRadians *
RadiansToDegrees));
ConvertRadiansToSingleDegrees(
toleranceRadians,
nameof(toleranceRadians)));
if (!success)
{