feat: observe one full EM direction segment

This commit is contained in:
梁薄云
2026-08-07 08:56:14 +08:00
parent 57d8abb3e1
commit ab3204bfde
6 changed files with 289 additions and 20 deletions
@@ -2,19 +2,21 @@ using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
public sealed class TrajectoryObservationSettings
{
public EmPlanningScope PlanningScope { get; set; } = EmPlanningScope.FullDirectionSegment;
public double MapPaddingMeters { get; set; } = 2d;
public float MapResolutionMillimeters { get; set; } = 50f;
public double ReplanPeriodSeconds { get; set; } = 0.20d;
public double ObserverPeriodSeconds { get; set; } = 0.05d;
public double SolverTimeoutSeconds { get; set; } = 5d;
public int MaximumOsqpIterations { get; set; } = 100000;
/// <summary>单次 ST 轨迹覆盖的未来时长,单位 s;默认 2 s,不等于观察循环周期。</summary>
/// <summary>仅滚动视窗兼容字段;完整方向段模式不用于截断。单次 ST 轨迹覆盖的未来时长,单位 s;默认 2 s,不等于观察循环周期。</summary>
public double TimeHorizonSeconds { get; set; } = 2d;
/// <summary>发布 Trajectory 相邻 TimeFromStart 时间戳的间隔,单位 s;不等于观察循环周期。</summary>
public double OutputTimeStepSeconds { get; set; } = 0.10d;
@@ -22,7 +24,7 @@ public sealed class TrajectoryObservationSettings
public double VehicleWidthMeters { get; set; } = 0.60d;
public double SafetyMarginMeters { get; set; } = 0.05d;
public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;
public bool EnableWebVisualization { get; set; } = false;
public bool EnableWebVisualization { get; set; } = true;
public bool AutoOpenWebVisualization { get; set; } = true;
public int WebVisualizationPort { get; set; } = 0;
public double WebRefreshRateHz { get; set; } = 10d;
@@ -37,6 +39,7 @@ public sealed class TrajectoryObservationSettings
{
var snapshot = new TrajectoryObservationSettings
{
PlanningScope = PlanningScope,
MapPaddingMeters = MapPaddingMeters,
MapResolutionMillimeters = MapResolutionMillimeters,
ReplanPeriodSeconds = ReplanPeriodSeconds,
@@ -66,6 +69,8 @@ public sealed class TrajectoryObservationSettings
public void Validate()
{
if (!Enum.IsDefined(typeof(EmPlanningScope), PlanningScope))
throw new ArgumentOutOfRangeException(nameof(PlanningScope), "Value must be a defined planning scope.");
EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters));
EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters));
EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds));