From df595c84a43d6bf5e83d6022c0bcb96d70fa65c2 Mon Sep 17 00:00:00 2001 From: Thad House Date: Thu, 8 Feb 2024 23:23:42 -0800 Subject: [PATCH] A bunch more NT functionality --- dev/desktopDev/Program.cs | 12 ++- src/cscore/VideoEvent.cs | 39 +++++-- src/hal/Natives/HalBase.cs | 5 + src/ntcore/MultiSubscriber.cs | 21 ++-- src/ntcore/Natives/NtCore.cs | 20 ++++ src/ntcore/NetworkTable.cs | 97 ++++++++++++++++-- src/ntcore/NetworkTableInstance.cs | 113 ++++++++++++++++++++- src/ntcore/ProtobufEntryImpl.cs | 81 ++++++++++++++- src/ntcore/StructArrayEntryImpl.cs | 83 +++++++++++++-- src/ntcore/StructEntryImpl.cs | 83 +++++++++++++-- src/ntcore/ValueEventData.cs | 22 +++- src/wpilibsharp/RobotBase.cs | 6 ++ src/wpilibsharp/RobotRunner.cs | 21 +++- src/wpilibsharp/wpilibsharp.csproj | 1 + src/wpiutil/Marshal/WPIStringMarshaller.cs | 2 +- test/wpiutil.test/WPIEventTest.cs | 6 +- 16 files changed, 550 insertions(+), 62 deletions(-) create mode 100644 src/wpilibsharp/RobotBase.cs diff --git a/dev/desktopDev/Program.cs b/dev/desktopDev/Program.cs index 66ea0cde..ffa0f738 100644 --- a/dev/desktopDev/Program.cs +++ b/dev/desktopDev/Program.cs @@ -53,7 +53,8 @@ static void Main(string[] args) Source = camera }; - RawSink sink = new RawSink("Sink") { + RawSink sink = new RawSink("Sink") + { Source = camera }; @@ -68,15 +69,18 @@ static void Main(string[] args) ConnectionStrategy = ConnectionStrategy.ConnectionKeepOpen }; - MJpegServer sinkServer = new MJpegServer("SinkServer", 1182) { + MJpegServer sinkServer = new MJpegServer("SinkServer", 1182) + { Source = source }; RawFrameReader reader = new RawFrameReader(); - while (true) { + while (true) + { long ts = sink.GrabFrame(reader); - if (ts <= 0) { + if (ts <= 0) + { continue; } diff --git a/src/cscore/VideoEvent.cs b/src/cscore/VideoEvent.cs index d1867399..f797be2f 100644 --- a/src/cscore/VideoEvent.cs +++ b/src/cscore/VideoEvent.cs @@ -3,6 +3,7 @@ using System.Runtime.InteropServices.Marshalling; using CsCore.Handles; using CsCore.Natives; +using WPIUtil; using WPIUtil.Marshal; namespace CsCore; @@ -11,9 +12,14 @@ namespace CsCore; [StructLayout(LayoutKind.Auto)] public readonly struct VideoEvent : INativeArrayFree { - public required EventKind Kind { get; init; } + public EventKind Kind { get; } + public CsListener Listener { get; } - public required CsListener Listener { get; init; } + public VideoEvent(in VideoEventMarshaller.NativeCsEvent csEvent) + { + Kind = csEvent.kind; + Listener = new CsListener(csEvent.listener); + } public static unsafe void FreeArray(VideoEventMarshaller.NativeCsEvent* ptr, int len) { @@ -21,18 +27,39 @@ public static unsafe void FreeArray(VideoEventMarshaller.NativeCsEvent* ptr, int } } +[StructLayout(LayoutKind.Explicit)] +internal struct VideoEventUnion +{ + [FieldOffset(0)] + public CsSource source; + [FieldOffset(0)] + public CsSink sink; + [FieldOffset(0)] + public VideoMode mode; + [FieldOffset(0)] + public PropertyTuple property; +} + +[StructLayout(LayoutKind.Auto)] +internal struct PropertyTuple +{ + public CsProperty property; + public PropertyKind kind; + public int value; +} + [CustomMarshaller(typeof(VideoEvent), MarshalMode.ElementOut, typeof(VideoEventMarshaller))] public static unsafe class VideoEventMarshaller { public static NativeCsEvent ConvertToUnmanaged(in VideoEvent managed) { - throw new System.NotSupportedException(); + throw new NotSupportedException(); } public static VideoEvent ConvertToManaged(in NativeCsEvent unmanaged) { - throw new NotImplementedException(); + return new(unmanaged); } [StructLayout(LayoutKind.Sequential)] @@ -41,12 +68,12 @@ public struct NativeCsEvent public EventKind kind; public int source; public int sink; - public byte* name; + public WpiStringMarshaller.WpiStringNative name; public VideoMode mode; public int property; public PropertyKind propertyKind; public int value; - public byte* valueStr; + public WpiStringMarshaller.WpiStringNative valueStr; public int listener; } } diff --git a/src/hal/Natives/HalBase.cs b/src/hal/Natives/HalBase.cs index f9241a64..a9ce3276 100644 --- a/src/hal/Natives/HalBase.cs +++ b/src/hal/Natives/HalBase.cs @@ -9,6 +9,11 @@ namespace WPIHal.Natives; public static partial class HalBase { + [LibraryImport("wpiHal", EntryPoint = "HAL_Initialize")] + [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] + [return: MarshalAs(UnmanagedType.I4)] + public static partial bool Initialize(int timeout, int mode); + [LibraryImport("wpiHal", EntryPoint = "HAL_ExpandFPGATime")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] internal static partial ulong ExpandFPGATimeRefShim(uint unexpandedLower, ref HalStatus status); diff --git a/src/ntcore/MultiSubscriber.cs b/src/ntcore/MultiSubscriber.cs index 6e988039..9a1fc3fc 100644 --- a/src/ntcore/MultiSubscriber.cs +++ b/src/ntcore/MultiSubscriber.cs @@ -4,29 +4,20 @@ namespace NetworkTables; -public sealed class MultiSubscriber : IDisposable +public sealed class MultiSubscriber(NetworkTableInstance inst, string[] prefixes, in PubSubOptions options) : IDisposable { - public MultiSubscriber(NetworkTableInstance inst, string[] prefixes, in PubSubOptions options) - { - Inst = inst; - Handle = NtCore.SubscribeMultiple(inst.Handle, prefixes, (nuint)prefixes.Length, options); - } - public void Dispose() { - lock (this) + if (Handle.Handle != 0) { - if (Handle.Handle != 0) - { - NtCore.UnsubscribeMultiple(Handle); - Handle = default; - } + NtCore.UnsubscribeMultiple(Handle); + Handle = default; } } - public NtMultiSubscriber Handle { get; private set; } + public NtMultiSubscriber Handle { get; private set; } = NtCore.SubscribeMultiple(inst.Handle, prefixes, (nuint)prefixes.Length, options); - public NetworkTableInstance Inst { get; } + public NetworkTableInstance Instance { get; } = inst; public bool IsValid => Handle.Handle != 0; } diff --git a/src/ntcore/Natives/NtCore.cs b/src/ntcore/Natives/NtCore.cs index 493dd1b3..d9d52eda 100644 --- a/src/ntcore/Natives/NtCore.cs +++ b/src/ntcore/Natives/NtCore.cs @@ -59,6 +59,16 @@ public static NetworkTableValue GetEntryValue(T entry) where T : struct, INtE return value; } + [LibraryImport("ntcore", EntryPoint = "NT_GetEntryValueType")] + [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] + internal static partial void GetEntryValue(int entry, NetworkTableType types, out NetworkTableValue value); + + public static NetworkTableValue GetEntryValue(T entry, NetworkTableType types) where T : struct, INtEntryHandle + { + GetEntryValue(entry.Handle, types, out var value); + return value; + } + [LibraryImport("ntcore", EntryPoint = "NT_SetDefaultEntryValue")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] [return: MarshalAs(UnmanagedType.I4)] @@ -107,6 +117,16 @@ public static unsafe NetworkTableValue[] ReadQueueValue(T subentry) where T : return ReadQueueValue(subentry.Handle, out var _); } + [LibraryImport("ntcore", EntryPoint = "NT_ReadQueueValueType")] + [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] + [return: MarshalUsing(typeof(ManagedFreeArrayMarshaller<,>), CountElementName = nameof(count))] + internal static unsafe partial NetworkTableValue[] ReadQueueValue(int subentry, NetworkTableType types, out nuint count); + + public static unsafe NetworkTableValue[] ReadQueueValue(T subentry, NetworkTableType types) where T : struct, INtEntryHandle + { + return ReadQueueValue(subentry.Handle, types, out var _); + } + [LibraryImport("ntcore", EntryPoint = "NT_GetTopics")] [UnmanagedCallConv(CallConvs = [typeof(CallConvCdecl)])] [return: MarshalUsing(typeof(ManagedFreeArrayMarshaller<,>), CountElementName = "count")] diff --git a/src/ntcore/NetworkTable.cs b/src/ntcore/NetworkTable.cs index 0e47f45b..9a351dbc 100644 --- a/src/ntcore/NetworkTable.cs +++ b/src/ntcore/NetworkTable.cs @@ -1,8 +1,10 @@ using System; using System.Collections.Concurrent; using System.Collections.Generic; +using System.IO; using NetworkTables.Handles; using NetworkTables.Natives; +using WPIUtil.Concurrent; namespace NetworkTables; @@ -21,12 +23,26 @@ public static ReadOnlySpan BasenameKey(ReadOnlySpan key) return key[(slash + 1)..]; } - public static string NormalizeKey(ReadOnlySpan key, bool withLeadingSlash = true) + public static string NormalizeKey(string key, bool withLeadingSlash = true) { - throw new NotImplementedException(); + string normalized; + if (withLeadingSlash) + { + normalized = $"{PATH_SEPARATOR}{key}"; + } + else + { + normalized = key; + } + normalized = normalized.Replace($"{PATH_SEPARATOR}{PATH_SEPARATOR}", $"{PATH_SEPARATOR}"); + if (!withLeadingSlash && normalized[0] == PATH_SEPARATOR) + { + normalized = normalized[1..]; + } + return normalized; } - public static List GetHierarchy(ReadOnlySpan key) + public static List GetHierarchy(string key) { string normal = NormalizeKey(key); List hierarchy = []; @@ -180,17 +196,82 @@ public NetworkTableValue GetValue(string key) public string Path { get; } - // public NtListener AddListener(EventFlags kinds, Action listener) { - // int prefixLex = Path.Length + 1; - // return Instance.Add - // } + public NtListener AddListener(EventFlags eventKinds, Action listener) + { + int prefixLex = Path.Length + 1; + return Instance.AddListener([m_pathWithSep], eventKinds, ntEvent => + { + string? topicName = null; + if (ntEvent.TopicInfo != null) + { + topicName = ntEvent.TopicInfo.Value.Name; + } + else if (ntEvent.ValueData != null) + { + // Don't fully construct the lazy object + topicName = ntEvent.ValueData.Value.GetTopicName(); + } + if (topicName == null) + { + return; + } + string relativeKey = topicName[prefixLex..]; + if (relativeKey.Contains(PATH_SEPARATOR)) + { + // part of a subtable + return; + } + listener(this, relativeKey, ntEvent); + }); + } + + public NtListener AddListener(string key, EventFlags eventKinds, Action listener) + { + var entry = GetEntry(key); + return Instance.AddListener(entry, eventKinds, ntEvent => listener(this, key, ntEvent)); + } + + private class SubTableListenerHolder(int prefixLen, NetworkTable parent, Action listener) + { + private readonly int m_prefixLen = prefixLen; + private readonly NetworkTable m_parent = parent; + private readonly Action m_listener = listener; + private readonly HashSet m_notifiedTables = []; + + public void OnEvent(NetworkTableEvent ntEvent) + { + if (ntEvent.TopicInfo == null) + { + return; + } + var relativeKey = ntEvent.TopicInfo.Value.Name.AsSpan()[m_prefixLen..]; + int endSubTable = relativeKey.IndexOf(PATH_SEPARATOR); + if (endSubTable == -1) + { + return; + } + string subTableKey = relativeKey[..endSubTable].ToString(); + if (m_notifiedTables.Contains(subTableKey)) + { + return; + } + m_notifiedTables.Add(subTableKey); + m_listener(m_parent, subTableKey, m_parent.GetSubTable(subTableKey)); + } + } + + public NtListener AddSubTableListener(Action listener) + { + int prefixLen = Path.Length + 1; + SubTableListenerHolder holder = new(prefixLen, this, listener); + return Instance.AddListener([m_pathWithSep], EventFlags.Publish | EventFlags.Immediate, holder.OnEvent); + } public void RemoveListener(NtListener listener) { Instance.RemoveListener(listener); } - public override bool Equals(object? obj) { return Equals(obj as NetworkTable); diff --git a/src/ntcore/NetworkTableInstance.cs b/src/ntcore/NetworkTableInstance.cs index 9dcb5c90..c2e68ef8 100644 --- a/src/ntcore/NetworkTableInstance.cs +++ b/src/ntcore/NetworkTableInstance.cs @@ -8,10 +8,11 @@ using System.Collections.Concurrent; using System.Collections.Generic; using System.Numerics; +using System.Runtime.CompilerServices; using System.Runtime.InteropServices; using System.Text; using System.Threading; -using Google.Protobuf.WellKnownTypes; +using CommunityToolkit.Diagnostics; using NetworkTables.Handles; using NetworkTables.Natives; using WPIUtil.Concurrent; @@ -41,6 +42,17 @@ namespace NetworkTables; */ public sealed partial class NetworkTableInstance : IDisposable, IEquatable, IEqualityOperators { + private static readonly ConcurrentDictionary s_instances = new(); + + public static NetworkTableInstance? GetInstanceForHandle(NtInst inst) + { + if (s_instances.TryGetValue(inst, out var value)) + { + return value; + } + return null; + } + public const int KDefaultPort3 = 1735; public const int KDefaultPort4 = 5810; @@ -61,20 +73,39 @@ public void Dispose() { if (m_owned & Handle.Handle != 0) { + m_listeners.Dispose(); + foreach (var s in m_schemas) + { + s.Value.Dispose(); + } NtCore.DestroyInstance(Handle); + s_instances.TryRemove(Handle, out var _); Handle = default; } } public bool IsValid => Handle.Handle != 0; - private static readonly Lazy s_defaultInstance = new(); + private static readonly Lazy s_defaultInstance = new(() => + { + NetworkTableInstance newInstance = new(); + if (!s_instances.TryAdd(newInstance.Handle, newInstance)) + { + throw new InvalidOperationException("Somehow a duplicate default instance was created"); + } + return newInstance; + }); public static NetworkTableInstance Default => s_defaultInstance.Value; public static NetworkTableInstance Create() { - return new(NtCore.CreateInstance(), true); + NetworkTableInstance newInstance = new(NtCore.CreateInstance(), true); + if (!s_instances.TryAdd(newInstance.Handle, newInstance)) + { + throw new InvalidOperationException("Somehow a duplicate handle was created"); + } + return newInstance; } public Topic GetTopic(string name) @@ -159,7 +190,7 @@ private class ListenerStorage(NetworkTableInstance inst) : IDisposable private readonly Event m_waitQueueEvent = new(); private readonly NetworkTableInstance m_inst = inst; - public NtListener Add(string[] prefixes, EventFlags eventKinds, Action listener) + public NtListener Add(ReadOnlySpan prefixes, EventFlags eventKinds, Action listener) { lock (m_lock) { @@ -189,6 +220,36 @@ public NtListener Add(NtInst handle, EventFlags eventKinds, Action listener) + { + lock (m_lock) + { + if (m_poller.Handle == 0) + { + m_poller = NtCore.CreateListenerPoller(m_inst.Handle); + StartThread(); + } + NtListener h = NtCore.AddListener(m_poller, handle, eventKinds); + m_listeners.Add(h, listener); + return h; + } + } + + public NtListener Add(NtMultiSubscriber handle, EventFlags eventKinds, Action listener) + { + lock (m_lock) + { + if (m_poller.Handle == 0) + { + m_poller = NtCore.CreateListenerPoller(m_inst.Handle); + StartThread(); + } + NtListener h = NtCore.AddListener(m_poller, handle, eventKinds); + m_listeners.Add(h, listener); + return h; + } + } + public NtListener Add(T handle, EventFlags eventKinds, Action listener) where T : struct, INtEntryHandle { lock (m_lock) @@ -337,6 +398,48 @@ public NtListener AddConnectionListener(bool immediateNotify, Action listener) + { + EventFlags flags = EventFlags.TimeSync; + if (immediateNotify) + { + flags |= EventFlags.Immediate; + } + return m_listeners.Add(Handle, flags, listener); + } + + public NtListener AddListener(Topic topic, EventFlags eventKinds, Action listener) + { + if (topic.Instance.Handle != Handle) + { + ThrowHelper.ThrowArgumentException("Topic is not from this instance"); + } + return m_listeners.Add(topic.Handle, eventKinds, listener); + } + + public NtListener AddListener(ISubscriber subscriber, EventFlags eventKinds, Action listener) + { + if (subscriber.Topic.Instance.Handle != Handle) + { + ThrowHelper.ThrowArgumentException("Topic is not from this instance"); + } + return m_listeners.Add(subscriber.Handle, eventKinds, listener); + } + + public NtListener AddListener(MultiSubscriber subscriber, EventFlags eventKinds, Action listener) + { + if (subscriber.Instance.Handle != Handle) + { + ThrowHelper.ThrowArgumentException("Topic is not from this instance"); + } + return m_listeners.Add(subscriber.Handle, eventKinds, listener); + } + + public NtListener AddListener(ReadOnlySpan prefixes, EventFlags eventKinds, Action listener) + { + return m_listeners.Add(prefixes, eventKinds, listener); + } + public NetworkMode GetNetworkMode() { return NtCore.GetNetworkMode(Handle); @@ -423,7 +526,7 @@ public void StopDsClient() NtCore.StopDSClient(Handle); } - public void FlushLLocal() + public void FlushLocal() { NtCore.FlushLocal(Handle); } diff --git a/src/ntcore/ProtobufEntryImpl.cs b/src/ntcore/ProtobufEntryImpl.cs index 7e194c00..b9fee201 100644 --- a/src/ntcore/ProtobufEntryImpl.cs +++ b/src/ntcore/ProtobufEntryImpl.cs @@ -34,7 +34,7 @@ public T Get(T defaultValue) public bool GetInto(ref T output) { - NetworkTableValue value = NtCore.GetEntryValue(Handle); + NetworkTableValue value = NtCore.GetEntryValue(Handle, NetworkTableType.Raw); if (!value.IsRaw) { return false; @@ -71,13 +71,66 @@ public TimestampedObject GetAtomic(T defaultValue) public TimestampedObject[] ReadQueue() { + NetworkTableValue[] raw = NtCore.ReadQueueValue(Handle, NetworkTableType.Raw); + var arr = new TimestampedObject[raw.Length]; + int arrCount = 0; + int shrinkCount = 0; + for (int i = 0; i < raw.Length; i++) + { + var parsed = FromRaw(in raw[i]); + if (parsed.HasValue) + { + arr[arrCount] = parsed.Value; + arrCount++; + } + else + { + shrinkCount++; + } + } - throw new System.NotImplementedException(); + if (shrinkCount > 0) + { + var old = arr; + arr = new TimestampedObject[arr.Length - shrinkCount]; + for (int i = 0; i < arr.Length; i++) + { + arr[i] = old[i]; + } + } + return arr; } public T[] ReadQueueValues() { - throw new System.NotImplementedException(); + NetworkTableValue[] raw = NtCore.ReadQueueValue(Handle, NetworkTableType.Raw); + var arr = new T[raw.Length]; + int arrCount = 0; + int shrinkCount = 0; + for (int i = 0; i < raw.Length; i++) + { + var parsed = FromRaw(in raw[i]); + if (parsed.HasValue) + { + arr[arrCount] = parsed.Value.Value; + arrCount++; + } + else + { + shrinkCount++; + } + } + + if (shrinkCount > 0) + { + var old = arr; + arr = new T[arr.Length - shrinkCount]; + for (int i = 0; i < arr.Length; i++) + { + arr[i] = old[i]; + } + } + return arr; } public void Set(T value) @@ -132,7 +185,7 @@ public void Unpublish() private TimestampedObject FromRaw(T defaultValue) { - NetworkTableValue value = NtCore.GetEntryValue(Handle); + NetworkTableValue value = NtCore.GetEntryValue(Handle, NetworkTableType.Raw); if (!value.IsRaw) { return new TimestampedObject(value.Time, value.ServerTime, defaultValue); @@ -154,4 +207,24 @@ private TimestampedObject FromRaw(T defaultValue) return new TimestampedObject(0, 0, defaultValue); } } + + private TimestampedObject? FromRaw(ref readonly NetworkTableValue value) + { + byte[] raw = value.GetRaw(); + if (raw.Length == 0) + { + return null; + } + try + { + lock (m_lockObject) + { + return new TimestampedObject(value.Time, value.ServerTime, m_buf.Read(raw)); + } + } + catch + { + return null; + } + } } diff --git a/src/ntcore/StructArrayEntryImpl.cs b/src/ntcore/StructArrayEntryImpl.cs index 8c33e1fb..7f9bf1c8 100644 --- a/src/ntcore/StructArrayEntryImpl.cs +++ b/src/ntcore/StructArrayEntryImpl.cs @@ -34,7 +34,7 @@ public T[] Get(T[] defaultValue) public ReadOnlySpan GetInto(Span output, out bool copiedAll) { - NetworkTableValue value = NtCore.GetEntryValue(Handle); + NetworkTableValue value = NtCore.GetEntryValue(Handle, NetworkTableType.Raw); if (!value.IsRaw) { copiedAll = false; @@ -73,13 +73,66 @@ public TimestampedObject GetAtomic(T[] defaultValue) public TimestampedObject[] ReadQueue() { + NetworkTableValue[] raw = NtCore.ReadQueueValue(Handle, NetworkTableType.Raw); + var arr = new TimestampedObject[raw.Length]; + int arrCount = 0; + int shrinkCount = 0; + for (int i = 0; i < raw.Length; i++) + { + var parsed = FromRaw(in raw[i]); + if (parsed.HasValue) + { + arr[arrCount] = parsed.Value; + arrCount++; + } + else + { + shrinkCount++; + } + } - throw new System.NotImplementedException(); + if (shrinkCount > 0) + { + var old = arr; + arr = new TimestampedObject[arr.Length - shrinkCount]; + for (int i = 0; i < arr.Length; i++) + { + arr[i] = old[i]; + } + } + return arr; } public T[][] ReadQueueValues() { - throw new System.NotImplementedException(); + NetworkTableValue[] raw = NtCore.ReadQueueValue(Handle, NetworkTableType.Raw); + var arr = new T[raw.Length][]; + int arrCount = 0; + int shrinkCount = 0; + for (int i = 0; i < raw.Length; i++) + { + var parsed = FromRaw(in raw[i]); + if (parsed.HasValue) + { + arr[arrCount] = parsed.Value.Value; + arrCount++; + } + else + { + shrinkCount++; + } + } + + if (shrinkCount > 0) + { + var old = arr; + arr = new T[arr.Length - shrinkCount][]; + for (int i = 0; i < arr.Length; i++) + { + arr[i] = old[i]; + } + } + return arr; } public void Set(ReadOnlySpan value) @@ -134,15 +187,31 @@ public void Unpublish() private TimestampedObject FromRaw(T[] defaultValue) { - NetworkTableValue value = NtCore.GetEntryValue(Handle); - if (!value.IsRaw) + NetworkTableValue value = NtCore.GetEntryValue(Handle, NetworkTableType.Raw); + byte[] raw = value.GetRaw(); + if (raw.Length == 0) { return new TimestampedObject(value.Time, value.ServerTime, defaultValue); } + try + { + lock (m_lockObject) + { + return new TimestampedObject(value.Time, value.ServerTime, m_buf.ReadArray(raw)); + } + } + catch + { + return new TimestampedObject(0, 0, defaultValue); + } + } + + private TimestampedObject? FromRaw(ref readonly NetworkTableValue value) + { byte[] raw = value.GetRaw(); if (raw.Length == 0) { - return new TimestampedObject(value.Time, value.ServerTime, defaultValue); + return null; } try { @@ -153,7 +222,7 @@ private TimestampedObject FromRaw(T[] defaultValue) } catch { - return new TimestampedObject(0, 0, defaultValue); + return null; } } } diff --git a/src/ntcore/StructEntryImpl.cs b/src/ntcore/StructEntryImpl.cs index 07c456a9..c1158d34 100644 --- a/src/ntcore/StructEntryImpl.cs +++ b/src/ntcore/StructEntryImpl.cs @@ -34,7 +34,7 @@ public T Get(T defaultValue) public bool GetInto(ref T output) { - NetworkTableValue value = NtCore.GetEntryValue(Handle); + NetworkTableValue value = NtCore.GetEntryValue(Handle, NetworkTableType.Raw); if (!value.IsRaw) { return false; @@ -71,13 +71,66 @@ public TimestampedObject GetAtomic(T defaultValue) public TimestampedObject[] ReadQueue() { + NetworkTableValue[] raw = NtCore.ReadQueueValue(Handle, NetworkTableType.Raw); + var arr = new TimestampedObject[raw.Length]; + int arrCount = 0; + int shrinkCount = 0; + for (int i = 0; i < raw.Length; i++) + { + var parsed = FromRaw(in raw[i]); + if (parsed.HasValue) + { + arr[arrCount] = parsed.Value; + arrCount++; + } + else + { + shrinkCount++; + } + } - throw new System.NotImplementedException(); + if (shrinkCount > 0) + { + var old = arr; + arr = new TimestampedObject[arr.Length - shrinkCount]; + for (int i = 0; i < arr.Length; i++) + { + arr[i] = old[i]; + } + } + return arr; } public T[] ReadQueueValues() { - throw new System.NotImplementedException(); + NetworkTableValue[] raw = NtCore.ReadQueueValue(Handle, NetworkTableType.Raw); + var arr = new T[raw.Length]; + int arrCount = 0; + int shrinkCount = 0; + for (int i = 0; i < raw.Length; i++) + { + var parsed = FromRaw(in raw[i]); + if (parsed.HasValue) + { + arr[arrCount] = parsed.Value.Value; + arrCount++; + } + else + { + shrinkCount++; + } + } + + if (shrinkCount > 0) + { + var old = arr; + arr = new T[arr.Length - shrinkCount]; + for (int i = 0; i < arr.Length; i++) + { + arr[i] = old[i]; + } + } + return arr; } public void Set(T value) @@ -132,15 +185,31 @@ public void Unpublish() private TimestampedObject FromRaw(T defaultValue) { - NetworkTableValue value = NtCore.GetEntryValue(Handle); - if (!value.IsRaw) + NetworkTableValue value = NtCore.GetEntryValue(Handle, NetworkTableType.Raw); + byte[] raw = value.GetRaw(); + if (raw.Length == 0) { return new TimestampedObject(value.Time, value.ServerTime, defaultValue); } + try + { + lock (m_lockObject) + { + return new TimestampedObject(value.Time, value.ServerTime, m_buf.Read(raw)); + } + } + catch + { + return new TimestampedObject(0, 0, defaultValue); + } + } + + private TimestampedObject? FromRaw(ref readonly NetworkTableValue value) + { byte[] raw = value.GetRaw(); if (raw.Length == 0) { - return new TimestampedObject(value.Time, value.ServerTime, defaultValue); + return null; } try { @@ -151,7 +220,7 @@ private TimestampedObject FromRaw(T defaultValue) } catch { - return new TimestampedObject(0, 0, defaultValue); + return null; } } } diff --git a/src/ntcore/ValueEventData.cs b/src/ntcore/ValueEventData.cs index f4344b88..3d6ebbe3 100644 --- a/src/ntcore/ValueEventData.cs +++ b/src/ntcore/ValueEventData.cs @@ -1,12 +1,30 @@ using System.Runtime.InteropServices; using System.Runtime.InteropServices.Marshalling; +using NetworkTables.Handles; using NetworkTables.Natives; namespace NetworkTables; [NativeMarshalling(typeof(ValueEventDataMarshaller))] [StructLayout(LayoutKind.Auto)] -public readonly record struct ValueEventData(int TopicHandle, int Subentry, NetworkTableValue Value); +public record struct ValueEventData(NtTopic TopicHandle, int Subentry, NetworkTableValue Value) +{ + private Topic? m_topicObject; + public Topic GetTopic(NetworkTableInstance instance) + { + if (m_topicObject == null) + { + m_topicObject = new Topic(instance, TopicHandle); + } + return m_topicObject; + } + + public string GetTopicName() + { + NtCore.GetTopicName(TopicHandle, out var name); + return name; + } +} [CustomMarshaller(typeof(ValueEventData), MarshalMode.ManagedToUnmanagedOut, typeof(ValueEventDataMarshaller))] public static unsafe class ValueEventDataMarshaller @@ -15,7 +33,7 @@ public static ValueEventData ConvertToManaged(in NativeValueEventData unmanaged) { return new ValueEventData { - TopicHandle = unmanaged.topic, + TopicHandle = new(unmanaged.topic), Subentry = unmanaged.subentry, Value = NetworkTableValueMarshaller.ReturnFrom.ConvertToManaged(unmanaged.value), }; diff --git a/src/wpilibsharp/RobotBase.cs b/src/wpilibsharp/RobotBase.cs new file mode 100644 index 00000000..59640b24 --- /dev/null +++ b/src/wpilibsharp/RobotBase.cs @@ -0,0 +1,6 @@ +namespace WPILib; + +public abstract class RobotBase +{ + +} diff --git a/src/wpilibsharp/RobotRunner.cs b/src/wpilibsharp/RobotRunner.cs index e328f6e4..abd16a65 100644 --- a/src/wpilibsharp/RobotRunner.cs +++ b/src/wpilibsharp/RobotRunner.cs @@ -1,5 +1,24 @@ +using System; +using WPIHal.Natives; + namespace WPILib; -public static class RobotRunner { +public static class RobotRunner where T : RobotBase, new() +{ + private static void RunRobot() + { + Console.WriteLine("********** Robot program starting **********"); + + T robot = new(); + } + + public static void StartRobot() + { + if (!HalBase.Initialize(500, 0)) + { + throw new InvalidOperationException("Failed to initialize. Terminating"); + } + RunRobot(); + } } diff --git a/src/wpilibsharp/wpilibsharp.csproj b/src/wpilibsharp/wpilibsharp.csproj index 81ce0538..75742631 100644 --- a/src/wpilibsharp/wpilibsharp.csproj +++ b/src/wpilibsharp/wpilibsharp.csproj @@ -10,6 +10,7 @@ + diff --git a/src/wpiutil/Marshal/WPIStringMarshaller.cs b/src/wpiutil/Marshal/WPIStringMarshaller.cs index cf1e0e5a..eacdf847 100644 --- a/src/wpiutil/Marshal/WPIStringMarshaller.cs +++ b/src/wpiutil/Marshal/WPIStringMarshaller.cs @@ -157,7 +157,7 @@ public unsafe struct WpiStringNative(byte* str, nuint len) : INativeArrayFree