69 lines
2.9 KiB
C#
69 lines
2.9 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
namespace TrajectoryOutputDemo;
|
|
|
|
/// <summary>供外部控制模块消费的单个只读轨迹点；全部字段直接来自已验证的 <see cref="EmTrajectoryPoint"/>。</summary>
|
|
public sealed class ControlTrajectoryPoint
|
|
{
|
|
internal ControlTrajectoryPoint(EmTrajectoryPoint source)
|
|
{
|
|
TimeFromStartSeconds = source.TimeFromStart;
|
|
XMeters = source.X;
|
|
YMeters = source.Y;
|
|
YawRadians = source.Yaw;
|
|
SignedLongitudinalVelocityMetersPerSecond = source.SignedLongitudinalVelocity;
|
|
YawRateRadiansPerSecond = source.YawRate;
|
|
CurvaturePerMeter = source.VehicleCurvature;
|
|
Direction = source.Direction;
|
|
SegmentIndex = source.SegmentIndex;
|
|
PathSMeters = source.PathS;
|
|
BoundaryType = source.BoundaryType;
|
|
}
|
|
|
|
public double TimeFromStartSeconds { get; }
|
|
public double XMeters { get; }
|
|
public double YMeters { get; }
|
|
public double YawRadians { get; }
|
|
public double SignedLongitudinalVelocityMetersPerSecond { get; }
|
|
public double YawRateRadiansPerSecond { get; }
|
|
public double CurvaturePerMeter { get; }
|
|
public TravelDirection Direction { get; }
|
|
public int SegmentIndex { get; }
|
|
public double PathSMeters { get; }
|
|
public EmBoundaryType BoundaryType { get; }
|
|
}
|
|
|
|
/// <summary>轨迹元数据与控制点序列的不可变组合；它不包含也不发送任何硬件命令。</summary>
|
|
public sealed class ControlTrajectorySequence
|
|
{
|
|
internal ControlTrajectorySequence(EmTrajectoryMetadata metadata, IReadOnlyList<ControlTrajectoryPoint> points)
|
|
{
|
|
Metadata = metadata ?? throw new ArgumentNullException(nameof(metadata));
|
|
Points = points ?? throw new ArgumentNullException(nameof(points));
|
|
}
|
|
|
|
public EmTrajectoryMetadata Metadata { get; }
|
|
public IReadOnlyList<ControlTrajectoryPoint> Points { get; }
|
|
}
|
|
|
|
/// <summary>将不可变 EM 轨迹投影为控制模块可引用的只读序列，不进行采样、插值或底盘协议转换。</summary>
|
|
public sealed class ControlModuleTrajectoryAdapter
|
|
{
|
|
/// <summary>逐点复制公开控制字段；调用方只能在完整非空轨迹上调用此方法。</summary>
|
|
public ControlTrajectorySequence Create(EmTrajectory trajectory)
|
|
{
|
|
if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
|
|
throw new ArgumentException("必须提供完整非空的 EM 轨迹。", nameof(trajectory));
|
|
|
|
var points = new List<ControlTrajectoryPoint>(trajectory.Points.Count);
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
points.Add(new ControlTrajectoryPoint(trajectory.Points[index]));
|
|
return new ControlTrajectorySequence(trajectory.Metadata,
|
|
new ReadOnlyCollection<ControlTrajectoryPoint>(points));
|
|
}
|
|
}
|