65 lines
2.3 KiB
C#
65 lines
2.3 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)
|
|
{
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
arcLengthMeters,
|
|
nameof(arcLengthMeters));
|
|
NumericGuard.EnsureFinite(
|
|
poseInWorld,
|
|
nameof(poseInWorld));
|
|
NumericGuard.EnsureFinite(
|
|
curvaturePerMeter,
|
|
nameof(curvaturePerMeter));
|
|
NumericGuard.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; }
|
|
|
|
}
|
|
}
|