103 lines
3.2 KiB
C#
103 lines
3.2 KiB
C#
using System;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Trajectory
|
|
{
|
|
/// <summary>
|
|
/// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
|
|
/// </summary>
|
|
public readonly struct TrajectoryPoint
|
|
{
|
|
/// <summary>
|
|
/// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
|
|
/// </summary>
|
|
public TrajectoryPoint(
|
|
double arcLengthMeters,
|
|
Pose2D poseInWorld,
|
|
double curvaturePerMeter,
|
|
double referenceSpeedMetersPerSecond)
|
|
{
|
|
EnsureFiniteNonNegative(
|
|
arcLengthMeters,
|
|
nameof(arcLengthMeters));
|
|
EnsureFinite(
|
|
poseInWorld.XMeters,
|
|
nameof(poseInWorld));
|
|
EnsureFinite(
|
|
poseInWorld.YMeters,
|
|
nameof(poseInWorld));
|
|
EnsureFinite(
|
|
poseInWorld.YawRadians,
|
|
nameof(poseInWorld));
|
|
EnsureFinite(
|
|
curvaturePerMeter,
|
|
nameof(curvaturePerMeter));
|
|
EnsureFinite(
|
|
referenceSpeedMetersPerSecond,
|
|
nameof(referenceSpeedMetersPerSecond));
|
|
ArcLengthMeters = arcLengthMeters;
|
|
PoseInWorld = new Pose2D(
|
|
poseInWorld.XMeters,
|
|
poseInWorld.YMeters,
|
|
AngleMath.NormalizeRadians(
|
|
poseInWorld.YawRadians));
|
|
CurvaturePerMeter = curvaturePerMeter;
|
|
ReferenceSpeedMetersPerSecond =
|
|
referenceSpeedMetersPerSecond;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取从轨迹起点累计到当前点的弧长，单位为m。
|
|
/// </summary>
|
|
public double ArcLengthMeters { get; }
|
|
|
|
/// <summary>
|
|
/// 获取车体中心参考坐标系在世界坐标系中的位姿。
|
|
/// </summary>
|
|
public Pose2D PoseInWorld { get; }
|
|
|
|
/// <summary>
|
|
/// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
|
|
/// </summary>
|
|
public double CurvaturePerMeter { get; }
|
|
|
|
/// <summary>
|
|
/// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
|
|
/// </summary>
|
|
public double ReferenceSpeedMetersPerSecond { get; }
|
|
|
|
/// <summary>
|
|
/// 检查数值是否为有限值。
|
|
/// </summary>
|
|
private static void EnsureFinite(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
if (double.IsNaN(value) ||
|
|
double.IsInfinity(value))
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"轨迹点参数必须是有限值。");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 检查数值是否为非负有限值。
|
|
/// </summary>
|
|
private static void EnsureFiniteNonNegative(
|
|
double value,
|
|
string parameterName)
|
|
{
|
|
EnsureFinite(value, parameterName);
|
|
|
|
if (value < 0.0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
parameterName,
|
|
"轨迹累计弧长不能为负数。");
|
|
}
|
|
}
|
|
}
|
|
}
|