Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/IO/ProtoStreamDeserializer.cs

@@ -0,0 +1,173 @@
+/*
+ * Copyright 2018 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using CartographerSharp.Models.Mapping;
+
+namespace CartographerSharp.IO;
+
+/// <summary>
+/// Helper for deserializing a previously serialized mapping state from a proto stream,
+/// abstracting away the format parsing logic.
+/// </summary>
+public class ProtoStreamDeserializer
+{
+    private const uint FormatVersion = 2;
+    private const uint FormatVersionWithoutSubmapHistograms = 1;
+
+    private readonly IProtoStreamReader _reader;
+    private readonly SerializationHeader _header;
+    private readonly SerializedData _poseGraph;
+    private readonly SerializedData _allTrajectoryBuilderOptions;
+
+    public ProtoStreamDeserializer(IProtoStreamReader reader)
+    {
+        ArgumentNullException.ThrowIfNull(reader);
+
+        _reader = reader;
+
+        // Read header - use same pattern as ReadNextSerializedData
+        // ReadProto returns T? which for structs becomes Nullable<T>
+        // Using out var should work, but compiler may not infer nullable correctly
+        // So we use a helper method pattern
+        if (!ReadNextSerializedData(out var headerDataNullable) || !headerDataNullable.HasValue)
+        {
+            throw new InvalidOperationException("Failed to read SerializationHeader.");
+        }
+        
+        var headerData = headerDataNullable.Value;
+        if (!headerData.SerializationHeader.HasValue)
+        {
+            throw new InvalidOperationException("SerializedData does not contain SerializationHeader.");
+        }
+
+        _header = headerData.SerializationHeader.Value;
+
+        // Validate format version
+        if (!IsVersionSupported(_header))
+        {
+            throw new NotSupportedException(
+                $"Unsupported serialization format version: {_header.FormatVersion}. " +
+                $"Supported versions: {FormatVersionWithoutSubmapHistograms}, {FormatVersion}");
+        }
+
+        // Read pose graph
+        if (!ReadNextSerializedData(out SerializedData? poseGraphDataNullable) || !poseGraphDataNullable.HasValue)
+        {
+            throw new InvalidOperationException(
+                "Serialized stream misses PoseGraph. Expecting `PoseGraph` after `SerializationHeader`.");
+        }
+
+        var poseGraphData = poseGraphDataNullable.Value;
+        if (!poseGraphData.PoseGraph.HasValue)
+        {
+            throw new InvalidOperationException(
+                "SerializedData does not contain PoseGraph. Expecting `PoseGraph` after `SerializationHeader`.");
+        }
+
+        _poseGraph = poseGraphData;
+
+        // Read trajectory builder options
+        if (!ReadNextSerializedData(out var optionsDataNullable) || !optionsDataNullable.HasValue)
+        {
+            throw new InvalidOperationException(
+                "Serialized stream misses `AllTrajectoryBuilderOptions`. " +
+                "Expecting `AllTrajectoryBuilderOptions` after PoseGraph.");
+        }
+
+        var optionsData = optionsDataNullable.Value;
+        if (!optionsData.AllTrajectoryBuilderOptions.HasValue)
+        {
+            throw new InvalidOperationException(
+                "SerializedData does not contain AllTrajectoryBuilderOptions. " +
+                "Expecting `AllTrajectoryBuilderOptions` after PoseGraph.");
+        }
+
+        _allTrajectoryBuilderOptions = optionsData;
+
+        // Validate that trajectory count matches
+        if (_poseGraph.PoseGraph.HasValue && _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.HasValue)
+        {
+            var poseGraphProto = _poseGraph.PoseGraph.Value;
+            var optionsProto = _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.Value;
+
+            if (poseGraphProto.Trajectories.Count != optionsProto.OptionsWithSensorIds.Count)
+            {
+                throw new InvalidOperationException(
+                    $"Trajectory count mismatch: PoseGraph has {poseGraphProto.Trajectories.Count} trajectories, " +
+                    $"but AllTrajectoryBuilderOptions has {optionsProto.OptionsWithSensorIds.Count} options.");
+            }
+        }
+    }
+
+    /// <summary>
+    /// Gets the serialization header.
+    /// </summary>
+    public SerializationHeader Header => _header;
+
+    /// <summary>
+    /// Gets the pose graph proto.
+    /// </summary>
+    public PoseGraph PoseGraph
+    {
+        get
+        {
+            if (!_poseGraph.PoseGraph.HasValue)
+                throw new InvalidOperationException("PoseGraph is not available.");
+            return _poseGraph.PoseGraph.Value;
+        }
+    }
+
+    /// <summary>
+    /// Gets all trajectory builder options.
+    /// </summary>
+    public AllTrajectoryBuilderOptions AllTrajectoryBuilderOptions
+    {
+        get
+        {
+            if (!_allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.HasValue)
+                throw new InvalidOperationException("AllTrajectoryBuilderOptions is not available.");
+            return _allTrajectoryBuilderOptions.AllTrajectoryBuilderOptions.Value;
+        }
+    }
+
+    /// <summary>
+    /// Reads the next SerializedData message from the ProtoStream.
+    /// Returns true if the message was successfully read, or false if there are no more messages or an error occurred.
+    /// </summary>
+    public bool ReadNextSerializedData(out SerializedData? data)
+    {
+        if (_reader.Eof)
+        {
+            data = null;
+            return false;
+        }
+
+        if (!_reader.ReadProto<SerializedData>(out var proto))
+        {
+            data = null;
+            return false;
+        }
+
+        data = proto;
+        return data.HasValue;
+    }
+
+    private static bool IsVersionSupported(SerializationHeader header)
+    {
+        return header.FormatVersion == FormatVersion ||
+               header.FormatVersion == FormatVersionWithoutSubmapHistograms;
+    }
+}