BQP/srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Models/Mapping/Serialization.cs

/*
 * Copyright 2016 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 System.Collections.Generic;
using System.Text.Json.Serialization;
using CartographerSharp.Models.Sensor;
using CartographerSharp.Models.Transform;

namespace CartographerSharp.Models.Mapping;

/// <summary>
/// Header of the serialization format. At the moment it only contains the version of the format.
/// </summary>
public struct SerializationHeader
{
    [JsonPropertyName("format_version")]
    public uint FormatVersion { get; set; }

    public SerializationHeader(uint formatVersion)
    {
        FormatVersion = formatVersion;
    }
}

/// <summary>
/// Protocol buffer representation of a Submap with ID.
/// </summary>
public struct Submap
{
    [JsonPropertyName("submap_id")]
    public PoseGraph.SubmapId SubmapId { get; set; }

    [JsonPropertyName("submap_2d")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Submap2D? Submap2D { get; set; }

    [JsonPropertyName("submap_3d")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Submap3D? Submap3D { get; set; }

    public Submap(PoseGraph.SubmapId submapId, Submap2D? submap2D = null, Submap3D? submap3D = null)
    {
        SubmapId = submapId;
        Submap2D = submap2D;
        Submap3D = submap3D;
    }
}

/// <summary>
/// Protocol buffer representation of a Node with ID.
/// </summary>
public struct Node
{
    [JsonPropertyName("node_id")]
    public PoseGraph.NodeId NodeId { get; set; }

    [JsonPropertyName("node_data")]
    public TrajectoryNodeData NodeData { get; set; }

    public Node(PoseGraph.NodeId nodeId, TrajectoryNodeData nodeData)
    {
        NodeId = nodeId;
        NodeData = nodeData;
    }
}

/// <summary>
/// IMU data with trajectory ID for serialization.
/// </summary>
public struct SerializedImuData
{
    [JsonPropertyName("trajectory_id")]
    public int TrajectoryId { get; set; }

    [JsonPropertyName("imu_data")]
    public Sensor.ImuData ImuDataValue { get; set; }

    public SerializedImuData(int trajectoryId, Sensor.ImuData imuData)
    {
        TrajectoryId = trajectoryId;
        ImuDataValue = imuData;
    }
}

/// <summary>
/// Odometry data with trajectory ID for serialization.
/// </summary>
public struct SerializedOdometryData
{
    [JsonPropertyName("trajectory_id")]
    public int TrajectoryId { get; set; }

    [JsonPropertyName("odometry_data")]
    public Sensor.OdometryData OdometryDataValue { get; set; }

    public SerializedOdometryData(int trajectoryId, Sensor.OdometryData odometryData)
    {
        TrajectoryId = trajectoryId;
        OdometryDataValue = odometryData;
    }
}

/// <summary>
/// Fixed frame pose data with trajectory ID for serialization.
/// </summary>
public struct SerializedFixedFramePoseData
{
    [JsonPropertyName("trajectory_id")]
    public int TrajectoryId { get; set; }

    [JsonPropertyName("fixed_frame_pose_data")]
    public Sensor.FixedFramePoseData FixedFramePoseDataValue { get; set; }

    public SerializedFixedFramePoseData(int trajectoryId, Sensor.FixedFramePoseData fixedFramePoseData)
    {
        TrajectoryId = trajectoryId;
        FixedFramePoseDataValue = fixedFramePoseData;
    }
}

/// <summary>
/// Landmark data with trajectory ID for serialization.
/// </summary>
public struct SerializedLandmarkData
{
    [JsonPropertyName("trajectory_id")]
    public int TrajectoryId { get; set; }

    [JsonPropertyName("landmark_data")]
    public Sensor.LandmarkData LandmarkDataValue { get; set; }

    public SerializedLandmarkData(int trajectoryId, Sensor.LandmarkData landmarkData)
    {
        TrajectoryId = trajectoryId;
        LandmarkDataValue = landmarkData;
    }
}

/// <summary>
/// Trajectory data for serialization.
/// </summary>
public struct SerializedTrajectoryData
{
    [JsonPropertyName("trajectory_id")]
    public int TrajectoryId { get; set; }

    [JsonPropertyName("gravity_constant")]
    public double GravityConstant { get; set; }

    [JsonPropertyName("imu_calibration")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Quaterniond? ImuCalibration { get; set; }

    [JsonPropertyName("fixed_frame_origin_in_map")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Rigid3dProto? FixedFrameOriginInMap { get; set; }

    public SerializedTrajectoryData(int trajectoryId, double gravityConstant, Quaterniond? imuCalibration = null, Rigid3dProto? fixedFrameOriginInMap = null)
    {
        TrajectoryId = trajectoryId;
        GravityConstant = gravityConstant;
        ImuCalibration = imuCalibration;
        FixedFrameOriginInMap = fixedFrameOriginInMap;
    }
}

/// <summary>
/// Stop station data for serialization.
/// </summary>
public struct StopStationData
{
    [JsonPropertyName("id")]
    public int Id { get; set; }

    [JsonPropertyName("time")]
    public long Time { get; set; }

    [JsonPropertyName("local_pose")]
    public Rigid3dProto LocalPose { get; set; }

    public StopStationData(int id, long time, Rigid3dProto localPose)
    {
        Id = id;
        Time = time;
        LocalPose = localPose;
    }
}

/// <summary>
/// Accurate submap for serialization (different from PoseGraph.AccurateSubmap).
/// </summary>
public struct SerializedAccurateSubmap
{
    [JsonPropertyName("submap")]
    public Submap Submap { get; set; }

    [JsonPropertyName("stop_stations")]
    public List<StopStationData> StopStations { get; set; }

    public SerializedAccurateSubmap(Submap submap, List<StopStationData>? stopStations = null)
    {
        Submap = submap;
        StopStations = stopStations ?? new List<StopStationData>();
    }
}

/// <summary>
/// Serialized data container (oneof in proto).
/// </summary>
public struct SerializedData
{
    [JsonPropertyName("serialization_header")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializationHeader? SerializationHeader { get; set; }

    [JsonPropertyName("pose_graph")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public PoseGraph? PoseGraph { get; set; }

    [JsonPropertyName("all_trajectory_builder_options")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public AllTrajectoryBuilderOptions? AllTrajectoryBuilderOptions { get; set; }

    [JsonPropertyName("submap")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Submap? Submap { get; set; }

    [JsonPropertyName("node")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public Node? Node { get; set; }

    [JsonPropertyName("trajectory_data")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializedTrajectoryData? SerializedTrajectoryData { get; set; }

    [JsonPropertyName("imu_data")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializedImuData? ImuData { get; set; }

    [JsonPropertyName("odometry_data")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializedOdometryData? OdometryData { get; set; }

    [JsonPropertyName("fixed_frame_pose_data")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializedFixedFramePoseData? FixedFramePoseData { get; set; }

    [JsonPropertyName("landmark_data")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializedLandmarkData? LandmarkData { get; set; }

    [JsonPropertyName("accurate_submap")]
    [JsonIgnore(Condition = JsonIgnoreCondition.WhenWritingNull)]
    public SerializedAccurateSubmap? AccurateSubmap { get; set; }
}