61 lines
2.4 KiB
C#
61 lines
2.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.Trajectory;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
/// <summary>将冻结的单方向段 EM 轨迹转换为几何控制器使用的 SI 单位有符号速度轨迹。</summary>
|
|
public sealed class EmControlTrajectoryAdapter
|
|
{
|
|
private const double MinimumSegmentLengthMeters = 1e-6d;
|
|
|
|
/// <summary>按世界位置重建从零开始的弧长，同时保留航向、曲率以及前进为正倒车为负的参考速度。</summary>
|
|
public Trajectory2D Create(EmTrajectory trajectory)
|
|
{
|
|
if (trajectory == null)
|
|
throw new ArgumentNullException(nameof(trajectory));
|
|
|
|
var points = new List<TrajectoryPoint>(trajectory.Points.Count);
|
|
double arcLengthMeters = 0d;
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint source = trajectory.Points[index];
|
|
var pose = new Pose2D(source.X, source.Y, source.Yaw);
|
|
var converted = new TrajectoryPoint(arcLengthMeters, pose,
|
|
source.VehicleCurvature, source.SignedLongitudinalVelocity);
|
|
|
|
if (points.Count == 0)
|
|
{
|
|
points.Add(converted);
|
|
continue;
|
|
}
|
|
|
|
TrajectoryPoint previous = points[points.Count - 1];
|
|
double deltaX = pose.XMeters - previous.PoseInWorld.XMeters;
|
|
double deltaY = pose.YMeters - previous.PoseInWorld.YMeters;
|
|
double distanceMeters = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
if (distanceMeters < MinimumSegmentLengthMeters)
|
|
{
|
|
points[points.Count - 1] = new TrajectoryPoint(
|
|
previous.ArcLengthMeters, pose, source.VehicleCurvature,
|
|
source.SignedLongitudinalVelocity);
|
|
continue;
|
|
}
|
|
|
|
arcLengthMeters += distanceMeters;
|
|
points.Add(new TrajectoryPoint(arcLengthMeters, pose,
|
|
source.VehicleCurvature, source.SignedLongitudinalVelocity));
|
|
}
|
|
|
|
if (points.Count < 2)
|
|
{
|
|
throw new ArgumentException(
|
|
"EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));
|
|
}
|
|
|
|
return new Trajectory2D(points);
|
|
}
|
|
}
|