46 lines
2.1 KiB
C#
46 lines
2.1 KiB
C#
|
using System;
|
||
|
|
using System.IO;
|
||
|
|
using TrajectoryOutputDemo;
|
||
|
|
|
||
|
|
namespace TrajectoryOutputDemoTests;
|
||
|
|
|
||
|
|
internal static class Program
|
||
|
|
{
|
||
|
|
private static int Main()
|
||
|
|
{
|
||
|
|
string temporaryDirectory = Path.Combine(Path.GetTempPath(), "trajectory-output-demo-tests", Guid.NewGuid().ToString("N"));
|
||
|
|
try
|
||
|
|
{
|
||
|
|
Directory.CreateDirectory(temporaryDirectory);
|
||
|
|
string csvPath = Path.Combine(temporaryDirectory, "trajectory.csv");
|
||
|
|
var configuration = TrajectoryOutputDemoConfiguration.CreateDefault(csvPath);
|
||
|
|
TrajectoryOutputDemoResult result = new TrajectoryOutputDemoRunner().Run(configuration);
|
||
|
|
|
||
|
|
if (!result.Succeeded)
|
||
|
|
throw new InvalidOperationException("真实 OSQP 规划未成功：" + result.Diagnostic);
|
||
|
|
if (!File.Exists(csvPath))
|
||
|
|
throw new InvalidOperationException("成功规划必须导出 CSV。");
|
||
|
|
ControlTrajectorySequence sequence = result.ControlTrajectory ??
|
||
|
|
throw new InvalidOperationException("成功规划必须提供控制模块序列。");
|
||
|
|
var trajectory = result.Trajectory ??
|
||
|
|
throw new InvalidOperationException("成功规划必须提供 EM 轨迹。");
|
||
|
|
if (sequence.Points.Count != trajectory.Points.Count)
|
||
|
|
throw new InvalidOperationException("控制序列必须逐点对应 EM 轨迹。");
|
||
|
|
|
||
|
|
string[] lines = File.ReadAllLines(csvPath);
|
||
|
|
if (lines.Length < 2)
|
||
|
|
throw new InvalidOperationException("CSV 必须包含表头和至少一个轨迹点。");
|
||
|
|
if (lines[0] != "time_s,x_m,y_m,yaw_rad,signed_velocity_mps,yaw_rate_radps,curvature_per_m,direction,segment_index,path_s_m,boundary_type")
|
||
|
|
throw new InvalidOperationException("CSV 表头不符合控制模块契约。");
|
||
|
|
|
||
|
|
Console.WriteLine("PASS trajectory-output-demo");
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
finally
|
||
|
|
{
|
||
|
|
if (Directory.Exists(temporaryDirectory))
|
||
|
|
Directory.Delete(temporaryDirectory, true);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|