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 16:19:15 +08:00
parent a13e345f83
commit 9fe8901c4c
25 changed files with 381 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Shared/Chassis/MultiWheelChassisAdapter.cs
+163 -78
View File
@@ -12,7 +12,9 @@ namespace MyParking.Shared
{
#region 辅助内容
private const float BiasTolerance = 0.001f;
private const double MotionDeadband = 1e-6;
private readonly MultiWheelChassis _chassis;
private double _activeMotionDirectionRadians;
/// <summary>
/// 当前适配器对应的车辆编号。
/// </summary>
@@ -31,10 +33,22 @@ namespace MyParking.Shared
/// <summary>
/// 车体原点到最外侧舵轮中心的最大横向距离，单位为米。
/// 对称四舵轮底盘中，它也是蟹行虚拟阿克曼模型的半轴距。
/// 对称四舵轮底盘中，它通常等于物理轮距的一半。
/// </summary>
public double HalfTrackWidthMeters { get; }
/// <summary>
/// 获取旧版SendMotion使用的对称前后GCP半径，单位为m。
/// </summary>
public double ControlPointRadiusMeters =>
_chassis.ControlPointRadius / 1000.0;
/// <summary>
/// 获取当前已经准备并激活的滚动运动系X轴在真实车体系中的方向，单位为rad。
/// </summary>
public double ActiveMotionDirectionRadians =>
_activeMotionDirectionRadians;
/// <summary>
/// Width of the steering-alignment speed gate, in degrees.
/// </summary>
@@ -168,6 +182,7 @@ namespace MyParking.Shared
/// <summary>
/// 激活指定运动方向对应的SendMotion坐标系。
/// 0表示真实车头，正90度表示将车体左侧作为虚拟车头。
/// 调用方必须先停车，并确认舵轮已经按该方向完成预对齐。
/// </summary>
public void ActivateMotionFrame(
double motionDirectionRadians)
@@ -176,10 +191,13 @@ namespace MyParking.Shared
motionDirectionRadians,
nameof(motionDirectionRadians));
var normalizedDirectionRadians =
AngleMath.NormalizeRadians(
motionDirectionRadians);
var biasDegrees =
ConvertRadiansToSingleDegrees(
-AngleMath.NormalizeRadians(
motionDirectionRadians),
-normalizedDirectionRadians,
nameof(motionDirectionRadians));
var currentBias =
_chassis.GetOriginBias();
@@ -194,6 +212,8 @@ namespace MyParking.Shared
biasDegrees)) <=
BiasTolerance)
{
SetActiveMotionDirection(
normalizedDirectionRadians);
return;
}
@@ -201,6 +221,19 @@ namespace MyParking.Shared
x: 0.0f,
y: 0.0f,
th: biasDegrees);
SetActiveMotionDirection(
normalizedDirectionRadians);
}
/// <summary>
/// 缓存当前运动坐标系方向，供控制周期内转换车体速度。
/// </summary>
private void SetActiveMotionDirection(
double motionDirectionRadians)
{
_activeMotionDirectionRadians =
AngleMath.NormalizeRadians(
motionDirectionRadians);
}
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
{
@@ -251,94 +284,146 @@ namespace MyParking.Shared
if (MaximumWheelRadiusMeters <= 0.0 ||
HalfWheelBaseMeters <= 0.0 ||
HalfTrackWidthMeters <= 0.0)
HalfTrackWidthMeters <= 0.0 ||
ControlPointRadiusMeters <= 0.0)
{
throw new InvalidOperationException(
"Wheel positions cannot produce valid chassis dimensions.");
"Wheel positions and ControlPointRadius must produce valid chassis dimensions.");
}
var initialBias = _chassis.GetOriginBias();
SetActiveMotionDirection(
-AngleMath.DegreesToRadians(
initialBias.Z));
// 通过反转轮速表达反向运动，避免蟹行正反切换时舵轮无意义地旋转180°。
_chassis.PreferMinimumSteeringTravel = true;
}
/// <summary>
/// 将车体坐标系速度命令发送给多舵轮底盘。
/// 将车体坐标系刚体速度统一转换为滚动SendMotion、原地自转或停车命令。
/// 非零平移命令使用调用方在运动段开始前已经准备并激活的β运动坐标系。
/// </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 =
ConvertRadiansToSingleDegrees(
command.BodyTwist.OmegaRadiansPerSecond,
nameof(command.BodyTwist.OmegaRadiansPerSecond));
var success = _chassis.SendXYThSpeed(
vxMetersPerSecond,
vyMetersPerSecond,
omegaDegreesPerSecond,
interval,
enableDifferentialSteerFeedforward: true);
if (!success)
{
// 防止分解失败后继续执行上一条运动命令。
_chassis.PredefinedDriveStop();
}
return success;
}
/// <summary>
/// 在已经激活的运动坐标系中使用SendMotion执行虚拟阿克曼运动。
/// 转向角均相对该运动坐标系表达；正90度运动系对应车体左侧蟹行。
/// </summary>
public bool SendVirtualAckermannMotion(
double motionDirectionRadians,
double speedMetersPerSecond,
double steeringRadians,
public bool SendBodyTwist(
Twist2D bodyTwist,
TimeSpan? interval = null)
{
NumericGuard.EnsureFinite(
motionDirectionRadians,
nameof(motionDirectionRadians));
EnsureRepresentableAsSingle(
speedMetersPerSecond,
nameof(speedMetersPerSecond));
NumericGuard.EnsureFinite(
steeringRadians,
nameof(steeringRadians));
EnsureMotionFrameIsActive(
motionDirectionRadians);
ValidateTwist(bodyTwist);
if (Math.Abs(steeringRadians) >=
Math.PI / 2.0)
var linearSpeedMetersPerSecond =
Math.Sqrt(
bodyTwist.VxMetersPerSecond *
bodyTwist.VxMetersPerSecond +
bodyTwist.VyMetersPerSecond *
bodyTwist.VyMetersPerSecond);
if (linearSpeedMetersPerSecond <= MotionDeadband)
{
throw new ArgumentOutOfRangeException(
nameof(steeringRadians),
"虚拟阿克曼转向角必须位于正负90度以内。");
if (Math.Abs(
bodyTwist.OmegaRadiansPerSecond) <=
MotionDeadband)
{
StopImmediately();
return true;
}
return SendPureRotation(
bodyTwist.OmegaRadiansPerSecond,
interval);
}
var steeringDegrees =
return SendRollingTwistInActiveMotionFrame(
bodyTwist,
linearSpeedMetersPerSecond,
interval);
}
/// <summary>
/// 将车体刚体速度转换到已准备的运动坐标系，并生成该坐标系中的前后GCP方向。
/// </summary>
private bool SendRollingTwistInActiveMotionFrame(
Twist2D bodyTwist,
double linearSpeedMetersPerSecond,
TimeSpan? interval)
{
EnsureMotionFrameIsActive(
_activeMotionDirectionRadians);
var bodyPoseInMotionFrame =
new Pose2D(
0.0,
0.0,
-_activeMotionDirectionRadians);
var motionTwist =
FrameTransform2D.TransformTwistAtSamePoint(
bodyPoseInMotionFrame,
bodyTwist);
var motionVxMetersPerSecond =
motionTwist.VxMetersPerSecond;
var motionVyMetersPerSecond =
motionTwist.VyMetersPerSecond;
if (Math.Abs(motionVxMetersPerSecond) <=
MotionDeadband)
{
StopImmediately();
throw new InvalidOperationException(
"当前车体速度几乎垂直于已经准备的运动坐标系，" +
"无法由方向型前后GCP稳定表示。请停车后按目标主运动方向重新准备并激活β。");
}
var travelDirection =
Math.Sign(
motionVxMetersPerSecond);
var signedCenterSpeedMetersPerSecond =
travelDirection *
linearSpeedMetersPerSecond;
var frontVelocityYMetersPerSecond =
motionVyMetersPerSecond +
bodyTwist.OmegaRadiansPerSecond *
ControlPointRadiusMeters;
var rearVelocityYMetersPerSecond =
motionVyMetersPerSecond -
bodyTwist.OmegaRadiansPerSecond *
ControlPointRadiusMeters;
var directedVxMetersPerSecond =
travelDirection *
motionVxMetersPerSecond;
var frontAngleRadians =
Math.Atan2(
travelDirection *
frontVelocityYMetersPerSecond,
directedVxMetersPerSecond);
var rearAngleRadians =
Math.Atan2(
travelDirection *
rearVelocityYMetersPerSecond,
directedVxMetersPerSecond);
return SendGcpMotionInActiveFrame(
signedCenterSpeedMetersPerSecond,
frontAngleRadians,
rearAngleRadians,
interval);
}
/// <summary>
/// 将已经完成自转舵轮准备的纯角速度命令交给XYTh底盘解算。
/// </summary>
private bool SendPureRotation(
double omegaRadiansPerSecond,
TimeSpan? interval)
{
EnsureBodyFrameIsActive();
var success = _chassis.SendXYThSpeed(
0.0f,
0.0f,
ConvertRadiansToSingleDegrees(
steeringRadians,
nameof(steeringRadians));
var success =
_chassis.SendMotion(
(float)speedMetersPerSecond,
steeringDegrees,
-steeringDegrees,
interval);
omegaRadiansPerSecond,
nameof(omegaRadiansPerSecond)),
interval,
enableDifferentialSteerFeedforward: true);
if (!success)
{
@@ -347,11 +432,10 @@ namespace MyParking.Shared
return success;
}
/// <summary>
/// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
/// 在当前已激活的运动坐标系中将有符号速度和前后GCP角度发送给旧版SendMotion。
/// </summary>
public bool SendGcpMotion(
private bool SendGcpMotionInActiveFrame(
double speedMetersPerSecond,
double frontAngleRadians,
double rearAngleRadians,
@@ -366,7 +450,8 @@ namespace MyParking.Shared
NumericGuard.EnsureFinite(
rearAngleRadians,
nameof(rearAngleRadians));
EnsureBodyFrameIsActive();
EnsureMotionFrameIsActive(
_activeMotionDirectionRadians);
if (Math.Abs(frontAngleRadians) >=
Math.PI / 2.0 ||