feat: 发布 EM 轨迹规划首个版本
This commit is contained in:
@@ -0,0 +1,576 @@
|
||||
// 将统一命令转换为原 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,
|
||||
enableDifferentialSteerFeedforward: true);
|
||||
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>
|
||||
/// 在真实车体坐标系中将有符号速度和独立前后GCP角度发送给旧版SendMotion。
|
||||
/// </summary>
|
||||
public bool SendGcpMotion(
|
||||
double speedMetersPerSecond,
|
||||
double frontAngleRadians,
|
||||
double rearAngleRadians,
|
||||
TimeSpan? interval = null)
|
||||
{
|
||||
ValidateFinite(
|
||||
speedMetersPerSecond,
|
||||
nameof(speedMetersPerSecond));
|
||||
ValidateFinite(
|
||||
frontAngleRadians,
|
||||
nameof(frontAngleRadians));
|
||||
ValidateFinite(
|
||||
rearAngleRadians,
|
||||
nameof(rearAngleRadians));
|
||||
EnsureBodyFrameIsActive();
|
||||
|
||||
if (Math.Abs(frontAngleRadians) >=
|
||||
Math.PI / 2.0 ||
|
||||
Math.Abs(rearAngleRadians) >=
|
||||
Math.PI / 2.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(frontAngleRadians),
|
||||
"前后GCP角度必须位于正负90度以内，避免四轮几何解算出现奇异值。");
|
||||
}
|
||||
|
||||
var success = _chassis.SendMotion(
|
||||
(float)speedMetersPerSecond,
|
||||
(float)(frontAngleRadians *
|
||||
RadiansToDegrees),
|
||||
(float)(rearAngleRadians *
|
||||
RadiansToDegrees),
|
||||
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,
|
||||
double alignmentToleranceDegrees = 2.0)
|
||||
{
|
||||
ValidateFinite(
|
||||
alignmentToleranceDegrees,
|
||||
nameof(alignmentToleranceDegrees));
|
||||
|
||||
if (alignmentToleranceDegrees < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(alignmentToleranceDegrees),
|
||||
"自转舵轮到位容差必须是非负有限值。");
|
||||
}
|
||||
|
||||
EnsureBodyFrameIsActive();
|
||||
|
||||
var success =
|
||||
_chassis.PrepareRotateWheels(
|
||||
alignmentToleranceDegrees:
|
||||
(float)alignmentToleranceDegrees);
|
||||
|
||||
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;
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
// 把车队整体速度分解为每辆车的局部速度
|
||||
@@ -0,0 +1,138 @@
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供与坐标系无关的角度归一化、角度差和单位转换功能。
|
||||
/// </summary>
|
||||
public static class AngleMath
|
||||
{
|
||||
public const double TwoPi = 2.0 * Math.PI;
|
||||
|
||||
/// <summary>
|
||||
/// 将弧度归一化到[-π, π)区间。
|
||||
/// -π包含在结果中，+π不包含在结果中，因此+π会返回-π。
|
||||
/// </summary>
|
||||
public static double NormalizeRadians(double angleRadians)
|
||||
{
|
||||
EnsureFinite(angleRadians, nameof(angleRadians));
|
||||
|
||||
var normalized = angleRadians % TwoPi;
|
||||
|
||||
if (normalized >= Math.PI)
|
||||
{
|
||||
normalized -= TwoPi;
|
||||
}
|
||||
else if (normalized < -Math.PI)
|
||||
{
|
||||
normalized += TwoPi;
|
||||
}
|
||||
|
||||
return normalized == 0.0 ? 0.0 : normalized;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度归一化到[-180°, 180°)区间。
|
||||
/// -180°包含在结果中，+180°不包含在结果中，因此+180°会返回-180°。
|
||||
/// </summary>
|
||||
public static double NormalizeDegrees(double angleDegrees)
|
||||
{
|
||||
EnsureFinite(angleDegrees, nameof(angleDegrees));
|
||||
|
||||
var normalized = angleDegrees % 360.0;
|
||||
|
||||
if (normalized >= 180.0)
|
||||
{
|
||||
normalized -= 360.0;
|
||||
}
|
||||
else if (normalized < -180.0)
|
||||
{
|
||||
normalized += 360.0;
|
||||
}
|
||||
|
||||
return normalized == 0.0 ? 0.0 : normalized;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为弧度。
|
||||
/// 返回值位于[-π, π)；正值表示逆时针，负值表示顺时针。
|
||||
/// </summary>
|
||||
public static double ShortestDifferenceRadians(
|
||||
double targetRadians,
|
||||
double currentRadians)
|
||||
{
|
||||
EnsureFinite(targetRadians, nameof(targetRadians));
|
||||
EnsureFinite(currentRadians, nameof(currentRadians));
|
||||
|
||||
return NormalizeRadians(targetRadians - currentRadians);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算从当前方向旋转到目标方向的最短有符号角度差，单位为度。
|
||||
/// 返回值位于[-180°, 180°)；正值表示逆时针，负值表示顺时针。
|
||||
/// </summary>
|
||||
public static double ShortestDifferenceDegrees(
|
||||
double targetDegrees,
|
||||
double currentDegrees)
|
||||
{
|
||||
EnsureFinite(targetDegrees, nameof(targetDegrees));
|
||||
EnsureFinite(currentDegrees, nameof(currentDegrees));
|
||||
|
||||
return NormalizeDegrees(targetDegrees - currentDegrees);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
|
||||
/// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
|
||||
/// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
|
||||
/// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
|
||||
/// </summary>
|
||||
public static double LerpRadians(
|
||||
double startRadians,
|
||||
double endRadians,
|
||||
double ratio)
|
||||
{
|
||||
EnsureFinite(startRadians, nameof(startRadians));
|
||||
EnsureFinite(endRadians, nameof(endRadians));
|
||||
EnsureFinite(ratio, nameof(ratio));
|
||||
|
||||
var shortestDifference = ShortestDifferenceRadians(
|
||||
endRadians,
|
||||
startRadians);
|
||||
|
||||
return NormalizeRadians(
|
||||
startRadians + ratio * shortestDifference);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度从度转换为弧度，不进行归一化。
|
||||
/// </summary>
|
||||
public static double DegreesToRadians(double angleDegrees)
|
||||
{
|
||||
EnsureFinite(angleDegrees, nameof(angleDegrees));
|
||||
return angleDegrees * Math.PI / 180.0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将角度从弧度转换为度，不进行归一化。
|
||||
/// </summary>
|
||||
public static double RadiansToDegrees(double angleRadians)
|
||||
{
|
||||
EnsureFinite(angleRadians, nameof(angleRadians));
|
||||
return angleRadians * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证角度是可用于计算的有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(double angle, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(angle) || double.IsInfinity(angle))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"角度必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
// 车体、运动、车队坐标系之间的转换
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
|
||||
/// 坐标系采用X向前、Y向左、逆时针为正的右手系。
|
||||
/// </summary>
|
||||
public static class FrameTransform2D
|
||||
{
|
||||
/// <summary>
|
||||
/// 将角度归一化到[-π, π)范围。
|
||||
/// </summary>
|
||||
public static double NormalizeAngle(double angleRadians)
|
||||
{
|
||||
return AngleMath.NormalizeRadians(angleRadians);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算从current到target的最短角度差。
|
||||
/// 返回正值表示逆时针旋转。
|
||||
/// </summary>
|
||||
public static double ShortestAngleDifference(
|
||||
double targetRadians,
|
||||
double currentRadians)
|
||||
{
|
||||
return AngleMath.ShortestDifferenceRadians(
|
||||
targetRadians,
|
||||
currentRadians);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将源坐标系中的点变换到目标坐标系。
|
||||
/// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
|
||||
/// </summary>
|
||||
public static Point2D TransformPoint(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Point2D pointInSource)
|
||||
{
|
||||
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||
|
||||
return new Point2D(
|
||||
sourcePoseInTarget.XMeters +
|
||||
cos * pointInSource.XMeters -
|
||||
sin * pointInSource.YMeters,
|
||||
|
||||
sourcePoseInTarget.YMeters +
|
||||
sin * pointInSource.XMeters +
|
||||
cos * pointInSource.YMeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将目标坐标系中的点反向变换到源坐标系。
|
||||
/// </summary>
|
||||
public static Point2D InverseTransformPoint(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Point2D pointInTarget)
|
||||
{
|
||||
var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
|
||||
|
||||
var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
|
||||
|
||||
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||
|
||||
return new Point2D(
|
||||
cos * dx + sin * dy,
|
||||
-sin * dx + cos * dy);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将源坐标系中的向量旋转到目标坐标系。
|
||||
/// 向量没有位置，因此不叠加平移量。
|
||||
/// </summary>
|
||||
public static Point2D TransformVector(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Point2D vectorInSource)
|
||||
{
|
||||
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||
|
||||
return new Point2D(
|
||||
cos * vectorInSource.XMeters -
|
||||
sin * vectorInSource.YMeters,
|
||||
|
||||
sin * vectorInSource.XMeters +
|
||||
cos * vectorInSource.YMeters);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 组合两级坐标变换。
|
||||
/// parentFromMiddle表示middle在parent中的位姿；
|
||||
/// middleFromChild表示child在middle中的位姿；
|
||||
/// 返回child在parent中的位姿。
|
||||
/// </summary>
|
||||
public static Pose2D Compose(
|
||||
Pose2D parentFromMiddle,
|
||||
Pose2D middleFromChild)
|
||||
{
|
||||
var childPositionInParent = TransformPoint(
|
||||
parentFromMiddle,
|
||||
middleFromChild.Position);
|
||||
|
||||
return new Pose2D(
|
||||
childPositionInParent.XMeters,
|
||||
childPositionInParent.YMeters,
|
||||
NormalizeAngle(
|
||||
parentFromMiddle.YawRadians +
|
||||
middleFromChild.YawRadians));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对坐标变换求逆。
|
||||
/// 输入child在parent中的位姿，返回parent在child中的位姿。
|
||||
/// </summary>
|
||||
public static Pose2D Inverse(Pose2D childPoseInParent)
|
||||
{
|
||||
var cos = Math.Cos(childPoseInParent.YawRadians);
|
||||
var sin = Math.Sin(childPoseInParent.YawRadians);
|
||||
|
||||
return new Pose2D(
|
||||
-cos * childPoseInParent.XMeters -
|
||||
sin * childPoseInParent.YMeters,
|
||||
|
||||
sin * childPoseInParent.XMeters -
|
||||
cos * childPoseInParent.YMeters,
|
||||
|
||||
NormalizeAngle(
|
||||
-childPoseInParent.YawRadians));
|
||||
}
|
||||
|
||||
|
||||
/// <summary>
|
||||
/// 将源坐标系中的位姿变换到目标坐标系。
|
||||
/// </summary>
|
||||
public static Pose2D TransformPose(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Pose2D poseInSource)
|
||||
{
|
||||
return Compose(sourcePoseInTarget, poseInSource);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 转换同一物理参考点处的速度表达坐标系。
|
||||
/// 只旋转线速度，角速度保持不变。
|
||||
/// </summary>
|
||||
public static Twist2D TransformTwistAtSamePoint(
|
||||
Pose2D sourcePoseInTarget,
|
||||
Twist2D twistInSource)
|
||||
{
|
||||
var linearVelocityInTarget = TransformVector(
|
||||
sourcePoseInTarget,
|
||||
new Point2D(
|
||||
twistInSource.VxMetersPerSecond,
|
||||
twistInSource.VyMetersPerSecond));
|
||||
|
||||
return new Twist2D(
|
||||
linearVelocityInTarget.XMeters,
|
||||
linearVelocityInTarget.YMeters,
|
||||
twistInSource.OmegaRadiansPerSecond);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
|
||||
using System;
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 提供与具体业务和坐标系无关的基础插值功能。
|
||||
/// </summary>
|
||||
public static class InterpolationMath
|
||||
{
|
||||
/// <summary>
|
||||
/// 将插值比例限制到[0, 1]闭区间。
|
||||
/// </summary>
|
||||
public static double Clamp01(double value)
|
||||
{
|
||||
EnsureFinite(value, nameof(value));
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
if (value >= 1.0)
|
||||
{
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对两个标量执行线性插值。
|
||||
/// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio，
|
||||
/// 因此也支持区间外的线性外插。
|
||||
/// </summary>
|
||||
public static double Lerp(
|
||||
double start,
|
||||
double end,
|
||||
double ratio)
|
||||
{
|
||||
EnsureFinite(start, nameof(start));
|
||||
EnsureFinite(end, nameof(end));
|
||||
EnsureFinite(ratio, nameof(ratio));
|
||||
|
||||
return start + ratio * (end - start);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证输入是可用于插值计算的有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(double value, string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) || double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"插值参数必须是有限数值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
// 纯数据层：只描述坐标、速度和命令
|
||||
// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
|
||||
// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。
|
||||
// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。
|
||||
// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。
|
||||
|
||||
namespace MyParking.Shared
|
||||
{
|
||||
/// <summary>
|
||||
/// 二维坐标点，X、Y单位均为米。
|
||||
/// </summary>
|
||||
public readonly struct Point2D
|
||||
{
|
||||
public Point2D(double xMeters, double yMeters)
|
||||
{
|
||||
XMeters = xMeters;
|
||||
YMeters = yMeters;
|
||||
}
|
||||
|
||||
public double XMeters { get; }
|
||||
|
||||
public double YMeters { get; }
|
||||
|
||||
public static Point2D Zero => new Point2D(0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二维局部坐标系在父坐标系中的位姿。
|
||||
/// 位置单位为米，朝向单位为弧度，逆时针为正。
|
||||
/// 具体父子关系由变量名称说明，例如RadarPoseInBody。
|
||||
/// </summary>
|
||||
public readonly struct Pose2D
|
||||
{
|
||||
public Pose2D(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double yawRadians)
|
||||
{
|
||||
XMeters = xMeters;
|
||||
YMeters = yMeters;
|
||||
YawRadians = yawRadians;
|
||||
}
|
||||
|
||||
public double XMeters { get; }
|
||||
|
||||
public double YMeters { get; }
|
||||
|
||||
public double YawRadians { get; }
|
||||
|
||||
public Point2D Position =>
|
||||
new Point2D(XMeters, YMeters);
|
||||
|
||||
public static Pose2D Identity =>
|
||||
new Pose2D(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 二维刚体速度。
|
||||
/// 线速度单位为m/s，角速度单位为rad/s。
|
||||
/// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
|
||||
/// </summary>
|
||||
public readonly struct Twist2D
|
||||
{
|
||||
public Twist2D(
|
||||
double vxMetersPerSecond,
|
||||
double vyMetersPerSecond,
|
||||
double omegaRadiansPerSecond)
|
||||
{
|
||||
VxMetersPerSecond = vxMetersPerSecond;
|
||||
VyMetersPerSecond = vyMetersPerSecond;
|
||||
OmegaRadiansPerSecond = omegaRadiansPerSecond;
|
||||
}
|
||||
|
||||
public double VxMetersPerSecond { get; }
|
||||
|
||||
public double VyMetersPerSecond { get; }
|
||||
|
||||
public double OmegaRadiansPerSecond { get; }
|
||||
|
||||
public static Twist2D Zero =>
|
||||
new Twist2D(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 发送给单辆车的车体坐标系速度命令。
|
||||
/// </summary>
|
||||
public readonly struct ChassisCommand
|
||||
{
|
||||
public ChassisCommand(
|
||||
int vehicleId,
|
||||
Twist2D bodyTwist)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
BodyTwist = bodyTwist;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
|
||||
/// </summary>
|
||||
public Twist2D BodyTwist { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建指定车辆的停止命令。
|
||||
/// </summary>
|
||||
public static ChassisCommand Stop(int vehicleId)
|
||||
{
|
||||
return new ChassisCommand(
|
||||
vehicleId,
|
||||
Twist2D.Zero);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 单辆车的车体坐标系在车队坐标系中的位姿。
|
||||
/// </summary>
|
||||
public readonly struct VehicleLayout
|
||||
{
|
||||
public VehicleLayout(
|
||||
int vehicleId,
|
||||
Pose2D poseInFleet)
|
||||
{
|
||||
VehicleId = vehicleId;
|
||||
PoseInFleet = poseInFleet;
|
||||
}
|
||||
|
||||
public int VehicleId { get; }
|
||||
|
||||
public Pose2D PoseInFleet { get; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 车队整体运动命令，速度分量均在车队坐标系中表达。
|
||||
/// </summary>
|
||||
public readonly struct FleetMotionCommand
|
||||
{
|
||||
public FleetMotionCommand(
|
||||
Point2D referencePointInFleet,
|
||||
Twist2D twistAtReferencePoint)
|
||||
{
|
||||
ReferencePointInFleet = referencePointInFleet;
|
||||
TwistAtReferencePoint = twistAtReferencePoint;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 速度命令对应的参考点，也可作为自定义旋转中心。
|
||||
/// </summary>
|
||||
public Point2D ReferencePointInFleet { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 参考点处的车队速度。
|
||||
/// </summary>
|
||||
public Twist2D TwistAtReferencePoint { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 创建绕指定中心原地旋转的车队命令。
|
||||
/// </summary>
|
||||
public static FleetMotionCommand RotateAround(
|
||||
Point2D rotationCenterInFleet,
|
||||
double omegaRadiansPerSecond)
|
||||
{
|
||||
return new FleetMotionCommand(
|
||||
rotationCenterInFleet,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建车队停止命令。
|
||||
/// </summary>
|
||||
public static FleetMotionCommand Stop()
|
||||
{
|
||||
return new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
Twist2D.Zero);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user