feat: add planning visualization contracts

2026-08-06 09:26:06 +08:00
parent fa4938b3fc
commit 55d8e1eebf
10 changed files with 581 additions and 0 deletions
@@ -12,11 +12,17 @@
  </ItemGroup>

  <ItemGroup>
+    <Compile Remove="TrajectoryPlanningVisualization\**\*.cs" />
+    <Compile Remove="tests\TrajectoryPlanningVisualizationVerificationHost\**\*.cs" />
    <Compile Remove="tests\EMPlannerVerificationHost\**\*.cs" />
    <Compile Remove="ParkrobTrajplanner\auto_avoidance\**\*.cs"
             Condition="'$(ExcludeLegacyAutoAvoidance)' == 'true'" />
  </ItemGroup>

+
+  <ItemGroup>
+    <ProjectReference Include="TrajectoryPlanningVisualization\TrajectoryPlanningVisualization.csproj" />
+  </ItemGroup>
  <ItemGroup>
    <Reference Include="CommonUsage">
      <HintPath>ref\CommonUsage.dll</HintPath>
@@ -0,0 +1,101 @@
+using System;
+using System.Collections.Generic;
+
+namespace TrajectoryPlanningVisualization;
+
+public sealed class PlanningVisualizationStaticSnapshot
+{
+    public PlanningVisualizationStaticSnapshot(string sessionNameChinese, VisualizationBounds worldBounds,
+        VisualizationOccupancyGrid occupancyGrid, IReadOnlyList<VisualizationPolyline> staticPolylines,
+        IReadOnlyList<VisualizationMarker> staticMarkers,
+        IReadOnlyList<VisualizationDirectionSegment> directionSegments,
+        IReadOnlyList<VisualizationConfigurationGroup> configurationGroups)
+    {
+        SessionNameChinese = VisualizationContract.RequireText(sessionNameChinese, nameof(sessionNameChinese));
+        WorldBounds = worldBounds ?? throw new ArgumentNullException(nameof(worldBounds));
+        OccupancyGrid = occupancyGrid;
+        StaticPolylines = VisualizationContract.Copy(staticPolylines, nameof(staticPolylines));
+        StaticMarkers = VisualizationContract.Copy(staticMarkers, nameof(staticMarkers));
+        DirectionSegments = VisualizationContract.Copy(directionSegments, nameof(directionSegments));
+        ConfigurationGroups = VisualizationContract.Copy(configurationGroups, nameof(configurationGroups));
+    }
+
+    public string SessionNameChinese { get; }
+    public VisualizationBounds WorldBounds { get; }
+    public VisualizationOccupancyGrid OccupancyGrid { get; }
+    public IReadOnlyList<VisualizationPolyline> StaticPolylines { get; }
+    public IReadOnlyList<VisualizationMarker> StaticMarkers { get; }
+    public IReadOnlyList<VisualizationDirectionSegment> DirectionSegments { get; }
+    public IReadOnlyList<VisualizationConfigurationGroup> ConfigurationGroups { get; }
+}
+
+public sealed class PlanningVisualizationDynamicSnapshot
+{
+    public PlanningVisualizationDynamicSnapshot(long sequence, DateTimeOffset observedAtUtc,
+        string sessionStateChinese, int activeSegmentIndex, string activeDirection,
+        VisualizationPose vehiclePose, IReadOnlyList<VisualizationPolyline> dynamicPolylines,
+        IReadOnlyList<VisualizationMarker> dynamicMarkers, IReadOnlyList<VisualizationChart> charts,
+        IReadOnlyList<VisualizationValue> statusValues, VisualizationCycleSummary cycleSummary)
+    {
+        Sequence = sequence;
+        ObservedAtUtc = observedAtUtc;
+        SessionStateChinese = VisualizationContract.RequireText(sessionStateChinese, nameof(sessionStateChinese));
+        ActiveSegmentIndex = activeSegmentIndex;
+        ActiveDirection = VisualizationContract.RequireText(activeDirection, nameof(activeDirection));
+        VehiclePose = vehiclePose ?? throw new ArgumentNullException(nameof(vehiclePose));
+        DynamicPolylines = VisualizationContract.Copy(dynamicPolylines, nameof(dynamicPolylines));
+        DynamicMarkers = VisualizationContract.Copy(dynamicMarkers, nameof(dynamicMarkers));
+        Charts = VisualizationContract.Copy(charts, nameof(charts));
+        StatusValues = VisualizationContract.Copy(statusValues, nameof(statusValues));
+        CycleSummary = cycleSummary ?? throw new ArgumentNullException(nameof(cycleSummary));
+    }
+
+    public long Sequence { get; }
+    public DateTimeOffset ObservedAtUtc { get; }
+    public string SessionStateChinese { get; }
+    public int ActiveSegmentIndex { get; }
+    public string ActiveDirection { get; }
+    public VisualizationPose VehiclePose { get; }
+    public IReadOnlyList<VisualizationPolyline> DynamicPolylines { get; }
+    public IReadOnlyList<VisualizationMarker> DynamicMarkers { get; }
+    public IReadOnlyList<VisualizationChart> Charts { get; }
+    public IReadOnlyList<VisualizationValue> StatusValues { get; }
+    public VisualizationCycleSummary CycleSummary { get; }
+}
+
+public sealed class VisualizationCycleSummary
+{
+    public VisualizationCycleSummary(long cycleVersion, DateTimeOffset occurredAtUtc, string status, bool published,
+        double planningElapsedMilliseconds, int segmentIndex, string direction, string longitudinalMode,
+        string terminalType, double? terminalVelocity, double? terminalAcceleration, string failureReason)
+    {
+        VisualizationContract.RequireFinite(planningElapsedMilliseconds, nameof(planningElapsedMilliseconds));
+        VisualizationContract.RequireFinite(terminalVelocity, nameof(terminalVelocity));
+        VisualizationContract.RequireFinite(terminalAcceleration, nameof(terminalAcceleration));
+        CycleVersion = cycleVersion;
+        OccurredAtUtc = occurredAtUtc;
+        Status = VisualizationContract.RequireText(status, nameof(status));
+        Published = published;
+        PlanningElapsedMilliseconds = planningElapsedMilliseconds;
+        SegmentIndex = segmentIndex;
+        Direction = VisualizationContract.RequireText(direction, nameof(direction));
+        LongitudinalMode = VisualizationContract.RequireText(longitudinalMode, nameof(longitudinalMode));
+        TerminalType = VisualizationContract.RequireText(terminalType, nameof(terminalType));
+        TerminalVelocity = terminalVelocity;
+        TerminalAcceleration = terminalAcceleration;
+        FailureReason = VisualizationContract.RequireText(failureReason, nameof(failureReason));
+    }
+
+    public long CycleVersion { get; }
+    public DateTimeOffset OccurredAtUtc { get; }
+    public string Status { get; }
+    public bool Published { get; }
+    public double PlanningElapsedMilliseconds { get; }
+    public int SegmentIndex { get; }
+    public string Direction { get; }
+    public string LongitudinalMode { get; }
+    public string TerminalType { get; }
+    public double? TerminalVelocity { get; }
+    public double? TerminalAcceleration { get; }
+    public string FailureReason { get; }
+}
@@ -0,0 +1,48 @@
+using System.Collections.Generic;
+
+namespace TrajectoryPlanningVisualization;
+
+public enum VisualizationLineStyle
+{
+    Solid,
+    Dashed,
+    Limit
+}
+
+public sealed class VisualizationSeries
+{
+    public VisualizationSeries(string id, string legend, VisualizationLineStyle lineStyle,
+        IReadOnlyList<VisualizationPoint> points)
+    {
+        Id = VisualizationContract.RequireText(id, nameof(id));
+        Legend = VisualizationContract.RequireText(legend, nameof(legend));
+        LineStyle = lineStyle;
+        Points = VisualizationContract.Copy(points, nameof(points));
+    }
+
+    public string Id { get; }
+    public string Legend { get; }
+    public VisualizationLineStyle LineStyle { get; }
+    public IReadOnlyList<VisualizationPoint> Points { get; }
+}
+
+public sealed class VisualizationChart
+{
+    public VisualizationChart(string id, string chineseTitle, string xAxisLabel, string yAxisLabel,
+        IReadOnlyList<VisualizationSeries> series, string noteChinese = "")
+    {
+        Id = VisualizationContract.RequireText(id, nameof(id));
+        ChineseTitle = VisualizationContract.RequireText(chineseTitle, nameof(chineseTitle));
+        XAxisLabel = VisualizationContract.RequireText(xAxisLabel, nameof(xAxisLabel));
+        YAxisLabel = VisualizationContract.RequireText(yAxisLabel, nameof(yAxisLabel));
+        Series = VisualizationContract.Copy(series, nameof(series));
+        NoteChinese = VisualizationContract.RequireText(noteChinese, nameof(noteChinese));
+    }
+
+    public string Id { get; }
+    public string ChineseTitle { get; }
+    public string XAxisLabel { get; }
+    public string YAxisLabel { get; }
+    public IReadOnlyList<VisualizationSeries> Series { get; }
+    public string NoteChinese { get; }
+}
@@ -0,0 +1,107 @@
+using System;
+using System.Collections.Generic;
+
+namespace TrajectoryPlanningVisualization;
+
+public sealed class VisualizationPolyline
+{
+    public VisualizationPolyline(string id, string legendChinese, string kind, VisualizationLineStyle lineStyle,
+        IReadOnlyList<VisualizationPoint> points)
+    {
+        Id = VisualizationContract.RequireText(id, nameof(id));
+        LegendChinese = VisualizationContract.RequireText(legendChinese, nameof(legendChinese));
+        Kind = VisualizationContract.RequireText(kind, nameof(kind));
+        LineStyle = lineStyle;
+        Points = VisualizationContract.Copy(points, nameof(points));
+    }
+
+    public string Id { get; }
+    public string LegendChinese { get; }
+    public string Kind { get; }
+    public VisualizationLineStyle LineStyle { get; }
+    public IReadOnlyList<VisualizationPoint> Points { get; }
+}
+
+public sealed class VisualizationMarker
+{
+    public VisualizationMarker(string id, string kind, string labelChinese, VisualizationPoint position)
+    {
+        Id = VisualizationContract.RequireText(id, nameof(id));
+        Kind = VisualizationContract.RequireText(kind, nameof(kind));
+        LabelChinese = VisualizationContract.RequireText(labelChinese, nameof(labelChinese));
+        Position = position ?? throw new ArgumentNullException(nameof(position));
+    }
+
+    public string Id { get; }
+    public string Kind { get; }
+    public string LabelChinese { get; }
+    public VisualizationPoint Position { get; }
+}
+
+public sealed class VisualizationDirectionSegment
+{
+    public VisualizationDirectionSegment(int segmentIndex, string direction, bool startsAtGearSwitch,
+        bool endsAtGearSwitch, IReadOnlyList<VisualizationPoint> points)
+    {
+        SegmentIndex = segmentIndex;
+        Direction = VisualizationContract.RequireText(direction, nameof(direction));
+        StartsAtGearSwitch = startsAtGearSwitch;
+        EndsAtGearSwitch = endsAtGearSwitch;
+        Points = VisualizationContract.Copy(points, nameof(points));
+    }
+
+    public int SegmentIndex { get; }
+    public string Direction { get; }
+    public bool StartsAtGearSwitch { get; }
+    public bool EndsAtGearSwitch { get; }
+    public IReadOnlyList<VisualizationPoint> Points { get; }
+}
+
+public sealed class VisualizationOccupancyGrid
+{
+    public VisualizationOccupancyGrid(VisualizationBounds bounds, double resolutionMeters, int rows, int columns,
+        byte[] occupancyBits)
+    {
+        Bounds = bounds ?? throw new ArgumentNullException(nameof(bounds));
+        VisualizationContract.RequireFinite(resolutionMeters, nameof(resolutionMeters));
+        if (resolutionMeters <= 0d)
+        {
+            throw new ArgumentOutOfRangeException(nameof(resolutionMeters));
+        }
+
+        if (rows <= 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(rows));
+        }
+
+        if (columns <= 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(columns));
+        }
+
+        if (occupancyBits == null)
+        {
+            throw new ArgumentNullException(nameof(occupancyBits));
+        }
+
+        long cellCount = (long)rows * columns;
+        long requiredByteCount = (cellCount + 7L) / 8L;
+        if (occupancyBits.Length != requiredByteCount)
+        {
+            throw new ArgumentException("Occupancy bit count does not match the grid dimensions.", nameof(occupancyBits));
+        }
+
+        var bitsCopy = new byte[occupancyBits.Length];
+        Buffer.BlockCopy(occupancyBits, 0, bitsCopy, 0, bitsCopy.Length);
+        ResolutionMeters = resolutionMeters;
+        Rows = rows;
+        Columns = columns;
+        OccupancyBitsBase64 = Convert.ToBase64String(bitsCopy);
+    }
+
+    public VisualizationBounds Bounds { get; }
+    public double ResolutionMeters { get; }
+    public int Rows { get; }
+    public int Columns { get; }
+    public string OccupancyBitsBase64 { get; }
+}
@@ -0,0 +1,147 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace TrajectoryPlanningVisualization;
+
+public sealed class VisualizationPoint
+{
+    public VisualizationPoint(double x, double y)
+    {
+        VisualizationContract.RequireFinite(x, nameof(x));
+        VisualizationContract.RequireFinite(y, nameof(y));
+        X = x;
+        Y = y;
+    }
+
+    public double X { get; }
+    public double Y { get; }
+}
+
+public sealed class VisualizationPose
+{
+    public VisualizationPose(double x, double y, double headingRadians)
+    {
+        VisualizationContract.RequireFinite(x, nameof(x));
+        VisualizationContract.RequireFinite(y, nameof(y));
+        VisualizationContract.RequireFinite(headingRadians, nameof(headingRadians));
+        X = x;
+        Y = y;
+        HeadingRadians = headingRadians;
+    }
+
+    public double X { get; }
+    public double Y { get; }
+    public double HeadingRadians { get; }
+}
+
+public sealed class VisualizationBounds
+{
+    public VisualizationBounds(double xMin, double xMax, double yMin, double yMax)
+    {
+        VisualizationContract.RequireFinite(xMin, nameof(xMin));
+        VisualizationContract.RequireFinite(xMax, nameof(xMax));
+        VisualizationContract.RequireFinite(yMin, nameof(yMin));
+        VisualizationContract.RequireFinite(yMax, nameof(yMax));
+        if (xMax < xMin || yMax < yMin)
+        {
+            throw new ArgumentOutOfRangeException(nameof(xMax), "Bounds maxima must not precede minima.");
+        }
+
+        XMin = xMin;
+        XMax = xMax;
+        YMin = yMin;
+        YMax = yMax;
+    }
+
+    public double XMin { get; }
+    public double XMax { get; }
+    public double YMin { get; }
+    public double YMax { get; }
+}
+
+public sealed class VisualizationValue
+{
+    public VisualizationValue(string chineseName, string rawName, string value, string unit, string severity)
+    {
+        ChineseName = VisualizationContract.RequireText(chineseName, nameof(chineseName));
+        RawName = VisualizationContract.RequireText(rawName, nameof(rawName));
+        Value = VisualizationContract.RequireText(value, nameof(value));
+        Unit = VisualizationContract.RequireText(unit, nameof(unit));
+        if (severity != "normal" && severity != "notice" && severity != "failure")
+        {
+            throw new ArgumentOutOfRangeException(nameof(severity), "Severity must be normal, notice, or failure.");
+        }
+
+        Severity = severity;
+    }
+
+    public string ChineseName { get; }
+    public string RawName { get; }
+    public string Value { get; }
+    public string Unit { get; }
+    public string Severity { get; }
+}
+
+public sealed class VisualizationConfigurationGroup
+{
+    public VisualizationConfigurationGroup(string chineseTitle, IReadOnlyList<VisualizationValue> entries)
+    {
+        ChineseTitle = VisualizationContract.RequireText(chineseTitle, nameof(chineseTitle));
+        Entries = VisualizationContract.Copy(entries, nameof(entries));
+    }
+
+    public string ChineseTitle { get; }
+    public IReadOnlyList<VisualizationValue> Entries { get; }
+}
+
+internal static class VisualizationContract
+{
+    public static string RequireText(string value, string parameterName)
+    {
+        if (value == null)
+        {
+            throw new ArgumentNullException(parameterName);
+        }
+
+        return value;
+    }
+
+    public static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+        {
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+        }
+    }
+
+    public static void RequireFinite(double? value, string parameterName)
+    {
+        if (value.HasValue)
+        {
+            RequireFinite(value.Value, parameterName);
+        }
+    }
+
+    public static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> values, string parameterName)
+    {
+        if (values == null)
+        {
+            throw new ArgumentNullException(parameterName);
+        }
+
+        var copy = new List<T>(values.Count);
+        for (var index = 0; index < values.Count; index++)
+        {
+            T value = values[index];
+            if (value is null)
+            {
+                throw new ArgumentException("Collections cannot contain null elements.", parameterName);
+            }
+
+            copy.Add(value);
+        }
+
+        return new ReadOnlyCollection<T>(copy);
+    }
+}
@@ -0,0 +1,9 @@
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <TargetFramework>netstandard2.0</TargetFramework>
+    <LangVersion>10</LangVersion>
+  </PropertyGroup>
+  <ItemGroup>
+    <PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
+  </ItemGroup>
+</Project>
@@ -0,0 +1,87 @@
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using TrajectoryPlanningVisualization;
+
+namespace TrajectoryPlanningVisualizationVerificationHost;
+
+internal static class ContractChecks
+{
+    public static void Run()
+    {
+        CopiesCollectionsAndRejectsNonFiniteNumbers();
+        StoresOccupancyAsDefensiveCompactBits();
+        ExposesImmutableSnapshotContracts();
+    }
+
+    private static void CopiesCollectionsAndRejectsNonFiniteNumbers()
+    {
+        var points = new List<VisualizationPoint> { new VisualizationPoint(1d, 2d) };
+        var series = new VisualizationSeries("current", "当前轨迹", VisualizationLineStyle.Solid, points);
+        points[0] = new VisualizationPoint(9d, 9d);
+
+        Verification.NearlyEqual(1d, series.Points[0].X, "series copies points");
+        Verification.Throws<ArgumentOutOfRangeException>(
+            () => new VisualizationPoint(double.NaN, 0d), "point rejects NaN");
+        Verification.Throws<ArgumentOutOfRangeException>(
+            () => new VisualizationPose(0d, 0d, double.PositiveInfinity), "pose rejects infinity");
+        Verification.Throws<ArgumentOutOfRangeException>(
+            () => new VisualizationBounds(0d, 1d, 0d, double.NaN), "bounds rejects NaN");
+        Verification.Throws<ArgumentException>(
+            () => new VisualizationSeries("invalid", "无效", VisualizationLineStyle.Solid,
+                new VisualizationPoint[] { null! }), "series rejects null point");
+    }
+
+    private static void StoresOccupancyAsDefensiveCompactBits()
+    {
+        byte[] bits = { 0x01 };
+        var grid = new VisualizationOccupancyGrid(new VisualizationBounds(0d, 1d, 0d, 1d),
+            0.5d, rows: 2, columns: 2, bits);
+        bits[0] = 0x00;
+
+        Verification.Equal("AQ==", grid.OccupancyBitsBase64, "occupancy grid copies compact bits");
+        Verification.Throws<ArgumentException>(
+            () => new VisualizationOccupancyGrid(new VisualizationBounds(0d, 1d, 0d, 1d),
+                0.5d, rows: 2, columns: 2, new byte[2]), "occupancy grid validates bit length");
+    }
+
+    private static void ExposesImmutableSnapshotContracts()
+    {
+        var staticSnapshot = new PlanningVisualizationStaticSnapshot(
+            "会话",
+            new VisualizationBounds(0d, 1d, 0d, 1d),
+            null,
+            Array.Empty<VisualizationPolyline>(),
+            Array.Empty<VisualizationMarker>(),
+            Array.Empty<VisualizationDirectionSegment>(),
+            Array.Empty<VisualizationConfigurationGroup>());
+        var dynamicSnapshot = new PlanningVisualizationDynamicSnapshot(
+            1L,
+            DateTimeOffset.UtcNow,
+            "运行中",
+            0,
+            "forward",
+            new VisualizationPose(0d, 0d, 0d),
+            Array.Empty<VisualizationPolyline>(),
+            Array.Empty<VisualizationMarker>(),
+            Array.Empty<VisualizationChart>(),
+            Array.Empty<VisualizationValue>(),
+            new VisualizationCycleSummary(1L, DateTimeOffset.UtcNow, "成功", true, 1d, 0,
+                "forward", "rolling", "none", null, null, ""));
+
+        Verification.True(staticSnapshot.StaticPolylines.GetType().Name.Contains("ReadOnlyCollection"),
+            "static snapshot uses read-only collection");
+        Verification.True(dynamicSnapshot.Charts.GetType().Name.Contains("ReadOnlyCollection"),
+            "dynamic snapshot uses read-only collection");
+        AssertNoSetters(typeof(PlanningVisualizationStaticSnapshot));
+        AssertNoSetters(typeof(PlanningVisualizationDynamicSnapshot));
+    }
+
+    private static void AssertNoSetters(Type type)
+    {
+        foreach (PropertyInfo property in type.GetProperties(BindingFlags.Instance | BindingFlags.Public))
+        {
+            Verification.True(property.SetMethod == null, type.Name + " exposes no property setters");
+        }
+    }
+}
@@ -0,0 +1,21 @@
+using System;
+
+namespace TrajectoryPlanningVisualizationVerificationHost;
+
+internal static class Program
+{
+    private static int Main()
+    {
+        try
+        {
+            ContractChecks.Run();
+            Console.WriteLine("PASS trajectory-planning-visualization");
+            return 0;
+        }
+        catch (Exception exception)
+        {
+            Console.Error.WriteLine(exception);
+            return 1;
+        }
+    }
+}
@@ -0,0 +1,10 @@
+<Project Sdk="Microsoft.NET.Sdk">
+  <PropertyGroup>
+    <OutputType>Exe</OutputType>
+    <TargetFramework>net10.0-windows</TargetFramework>
+    <LangVersion>10</LangVersion>
+  </PropertyGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\..\TrajectoryPlanningVisualization\TrajectoryPlanningVisualization.csproj" />
+  </ItemGroup>
+</Project>
@@ -0,0 +1,45 @@
+using System;
+
+namespace TrajectoryPlanningVisualizationVerificationHost;
+
+internal static class Verification
+{
+    public static void Equal<T>(T expected, T actual, string message)
+    {
+        if (!Equals(expected, actual))
+        {
+            throw new InvalidOperationException(message + ": expected " + expected + ", actual " + actual);
+        }
+    }
+
+    public static void NearlyEqual(double expected, double actual, string message)
+    {
+        if (Math.Abs(expected - actual) > 0.0000001d)
+        {
+            throw new InvalidOperationException(message + ": expected " + expected + ", actual " + actual);
+        }
+    }
+
+    public static void True(bool value, string message)
+    {
+        if (!value)
+        {
+            throw new InvalidOperationException(message);
+        }
+    }
+
+    public static void Throws<TException>(Action action, string message)
+        where TException : Exception
+    {
+        try
+        {
+            action();
+        }
+        catch (TException)
+        {
+            return;
+        }
+
+        throw new InvalidOperationException(message);
+    }
+}