Do an example of a full serializable struct

robotdotnet · Jan 15, 2024 · 660d826 · 660d826
1 parent a65d12b
commit 660d826
Show file tree

Hide file tree

Showing 14 changed files with 391 additions and 120 deletions.
diff --git a/WPILib.sln b/WPILib.sln
@@ -19,6 +19,10 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "cscore", "src\cscore\cscore
 EndProject
 Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "hal", "src\hal\hal.csproj", "{0F596472-F93A-4519-B8A5-FCCC321B3C44}"
 EndProject
+Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "dev", "dev", "{1CA9AB3B-4828-4F07-8C0E-88EF7C5A9ACD}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "desktopDev", "dev\desktopDev\desktopDev.csproj", "{7EF82002-5BBC-45DF-9A70-743A87EA5575}"
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|Any CPU = Debug|Any CPU
@@ -52,6 +56,10 @@ Global
 		{0F596472-F93A-4519-B8A5-FCCC321B3C44}.Debug|Any CPU.Build.0 = Debug|Any CPU
 		{0F596472-F93A-4519-B8A5-FCCC321B3C44}.Release|Any CPU.ActiveCfg = Release|Any CPU
 		{0F596472-F93A-4519-B8A5-FCCC321B3C44}.Release|Any CPU.Build.0 = Release|Any CPU
+		{7EF82002-5BBC-45DF-9A70-743A87EA5575}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+		{7EF82002-5BBC-45DF-9A70-743A87EA5575}.Debug|Any CPU.Build.0 = Debug|Any CPU
+		{7EF82002-5BBC-45DF-9A70-743A87EA5575}.Release|Any CPU.ActiveCfg = Release|Any CPU
+		{7EF82002-5BBC-45DF-9A70-743A87EA5575}.Release|Any CPU.Build.0 = Release|Any CPU
 	EndGlobalSection
 	GlobalSection(NestedProjects) = preSolution
 		{8F38C25E-641E-47FC-AC0A-0717F2159E8F} = {DB664556-4BF0-4874-8CB6-DC24E60A67AF}
@@ -60,5 +68,6 @@ Global
 		{E616A4EB-E1C1-4493-B23D-96CC32C83B28} = {AD95ECD8-E708-4FB4-9B7E-A8A8EF3FCB3E}
 		{33E753D6-DFC1-41F8-B488-59834326E4DB} = {DB664556-4BF0-4874-8CB6-DC24E60A67AF}
 		{0F596472-F93A-4519-B8A5-FCCC321B3C44} = {DB664556-4BF0-4874-8CB6-DC24E60A67AF}
+		{7EF82002-5BBC-45DF-9A70-743A87EA5575} = {1CA9AB3B-4828-4F07-8C0E-88EF7C5A9ACD}
 	EndGlobalSection
 EndGlobal
diff --git a/dev/desktopDev/Program.cs b/dev/desktopDev/Program.cs
@@ -1,83 +1,19 @@
-using Hal;
-using NetworkTables;
-using NetworkTables.Natives;
-using System;
-using System.Linq;
-using WPILib;
+using System;
+using System.Text.Json;
+using UnitsNet.NumberExtensions.NumberToAngle;
+using WPIMath.Geometry;
 
