feat: serve planning snapshots on loopback

2026-08-06 09:38:12 +08:00
parent 106201e87b
commit fa1a2666d3
6 changed files with 955 additions and 0 deletions
@@ -0,0 +1,119 @@
+using System;
+using System.Security.Cryptography;
+using System.Threading;
+
+namespace TrajectoryPlanningVisualization;
+
+public sealed class PlanningVisualizationSessionInfo
+{
+    public PlanningVisualizationSessionInfo(Uri uri, string token)
+    {
+        Uri = uri ?? throw new ArgumentNullException(nameof(uri));
+        Token = token ?? throw new ArgumentNullException(nameof(token));
+    }
+
+    public Uri Uri { get; }
+    public string Token { get; }
+}
+
+public sealed class PlanningVisualizationSession : IDisposable
+{
+    private readonly object lifecycleLock = new object();
+    private readonly PlanningVisualizationOptions options;
+    private LoopbackVisualizationServer server;
+    private PlanningVisualizationSessionInfo sessionInfo;
+
+    public PlanningVisualizationSession(PlanningVisualizationOptions options)
+    {
+        this.options = options ?? throw new ArgumentNullException(nameof(options));
+    }
+
+    public bool IsRunning
+    {
+        get
+        {
+            lock (lifecycleLock)
+            {
+                return server != null && server.IsRunning;
+            }
+        }
+    }
+
+    public PlanningVisualizationSessionInfo Start(PlanningVisualizationStaticSnapshot staticSnapshot)
+    {
+        if (staticSnapshot == null)
+        {
+            throw new ArgumentNullException(nameof(staticSnapshot));
+        }
+
+        lock (lifecycleLock)
+        {
+            if (server != null)
+            {
+                return sessionInfo;
+            }
+
+            PlanningVisualizationOptionsSnapshot validated = options.CreateValidatedSnapshot();
+            string token = CreateToken();
+            var created = new LoopbackVisualizationServer(validated, staticSnapshot, token);
+            try
+            {
+                created.Start();
+                server = created;
+                sessionInfo = new PlanningVisualizationSessionInfo(
+                    new Uri("http://127.0.0.1:" + created.Port + "/?token=" + token), token);
+                return sessionInfo;
+            }
+            catch
+            {
+                created.Dispose();
+                throw;
+            }
+        }
+    }
+
+    public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
+    {
+        if (snapshot == null)
+        {
+            throw new ArgumentNullException(nameof(snapshot));
+        }
+
+        try
+        {
+            Volatile.Read(ref server)?.Publish(snapshot);
+        }
+        catch
+        {
+        }
+    }
+
+    public void Stop()
+    {
+        LoopbackVisualizationServer current;
+        lock (lifecycleLock)
+        {
+            current = server;
+            server = null;
+            sessionInfo = null;
+        }
+
+        current?.Dispose();
+    }
+
+    public void Dispose()
+    {
+        Stop();
+    }
+
+    private static string CreateToken()
+    {
+        byte[] tokenBytes = new byte[32];
+        using (RandomNumberGenerator random = RandomNumberGenerator.Create())
+        {
+            random.GetBytes(tokenBytes);
+        }
+
+        return BitConverter.ToString(tokenBytes).Replace("-", string.Empty).ToLowerInvariant();
+    }
+}
@@ -0,0 +1,125 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.IO;
+using System.Text;
+
+namespace TrajectoryPlanningVisualization;
+
+internal sealed class LoopbackHttpRequest
+{
+    public LoopbackHttpRequest(string method, string path, string token)
+    {
+        Method = method;
+        Path = path;
+        Token = token;
+    }
+
+    public string Method { get; }
+    public string Path { get; }
+    public string Token { get; }
+}
+
+internal static class LoopbackHttpRequestReader
+{
+    internal const int MaximumHeaderBytes = 16 * 1024;
+    private const int RequestReadTimeoutMilliseconds = 2000;
+
+    public static bool TryRead(Stream stream, out LoopbackHttpRequest request)
+    {
+        request = null;
+        var bytes = new List<byte>();
+        bool tooLarge = false;
+        int terminatorBytes = 0;
+        var stopwatch = Stopwatch.StartNew();
+        while (stopwatch.ElapsedMilliseconds < RequestReadTimeoutMilliseconds)
+        {
+            int value;
+            try
+            {
+                int remainingMilliseconds = Math.Max(1, RequestReadTimeoutMilliseconds - (int)stopwatch.ElapsedMilliseconds);
+                stream.ReadTimeout = remainingMilliseconds;
+                value = stream.ReadByte();
+            }
+            catch (IOException)
+            {
+                return false;
+            }
+
+            if (value < 0)
+            {
+                return false;
+            }
+
+            byte next = (byte)value;
+            if (!tooLarge)
+            {
+                bytes.Add(next);
+                if (bytes.Count >= MaximumHeaderBytes)
+                {
+                    tooLarge = true;
+                }
+            }
+
+            terminatorBytes = next == (terminatorBytes == 0 || terminatorBytes == 2 ? (byte)'\r' : (byte)'\n')
+                ? terminatorBytes + 1
+                : next == '\r' ? 1 : 0;
+            if (terminatorBytes == 4)
+            {
+                return !tooLarge && TryParse(Encoding.ASCII.GetString(bytes.ToArray()), out request);
+            }
+        }
+
+        return false;
+    }
+
+    private static bool TryParse(string headerText, out LoopbackHttpRequest request)
+    {
+        request = null;
+        string[] lines = headerText.Split(new[] { "\r\n" }, StringSplitOptions.None);
+        if (lines.Length < 2 || string.IsNullOrEmpty(lines[0]))
+        {
+            return false;
+        }
+
+        string[] parts = lines[0].Split(' ');
+        if (parts.Length != 3 || parts[0].Length == 0 || parts[1].Length == 0 || parts[2] != "HTTP/1.1")
+        {
+            return false;
+        }
+
+        string target = parts[1];
+        if (!target.StartsWith("/", StringComparison.Ordinal) || target.IndexOf('#') >= 0)
+        {
+            return false;
+        }
+
+        int queryStart = target.IndexOf('?');
+        string path = queryStart < 0 ? target : target.Substring(0, queryStart);
+        string token = null;
+        if (queryStart >= 0)
+        {
+            string query = target.Substring(queryStart + 1);
+            string[] pairs = query.Split('&');
+            foreach (string pair in pairs)
+            {
+                int equals = pair.IndexOf('=');
+                string name = equals < 0 ? pair : pair.Substring(0, equals);
+                if (name == "token")
+                {
+                    try
+                    {
+                        token = Uri.UnescapeDataString(equals < 0 ? string.Empty : pair.Substring(equals + 1));
+                    }
+                    catch (UriFormatException)
+                    {
+                        return false;
+                    }
+                }
+            }
+        }
+
+        request = new LoopbackHttpRequest(parts[0], path, token);
+        return true;
+    }
+}
@@ -0,0 +1,333 @@
+using System;
+using System.Collections.Generic;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace TrajectoryPlanningVisualization;
+
+internal sealed class LoopbackVisualizationServer : IDisposable
+{
+    private readonly PlanningVisualizationOptionsSnapshot options;
+    private readonly PlanningVisualizationStaticSnapshot staticSnapshot;
+    private readonly string token;
+    private readonly LatestVisualizationFrameStore frames = new LatestVisualizationFrameStore();
+    private readonly BoundedCycleHistory history;
+    private readonly CancellationTokenSource cancellation = new CancellationTokenSource();
+    private readonly object clientsLock = new object();
+    private readonly List<SseClientConnection> clients = new List<SseClientConnection>();
+    private TcpListener listener;
+    private Task acceptTask;
+    private Task dispatcherTask;
+    private int stopped;
+    private long lastProcessedVersion;
+    private int clientGeneration;
+
+    public LoopbackVisualizationServer(PlanningVisualizationOptionsSnapshot options,
+        PlanningVisualizationStaticSnapshot staticSnapshot, string token)
+    {
+        this.options = options ?? throw new ArgumentNullException(nameof(options));
+        this.staticSnapshot = staticSnapshot ?? throw new ArgumentNullException(nameof(staticSnapshot));
+        this.token = token ?? throw new ArgumentNullException(nameof(token));
+        history = new BoundedCycleHistory(options.HistoryCycleLimit);
+    }
+
+    public int Port { get; private set; }
+    public string FaultReason { get; private set; }
+    public bool IsRunning => Volatile.Read(ref stopped) == 0 && listener != null;
+
+    public void Start()
+    {
+        listener = new TcpListener(IPAddress.Loopback, options.Port);
+        listener.Start();
+        Port = ((IPEndPoint)listener.LocalEndpoint).Port;
+        acceptTask = Task.Run((Func<Task>)AcceptLoop);
+        dispatcherTask = Task.Run((Func<Task>)DispatchLoop);
+    }
+
+    public void Publish(PlanningVisualizationDynamicSnapshot snapshot)
+    {
+        if (snapshot == null)
+        {
+            throw new ArgumentNullException(nameof(snapshot));
+        }
+
+        frames.Publish(snapshot);
+    }
+
+    public void Stop()
+    {
+        if (Interlocked.Exchange(ref stopped, 1) != 0)
+        {
+            return;
+        }
+
+        SseClientConnection[] currentClients = SnapshotClients();
+        const string terminal = "event: end\ndata: {\"reasonChinese\":\"会话已结束\"}\n\n";
+        foreach (SseClientConnection client in currentClients)
+        {
+            client.OfferTerminal(terminal);
+        }
+
+        DateTime deadline = DateTime.UtcNow.AddMilliseconds(100);
+        foreach (SseClientConnection client in currentClients)
+        {
+            TimeSpan remaining = deadline - DateTime.UtcNow;
+            if (remaining <= TimeSpan.Zero)
+            {
+                break;
+            }
+
+            client.WaitForCompletion(remaining);
+        }
+
+        cancellation.Cancel();
+        try
+        {
+            listener?.Stop();
+        }
+        catch (SocketException)
+        {
+        }
+
+        foreach (SseClientConnection client in currentClients)
+        {
+            client.Dispose();
+        }
+    }
+
+    public void Dispose()
+    {
+        Stop();
+        cancellation.Dispose();
+    }
+
+    private async Task AcceptLoop()
+    {
+        try
+        {
+            while (!cancellation.IsCancellationRequested)
+            {
+                TcpClient client;
+                try
+                {
+                    client = await listener.AcceptTcpClientAsync().ConfigureAwait(false);
+                }
+                catch (ObjectDisposedException)
+                {
+                    return;
+                }
+                catch (SocketException)
+                {
+                    if (cancellation.IsCancellationRequested)
+                    {
+                        return;
+                    }
+
+                    throw;
+                }
+
+                _ = Task.Run(() => HandleClient(client));
+            }
+        }
+        catch (Exception exception)
+        {
+            Fault(exception);
+        }
+    }
+
+    private async Task HandleClient(TcpClient client)
+    {
+        SseClientConnection sseClient = null;
+        try
+        {
+            NetworkStream stream = client.GetStream();
+            if (!LoopbackHttpRequestReader.TryRead(stream, out LoopbackHttpRequest request))
+            {
+                WriteResponse(stream, 400, "Bad Request", "text/plain; charset=utf-8", "bad request");
+                return;
+            }
+
+            if (request.Method != "GET")
+            {
+                WriteResponse(stream, 405, "Method Not Allowed", "text/plain; charset=utf-8", "method not allowed");
+                return;
+            }
+
+            if (!ConstantTimeEquals(token, request.Token))
+            {
+                WriteResponse(stream, 403, "Forbidden", "text/plain; charset=utf-8", "forbidden");
+                return;
+            }
+
+            if (request.Path == "/api/bootstrap")
+            {
+                WriteResponse(stream, 200, "OK", "application/json; charset=utf-8", VisualizationJson.Serialize(staticSnapshot));
+                return;
+            }
+
+            if (request.Path != "/api/events")
+            {
+                WriteResponse(stream, 404, "Not Found", "text/plain; charset=utf-8", "not found");
+                return;
+            }
+
+            if (!TryAddClient(client, out sseClient))
+            {
+                WriteResponse(stream, 503, "Service Unavailable", "text/plain; charset=utf-8", "too many clients");
+                return;
+            }
+
+            WriteSseHeaders(stream);
+            await sseClient.Completion.ConfigureAwait(false);
+        }
+        catch (SocketException)
+        {
+        }
+        catch (System.IO.IOException)
+        {
+        }
+        catch (ObjectDisposedException)
+        {
+        }
+        finally
+        {
+            if (sseClient != null)
+            {
+                RemoveClient(sseClient);
+                sseClient.Dispose();
+            }
+            else
+            {
+                client.Dispose();
+            }
+        }
+    }
+
+    private async Task DispatchLoop()
+    {
+        try
+        {
+            int deliveredGeneration = -1;
+            while (!cancellation.IsCancellationRequested)
+            {
+                await Task.Delay(TimeSpan.FromSeconds(1d / options.RefreshRateHz), cancellation.Token).ConfigureAwait(false);
+                if (frames.TryReadAfter(lastProcessedVersion, out VisualizationFrame latest))
+                {
+                    lastProcessedVersion = latest.Version;
+                    history.Add(latest.Snapshot.CycleSummary);
+                }
+
+                SseClientConnection[] currentClients = SnapshotClients();
+                int currentGeneration = Volatile.Read(ref clientGeneration);
+                if (currentClients.Length == 0 || (latest == null && deliveredGeneration == currentGeneration))
+                {
+                    continue;
+                }
+
+                if (latest == null && !frames.TryReadAfter(-1, out latest))
+                {
+                    continue;
+                }
+
+                string payload = "event: frame\ndata: " + VisualizationJson.Serialize(new
+                {
+                    snapshot = latest.Snapshot,
+                    history = history.Snapshot()
+                }) + "\n\n";
+                foreach (SseClientConnection client in currentClients)
+                {
+                    client.Offer(payload);
+                }
+
+                deliveredGeneration = currentGeneration;
+            }
+        }
+        catch (OperationCanceledException)
+        {
+        }
+        catch (Exception exception)
+        {
+            Fault(exception);
+        }
+    }
+
+    private bool TryAddClient(TcpClient tcpClient, out SseClientConnection client)
+    {
+        lock (clientsLock)
+        {
+            if (clients.Count >= options.MaximumClients || cancellation.IsCancellationRequested)
+            {
+                client = null;
+                return false;
+            }
+
+            client = new SseClientConnection(tcpClient, cancellation.Token);
+            clients.Add(client);
+            Interlocked.Increment(ref clientGeneration);
+            return true;
+        }
+    }
+
+    private void RemoveClient(SseClientConnection client)
+    {
+        lock (clientsLock)
+        {
+            clients.Remove(client);
+            Interlocked.Increment(ref clientGeneration);
+        }
+    }
+
+    private SseClientConnection[] SnapshotClients()
+    {
+        lock (clientsLock)
+        {
+            return clients.ToArray();
+        }
+    }
+
+    private void Fault(Exception exception)
+    {
+        FaultReason = exception.Message;
+        Stop();
+    }
+
+    private static bool ConstantTimeEquals(string expected, string actual)
+    {
+        if (actual == null || expected.Length != actual.Length)
+        {
+            return false;
+        }
+
+        int difference = 0;
+        for (int index = 0; index < expected.Length; index++)
+        {
+            difference |= expected[index] ^ actual[index];
+        }
+
+        return difference == 0;
+    }
+
+    private static void WriteResponse(NetworkStream stream, int status, string reason, string contentType, string body)
+    {
+        byte[] bodyBytes = Encoding.UTF8.GetBytes(body);
+        string headers = "HTTP/1.1 " + status + " " + reason + "\r\nContent-Type: " + contentType +
+            "\r\nContent-Length: " + bodyBytes.Length + "\r\nConnection: close\r\n\r\n";
+        byte[] headerBytes = Encoding.ASCII.GetBytes(headers);
+        stream.WriteTimeout = 1000;
+        stream.Write(headerBytes, 0, headerBytes.Length);
+        stream.Write(bodyBytes, 0, bodyBytes.Length);
+        stream.Flush();
+    }
+
+    private static void WriteSseHeaders(NetworkStream stream)
+    {
+        const string headers = "HTTP/1.1 200 OK\r\nContent-Type: text/event-stream; charset=utf-8\r\nCache-Control: no-cache\r\nConnection: keep-alive\r\n\r\n";
+        byte[] bytes = Encoding.ASCII.GetBytes(headers);
+        stream.WriteTimeout = 1000;
+        stream.Write(bytes, 0, bytes.Length);
+        stream.Flush();
+    }
+}
@@ -0,0 +1,115 @@
+using System;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using System.Threading.Tasks;
+
+namespace TrajectoryPlanningVisualization;
+
+internal sealed class SseClientConnection : IDisposable
+{
+    private readonly TcpClient client;
+    private readonly CancellationToken cancellationToken;
+    private readonly AutoResetEvent pendingSignal = new AutoResetEvent(false);
+    private readonly Task writerTask;
+    private string pendingPayload;
+    private int completeAfterWrite;
+    private int disposed;
+
+    public SseClientConnection(TcpClient client, CancellationToken cancellationToken)
+    {
+        this.client = client ?? throw new ArgumentNullException(nameof(client));
+        this.cancellationToken = cancellationToken;
+        writerTask = Task.Run((Action)WriteLoop);
+    }
+
+    public Task Completion => writerTask;
+
+    public void Offer(string payload)
+    {
+        if (payload == null || Volatile.Read(ref disposed) != 0)
+        {
+            return;
+        }
+
+        Interlocked.Exchange(ref pendingPayload, payload);
+        pendingSignal.Set();
+    }
+
+    public void OfferTerminal(string payload)
+    {
+        if (payload == null || Volatile.Read(ref disposed) != 0)
+        {
+            return;
+        }
+
+        Interlocked.Exchange(ref pendingPayload, payload);
+        Interlocked.Exchange(ref completeAfterWrite, 1);
+        pendingSignal.Set();
+    }
+
+    public bool WaitForCompletion(TimeSpan timeout)
+    {
+        try
+        {
+            return writerTask.Wait(timeout);
+        }
+        catch (AggregateException)
+        {
+            return true;
+        }
+    }
+
+    public void Dispose()
+    {
+        if (Interlocked.Exchange(ref disposed, 1) != 0)
+        {
+            return;
+        }
+
+        pendingSignal.Set();
+        try
+        {
+            client.Close();
+        }
+        finally
+        {
+            pendingSignal.Dispose();
+        }
+    }
+
+    private void WriteLoop()
+    {
+        try
+        {
+            NetworkStream stream = client.GetStream();
+            stream.WriteTimeout = 1000;
+            while (!cancellationToken.IsCancellationRequested && Volatile.Read(ref disposed) == 0)
+            {
+                string payload = Interlocked.Exchange(ref pendingPayload, null);
+                if (payload == null)
+                {
+                    pendingSignal.WaitOne(50);
+                    continue;
+                }
+
+                byte[] bytes = Encoding.UTF8.GetBytes(payload);
+                stream.Write(bytes, 0, bytes.Length);
+                stream.Flush();
+                if (Interlocked.Exchange(ref completeAfterWrite, 0) != 0)
+                {
+                    return;
+                }
+            }
+        }
+        catch (SocketException)
+        {
+        }
+        catch (System.IO.IOException)
+        {
+        }
+        catch (ObjectDisposedException)
+        {
+        }
+    }
+}