100 lines
5.1 KiB
C#
100 lines
5.1 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Threading;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
||
|
||
namespace TrajectoryOutputDemo;
|
||
|
||
/// <summary>一次 Demo 运行的结果；成功时同时提供原始 EM 轨迹、控制只读序列和已完成的 CSV 路径。</summary>
|
||
public sealed class TrajectoryOutputDemoResult
|
||
{
|
||
private TrajectoryOutputDemoResult(bool succeeded, string diagnostic, EmTrajectory? trajectory,
|
||
ControlTrajectorySequence? controlTrajectory, string? csvPath)
|
||
{
|
||
Succeeded = succeeded;
|
||
Diagnostic = diagnostic ?? string.Empty;
|
||
Trajectory = trajectory;
|
||
ControlTrajectory = controlTrajectory;
|
||
CsvPath = csvPath;
|
||
}
|
||
|
||
public bool Succeeded { get; }
|
||
public string Diagnostic { get; }
|
||
public EmTrajectory? Trajectory { get; }
|
||
public ControlTrajectorySequence? ControlTrajectory { get; }
|
||
public string? CsvPath { get; }
|
||
|
||
internal static TrajectoryOutputDemoResult Failure(string diagnostic) => new(false, diagnostic, null, null, null);
|
||
internal static TrajectoryOutputDemoResult Success(EmTrajectory trajectory, ControlTrajectorySequence sequence, string csvPath) =>
|
||
new(true, string.Empty, trajectory, sequence, csvPath);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 编排一次真实轨迹输出：粗路径、Local G2 平滑、真实 OSQP EM 规划、控制序列投影和 CSV 导出。
|
||
/// 它只处理冻结的演示输入，绝不读取硬件状态或发送控制命令。
|
||
/// </summary>
|
||
public sealed class TrajectoryOutputDemoRunner
|
||
{
|
||
/// <summary>在配置定义的演示场景运行一次完整规划；失败时不导出任何轨迹文件。</summary>
|
||
public TrajectoryOutputDemoResult Run(TrajectoryOutputDemoConfiguration configuration)
|
||
{
|
||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||
|
||
CoarsePathPlanningJob job = CreateCoarseJob(configuration);
|
||
TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
|
||
if (!bootstrap.Succeeded)
|
||
return TrajectoryOutputDemoResult.Failure("粗路径或平滑阶段失败：" + bootstrap.FailureReason);
|
||
if (bootstrap.Segments.Count == 0)
|
||
return TrajectoryOutputDemoResult.Failure("平滑路径没有可供 EM 规划的方向段。");
|
||
|
||
DateTimeOffset now = DateTimeOffset.UtcNow;
|
||
DirectionSegmentView segment = bootstrap.Segments[0];
|
||
var state = new VehicleMotionState(configuration.Start, configuration.InitialSignedSpeedMetersPerSecond,
|
||
0d, now, 1L);
|
||
var request = new EmPlanningRequest(bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state,
|
||
EmPlannerConfiguration.CreateDefault(), segment.SegmentIndex, null, now, now,
|
||
"trajectory-output-demo-" + now.ToUnixTimeMilliseconds(), "trajectory-output-demo-reference", string.Empty,
|
||
EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.FullDirectionSegment);
|
||
EmPlanningResult result = new EmPlanningService(new OsqpNativeSolver()).Plan(request, CancellationToken.None);
|
||
bool accepted = (result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback) &&
|
||
result.Trajectory != null && result.Trajectory.Points.Count > 0;
|
||
if (!accepted)
|
||
return TrajectoryOutputDemoResult.Failure("EM 规划失败：" + result.Status + "；" + result.FailureReason);
|
||
|
||
EmTrajectory trajectory = result.Trajectory!;
|
||
ControlTrajectorySequence sequence = new ControlModuleTrajectoryAdapter().Create(trajectory);
|
||
string csvPath = new TrajectorySequenceExporter().Export(sequence, configuration.CsvOutputPath);
|
||
return TrajectoryOutputDemoResult.Success(trajectory, sequence, csvPath);
|
||
}
|
||
|
||
private static CoarsePathPlanningJob CreateCoarseJob(TrajectoryOutputDemoConfiguration configuration)
|
||
{
|
||
return new CoarsePathPlanningJob
|
||
{
|
||
MapRequest = new PlanningMapRequest
|
||
{
|
||
Bounds = configuration.MapBounds,
|
||
ResolutionMm = configuration.MapResolutionMillimeters,
|
||
ObstacleSources = Array.Empty<IMapObstacleSource>(),
|
||
AllowExplicitEmptyMap = true,
|
||
},
|
||
Start = configuration.Start,
|
||
Goal = configuration.Goal,
|
||
Vehicle = new VehicleParameters
|
||
{
|
||
LengthMeters = configuration.VehicleLengthMeters,
|
||
WidthMeters = configuration.VehicleWidthMeters,
|
||
SafetyMarginMeters = configuration.SafetyMarginMeters,
|
||
MaximumCurvaturePerMeter = configuration.MaximumCurvaturePerMeter,
|
||
},
|
||
Configuration = new HybridAStarConfiguration(),
|
||
StartDirection = TravelDirection.Forward,
|
||
GoalDirection = GoalDirectionConstraint.Forward,
|
||
};
|
||
}
|
||
}
|