-namespace desktopDev
-{
-    public class Robot : TimedRobot
-    {
-        private TimeSpan lastTime = Timer.FPGATimestamp;
-        readonly double[] buffer = new double[50];
-        int idx = 0;
-
-        private readonly int pwm = Hal.PWMLowLevel.InitializePort(HALLowLevel.GetPort(42));
-
-        private readonly int can = CANAPILowLevel.Initialize(CANManufacturer.kTeamUse, 1, CANDeviceType.kMiscellaneous);
-
-
-
-        public override unsafe void RobotPeriodic()
-        {
-            int idxLocal = idx;
-            CANAPILowLevel.WritePacket(can, new Span<byte>(&idxLocal, 4), 42);
-
-            var current = Timer.FPGATimestamp;
-            var delta = current - lastTime;
-            lastTime = current;
-            buffer[idx] = delta.TotalMilliseconds;
-            idx++;
-            if (idx == 50)
-            {
-                Console.WriteLine(buffer.Average());
-                idx = 0;
-            }
-            base.RobotPeriodic();
-        }
-    }
+namespace DesktopDev;
 
-    class Program
+class Program
+{
+    static void Main(string[] args)
     {
+        Rotation2d rot = new(5.Radians());
+        string serialized = JsonSerializer.Serialize(rot);
+        Console.WriteLine(serialized);
 
-        public class HolderMethod
-        {
-            public void doThing()
-            {
-
-            }
-        }
-
-        public class Container
-        {
-            public Action holder;
-
-            public Container(HolderMethod holder)
-            {
-                this.holder = holder.doThing;
-            }
-        }
-
-        static void Main(string[] args)
-        {
-            NtCore.Initialize();
-            NetworkTableInstance instance = NetworkTableInstance.Default;
-            GC.KeepAlive(instance);
-            //var i = SendableRegistry.Instance;
-
-            //var map = new ConditionalWeakTable<HolderMethod, Container>();
-
-            //var holder = new HolderMethod();
-            //map.Add(holder, new Container(holder));
-            ////holder = null;
-
-            ////while (map.Any())
-            ////{
-            ////    //Console.WriteLine(map.Count());
-            ////    GC.Collect();
-            ////}
-            //RobotBase.StartRobot<Robot>();
-        }
+        Rotation2d r = JsonSerializer.Deserialize<Rotation2d>(serialized);
+        Console.WriteLine(r.Angle.Radians);
     }
 }
diff --git a/dev/desktopDev/desktopDev.csproj b/dev/desktopDev/desktopDev.csproj
@@ -6,15 +6,11 @@
     <AllowUnsafeBlocks>true</AllowUnsafeBlocks>
   </PropertyGroup>
 
-  <ItemGroup>
-    <PackageReference Include="UnitsNet.NumberExtensions" Version="4.83.0" />
-  </ItemGroup>
 
   <ItemGroup>
     <ProjectReference Include="..\..\src\hal\hal.csproj" />
     <ProjectReference Include="..\..\src\ntcore\ntcore.csproj" />
-    <ProjectReference Include="..\..\src\thirdparty\SparkMax\SparkMax.csproj" />
-    <ProjectReference Include="..\..\src\wpilibsharp\wpilibsharp.csproj" />
+    <ProjectReference Include="..\..\src\wpinet\wpinet.csproj" />
     <ProjectReference Include="..\..\src\wpiutil\wpiutil.csproj" />
   </ItemGroup>
 

diff --git a/src/ntcore/StructArrayTopic.cs b/src/ntcore/StructArrayTopic.cs
@@ -5,24 +5,24 @@ namespace NetworkTables;
 
 public sealed class StructArrayTopic<T> : Topic
 {
-    public Struct<T> Struct { get; }
+    public IStruct<T> Struct { get; }
 
-    private StructArrayTopic(Topic topic, Struct<T> value) : base(topic.Instance, topic.Handle)
+    private StructArrayTopic(Topic topic, IStruct<T> value) : base(topic.Instance, topic.Handle)
     {
         Struct = value;
     }
 
-    private StructArrayTopic(NetworkTableInstance inst, NtTopic handle, Struct<T> value) : base(inst, handle)
+    private StructArrayTopic(NetworkTableInstance inst, NtTopic handle, IStruct<T> value) : base(inst, handle)
     {
         Struct = value;
     }
 
-    public static StructArrayTopic<T> Wrap(Topic topic, Struct<T> value)
+    public static StructArrayTopic<T> Wrap(Topic topic, IStruct<T> value)
     {
         return new StructArrayTopic<T>(topic, value);
     }
 
-    public static StructArrayTopic<T> Wrap(NetworkTableInstance inst, NtTopic handle, Struct<T> value)
+    public static StructArrayTopic<T> Wrap(NetworkTableInstance inst, NtTopic handle, IStruct<T> value)
     {
         return new StructArrayTopic<T>(inst, handle, value);
     }

diff --git a/src/wpiutil/DataLog.cs b/src/wpiutil/DataLog.cs
@@ -1,6 +1,8 @@
 global using EntryHandle = int;
 
 using System;
+using System.Collections.Concurrent;
+using System.Collections.Generic;
 using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using WPIUtil.Handles;
@@ -67,10 +69,46 @@ public void AppendRaw(DataLogEntryHandle entry, ReadOnlySpan<byte> data, long ti
         DataLogNative.DataLogAppend(NativeHandle, entry, data, (ulong)timestamp);
     }
 
-    public void AddSchema<T>(Struct<T> value, long timestamp = 0)
+    public void AddSchema<T>(IStruct<T> value, long timestamp = 0)
     {
         throw new NotImplementedException();
     }
 
+    public bool HasSchema(string name)
+    {
+        return m_schemaMap.ContainsKey(name);
+    }
+
+    public void AddSchema(string name, string type, string schema, long timestamp = 0)
+    {
+        if (!m_schemaMap.TryAdd(name, 1))
+        {
+            return;
+        }
+        // TODO add schema functions
+        // DataLogNative
+    }
+
+    private void AddSchemaImpl(IStructBase value, long timestamp, HashSet<string> seen)
+    {
+        string typeString = value.TypeString;
+        if (HasSchema(typeString))
+        {
+            return;
+        }
+        if (!seen.Add(typeString))
+        {
+            throw new InvalidOperationException($"{typeString}: circular reference with {seen}");
+        }
+        AddSchema(typeString, "structschema", value.Schema, timestamp);
+        foreach (var inner in value.Nested)
+        {
+            AddSchemaImpl(inner, timestamp, seen);
+        }
+        seen.Remove(typeString);
+    }
+
+    private readonly ConcurrentDictionary<string, int> m_schemaMap = [];
+
     public unsafe OpaqueDataLog* NativeHandle { get; } = null;
 }
diff --git a/src/wpiutil/Logging/StructArrayLogEntry.cs b/src/wpiutil/Logging/StructArrayLogEntry.cs
@@ -4,22 +4,27 @@
 
 namespace WPIUtil.Logging;
 
-public sealed class StructArrayLogEntry<T> : DataLogEntry
+public sealed class StructArrayLogEntry<T> : DataLogEntry where T : IStructSerializable<T>
 {
     private StructBuffer<T> m_storage;
     private readonly object m_lockObject = new();
 
-    private StructArrayLogEntry(DataLog log, string name, Struct<T> value, string metadata, long timestamp) : base(log, name, $"{value.TypeString}[]", metadata, timestamp)
+    private StructArrayLogEntry(DataLog log, string name, IStruct<T> value, string metadata, long timestamp) : base(log, name, $"{value.TypeString}[]", metadata, timestamp)
     {
         m_storage = StructBuffer<T>.Create(value);
         log.AddSchema(value, timestamp);
     }
 
-    public StructArrayLogEntry<T> Create(DataLog log, string name, Struct<T> value, string metadata = "", long timestamp = 0)
+    public StructArrayLogEntry<T> Create(DataLog log, string name, IStruct<T> value, string metadata = "", long timestamp = 0)
     {
         return new StructArrayLogEntry<T>(log, name, value, metadata, timestamp);
     }
 
+    public StructArrayLogEntry<T> Create(DataLog log, string name, string metadata = "", long timestamp = 0)
+    {
+        return new StructArrayLogEntry<T>(log, name, T.Struct, metadata, timestamp);
+    }
+
     public void Reserve(int nelem)
     {
         lock (m_lockObject)

diff --git a/src/wpiutil/Logging/StructLogEntry.cs b/src/wpiutil/Logging/StructLogEntry.cs
@@ -3,31 +3,32 @@
 
 namespace WPIUtil.Logging;
 
-public sealed class StructLogEntry<T> : DataLogEntry
+public sealed class StructLogEntry<T> : DataLogEntry where T : IStructSerializable<T>
 {
-    private readonly Struct<T> m_struct;
-    private readonly byte[] m_storage;
+    private StructBuffer<T> m_storage;
+    private readonly object m_lockObject = new();
 
-    private StructLogEntry(DataLog log, string name, Struct<T> value, string metadata, long timestamp) : base(log, name, value.TypeString, metadata, timestamp)
+    private StructLogEntry(DataLog log, string name, IStruct<T> value, string metadata, long timestamp) : base(log, name, value.TypeString, metadata, timestamp)
     {
-        m_struct = value;
-        m_storage = new byte[value.Size];
+        m_storage = StructBuffer<T>.Create(value);
         log.AddSchema(value, timestamp);
     }
 
-    public StructLogEntry<T> Create(DataLog log, string name, Struct<T> value, string metadata = "", long timestamp = 0)
+    public StructLogEntry<T> Create(DataLog log, string name, IStruct<T> value, string metadata = "", long timestamp = 0)
     {
         return new StructLogEntry<T>(log, name, value, metadata, timestamp);
     }
 
+        public StructLogEntry<T> Create(DataLog log, string name, string metadata = "", long timestamp = 0)
+    {
+        return new StructLogEntry<T>(log, name, T.Struct, metadata, timestamp);
+    }
+
     public void Append(T value, long timestamp = 0)
     {
-        ReadOnlySpan<byte> toWrite;
-        lock (m_storage)
+        lock (m_lockObject)
         {
-            toWrite = m_struct.Pack(m_storage, value);
-
+            m_log.AppendRaw(m_entry, m_storage.Write(value), timestamp);
         }
-        m_log.AppendRaw(m_entry, toWrite, timestamp);
     }
 }