[ntcore] Allow numeric-compatible value sets (#4620)

Also fix entry publishing behavior to allow numerically compatible set
default publish following a subscribe.

ntcore/src/main/native/cpp/LocalStorage.cpp

@@ -20,6 +20,7 @@
 #include "Log.h"
 #include "PubSubOptions.h"
 #include "Types_internal.h"
+#include "Value_internal.h"
 #include "networktables/NetworkTableValue.h"
 #include "ntcore_c.h"
 
@@ -271,7 +272,7 @@ struct LSImpl {
                          unsigned int eventFlags, bool sendNetwork,
                          bool updateFlags = true);
 
-  void RefreshPubSubActive(TopicData* topic);
+  void RefreshPubSubActive(TopicData* topic, bool warnOnSubMismatch);
 
   void NetworkAnnounce(TopicData* topic, std::string_view typeStr,
                        const wpi::json& properties, NT_Publisher pubHandle);
@@ -394,17 +395,9 @@ void PublisherData::UpdateActive() {
 void SubscriberData::UpdateActive() {
   // for subscribers, unassigned is a wildcard
   // also allow numerically compatible subscribers
-  active =
-      config.type == NT_UNASSIGNED ||
-      (config.type == topic->type && config.typeStr == topic->typeStr) ||
-      ((config.type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE)) != 0 &&
-       (config.type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE)) ==
-           (topic->type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE))) ||
-      ((config.type & (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)) !=
-           0 &&
-       (config.type & (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)) ==
-           (topic->type &
-            (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)));
+  active = config.type == NT_UNASSIGNED ||
+           (config.type == topic->type && config.typeStr == topic->typeStr) ||
+           IsNumericCompatible(config.type, topic->type);
 }
 
 void LSImpl::NotifyTopic(TopicData* topic, unsigned int eventFlags) {
@@ -620,12 +613,19 @@ void LSImpl::PropertiesUpdated(TopicData* topic, const wpi::json& update,
   }
 }
 
-void LSImpl::RefreshPubSubActive(TopicData* topic) {
+void LSImpl::RefreshPubSubActive(TopicData* topic, bool warnOnSubMismatch) {
   for (auto&& publisher : topic->localPublishers) {
     publisher->UpdateActive();
   }
   for (auto&& subscriber : topic->localSubscribers) {
     subscriber->UpdateActive();
+    if (warnOnSubMismatch && topic->Exists() && !subscriber->active) {
+      // warn on type mismatch
+      INFO(
+          "local subscribe to '{}' disabled due to type mismatch (wanted '{}', "
+          "published as '{}')",
+          topic->name, subscriber->config.typeStr, topic->typeStr);
+    }
   }
 }
 
@@ -653,7 +653,7 @@ void LSImpl::NetworkAnnounce(TopicData* topic, std::string_view typeStr,
     }
     topic->type = type;
     topic->typeStr = typeStr;
-    RefreshPubSubActive(topic);
+    RefreshPubSubActive(topic, true);
   }
   if (!didExist) {
     event |= NT_EVENT_PUBLISH;
@@ -702,7 +702,7 @@ void LSImpl::RemoveNetworkPublisher(TopicData* topic) {
         nextPub->config.typeStr != topic->typeStr) {
       topic->type = nextPub->config.type;
       topic->typeStr = nextPub->config.typeStr;
-      RefreshPubSubActive(topic);
+      RefreshPubSubActive(topic, false);
       // this may result in a duplicate publish warning on the server side,
       // but send one anyway in this case just to be sure
       if (nextPub->active && m_network) {
@@ -730,19 +730,20 @@ PublisherData* LSImpl::AddLocalPublisher(TopicData* topic,
   topic->localPublishers.Add(publisher);
 
   if (!didExist) {
+    DEBUG4("AddLocalPublisher: setting {} type {} typestr {}", topic->name,
+           static_cast<int>(config.type), config.typeStr);
     // set the type to the published type
     topic->type = config.type;
     topic->typeStr = config.typeStr;
-    RefreshPubSubActive(topic);
+    RefreshPubSubActive(topic, true);
 
     if (properties.is_null()) {
       topic->properties = wpi::json::object();
     } else if (properties.is_object()) {
       topic->properties = properties;
     } else {
-      WPI_WARNING(m_logger,
-                  "ignoring non-object properties when publishing '{}'",
-                  topic->name);
+      WARNING("ignoring non-object properties when publishing '{}'",
+              topic->name);
       topic->properties = wpi::json::object();
     }
 
@@ -794,7 +795,7 @@ std::unique_ptr<PublisherData> LSImpl::RemoveLocalPublisher(
           nextPub->config.typeStr != topic->typeStr) {
         topic->type = nextPub->config.type;
         topic->typeStr = nextPub->config.typeStr;
-        RefreshPubSubActive(topic);
+        RefreshPubSubActive(topic, false);
         if (nextPub->active && m_network) {
           m_network->Publish(nextPub->handle, topic->handle, topic->name,
                              topic->typeStr, topic->properties,
@@ -817,7 +818,7 @@ SubscriberData* LSImpl::AddLocalSubscriber(TopicData* topic,
     // warn on type mismatch
     INFO(
         "local subscribe to '{}' disabled due to type mismatch (wanted '{}', "
-        "currently '{}')",
+        "published as '{}')",
         topic->name, config.typeStr, topic->typeStr);
   }
   if (m_network) {
@@ -1176,8 +1177,12 @@ PublisherData* LSImpl::PublishEntry(EntryData* entry, NT_Type type) {
     entry->subscriber->config.typeStr = typeStr;
   } else if (entry->subscriber->config.type != type ||
              entry->subscriber->config.typeStr != typeStr) {
-    // don't allow dynamically changing the type of an entry
-    return nullptr;
+    if (!IsNumericCompatible(type, entry->subscriber->config.type)) {
+      // don't allow dynamically changing the type of an entry
+      ERROR("cannot publish entry {} as type {}, previously subscribed as {}",
+            entry->topic->name, typeStr, entry->subscriber->config.typeStr);
+      return nullptr;
+    }
   }
   // create publisher
   entry->publisher = AddLocalPublisher(entry->topic, wpi::json::object(),
@@ -1199,6 +1204,10 @@ bool LSImpl::PublishLocalValue(PublisherData* publisher, const Value& value) {
   }
   if (publisher->topic->type != NT_UNASSIGNED &&
       publisher->topic->type != value.type()) {
+    if (IsNumericCompatible(publisher->topic->type, value.type())) {
+      return PublishLocalValue(
+          publisher, ConvertNumericValue(value, publisher->topic->type));
+    }
     return false;
   }
   if (publisher->active) {
@@ -1229,27 +1238,39 @@ bool LSImpl::SetEntryValue(NT_Handle pubentryHandle, const Value& value) {
 
 bool LSImpl::SetDefaultEntryValue(NT_Handle pubsubentryHandle,
                                   const Value& value) {
+  DEBUG4("SetDefaultEntryValue({}, {})", pubsubentryHandle,
+         static_cast<int>(value.type()));
   if (!value) {
     return false;
   }
   if (auto topic = GetTopic(pubsubentryHandle)) {
-    if (topic->type == NT_UNASSIGNED || topic->type == value.type()) {
-      // force value timestamps to 0
-      topic->type = value.type();
-      topic->lastValue = value;
-      topic->lastValue.SetTime(0);
-      topic->lastValue.SetServerTime(0);
-
+    if (!topic->lastValue &&
+        (topic->type == NT_UNASSIGNED || topic->type == value.type() ||
+         IsNumericCompatible(topic->type, value.type()))) {
+      // publish if we haven't yet
       auto publisher = m_publishers.Get(pubsubentryHandle);
       if (!publisher) {
         if (auto entry = m_entries.Get(pubsubentryHandle)) {
           publisher = PublishEntry(entry, value.type());
         }
-        if (!publisher) {
-          return true;
-        }
       }
-      PublishLocalValue(publisher, topic->lastValue);
+
+      // force value timestamps to 0
+      if (topic->type == NT_UNASSIGNED) {
+        topic->type = value.type();
+      }
+      if (topic->type == value.type()) {
+        topic->lastValue = value;
+      } else if (IsNumericCompatible(topic->type, value.type())) {
+        topic->lastValue = ConvertNumericValue(value, topic->type);
+      } else {
+        return true;
+      }
+      topic->lastValue.SetTime(0);
+      topic->lastValue.SetServerTime(0);
+      if (publisher) {
+        PublishLocalValue(publisher, topic->lastValue);
+      }
       return true;
     }
   }
@@ -2045,10 +2066,17 @@ READ_QUEUE_NUMBER(Double)
 Value LocalStorage::GetEntryValue(NT_Handle subentryHandle) {
   std::scoped_lock lock{m_mutex};
   if (auto subscriber = m_impl->GetSubEntry(subentryHandle)) {
-    return subscriber->topic->lastValue;
-  } else {
-    return {};
+    if (subscriber->config.type == NT_UNASSIGNED ||
+        !subscriber->topic->lastValue ||
+        subscriber->config.type == subscriber->topic->lastValue.type()) {
+      return subscriber->topic->lastValue;
+    } else if (IsNumericCompatible(subscriber->config.type,
+                                   subscriber->topic->lastValue.type())) {
+      return ConvertNumericValue(subscriber->topic->lastValue,
+                                 subscriber->config.type);
+    }
   }
+  return {};
 }
 
 void LocalStorage::SetEntryFlags(NT_Entry entryHandle, unsigned int flags) {

ntcore/src/main/native/cpp/Types_internal.h

@@ -14,4 +14,17 @@ std::string_view TypeToString(NT_Type type);
 NT_Type StringToType(std::string_view typeStr);
 NT_Type StringToType3(std::string_view typeStr);
 
+constexpr bool IsNumeric(NT_Type type) {
+  return (type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE)) != 0;
+}
+
+constexpr bool IsNumericArray(NT_Type type) {
+  return (type & (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)) != 0;
+}
+
+constexpr bool IsNumericCompatible(NT_Type a, NT_Type b) {
+  return (IsNumeric(a) && IsNumeric(b)) ||
+         (IsNumericArray(a) && IsNumericArray(b));
+}
+
 }  // namespace nt

ntcore/src/main/native/cpp/Value.cpp

@@ -89,6 +89,15 @@ Value Value::MakeBooleanArray(std::span<const int> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeBooleanArray(std::vector<int>&& value, int64_t time) {
+  Value val{NT_BOOLEAN_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<int>>(std::move(value));
+  val.m_val.data.arr_boolean.arr = data->data();
+  val.m_val.data.arr_boolean.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeIntegerArray(std::span<const int64_t> value, int64_t time) {
   Value val{NT_INTEGER_ARRAY, time, private_init{}};
   auto data =
@@ -99,6 +108,15 @@ Value Value::MakeIntegerArray(std::span<const int64_t> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeIntegerArray(std::vector<int64_t>&& value, int64_t time) {
+  Value val{NT_INTEGER_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<int64_t>>(std::move(value));
+  val.m_val.data.arr_int.arr = data->data();
+  val.m_val.data.arr_int.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeFloatArray(std::span<const float> value, int64_t time) {
   Value val{NT_FLOAT_ARRAY, time, private_init{}};
   auto data = std::make_shared<std::vector<float>>(value.begin(), value.end());
@@ -108,6 +126,15 @@ Value Value::MakeFloatArray(std::span<const float> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeFloatArray(std::vector<float>&& value, int64_t time) {
+  Value val{NT_FLOAT_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<float>>(std::move(value));
+  val.m_val.data.arr_float.arr = data->data();
+  val.m_val.data.arr_float.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeDoubleArray(std::span<const double> value, int64_t time) {
   Value val{NT_DOUBLE_ARRAY, time, private_init{}};
   auto data = std::make_shared<std::vector<double>>(value.begin(), value.end());
@@ -117,6 +144,15 @@ Value Value::MakeDoubleArray(std::span<const double> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeDoubleArray(std::vector<double>&& value, int64_t time) {
+  Value val{NT_DOUBLE_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<double>>(std::move(value));
+  val.m_val.data.arr_double.arr = data->data();
+  val.m_val.data.arr_double.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeStringArray(std::span<const std::string> value, int64_t time) {
   Value val{NT_STRING_ARRAY, time, private_init{}};
   auto data = std::make_shared<StringArrayStorage>(value);

ntcore/src/main/native/cpp/Value_internal.cpp

@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Value_internal.h"
+
+using namespace nt;
+
+Value nt::ConvertNumericValue(const Value& value, NT_Type type) {
+  switch (type) {
+    case NT_INTEGER: {
+      Value newval =
+          Value::MakeInteger(GetNumericAs<int64_t>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_FLOAT: {
+      Value newval = Value::MakeFloat(GetNumericAs<float>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_DOUBLE: {
+      Value newval =
+          Value::MakeDouble(GetNumericAs<double>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_INTEGER_ARRAY: {
+      Value newval = Value::MakeIntegerArray(GetNumericArrayAs<int64_t>(value),
+                                             value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_FLOAT_ARRAY: {
+      Value newval =
+          Value::MakeFloatArray(GetNumericArrayAs<float>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_DOUBLE_ARRAY: {
+      Value newval = Value::MakeDoubleArray(GetNumericArrayAs<double>(value),
+                                            value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    default:
+      return {};
+  }
+}

ntcore/src/main/native/cpp/Value_internal.h

@@ -12,6 +12,7 @@
 
 #include <wpi/MemAlloc.h>
 
+#include "networktables/NetworkTableValue.h"
 #include "ntcore_c.h"
 
 namespace nt {
@@ -56,4 +57,35 @@ O* ConvertToC(const std::basic_string<I>& in, size_t* out_len) {
   return out;
 }
 
+template <typename T>
+T GetNumericAs(const Value& value) {
+  if (value.IsInteger()) {
+    return static_cast<T>(value.GetInteger());
+  } else if (value.IsFloat()) {
+    return static_cast<T>(value.GetFloat());
+  } else if (value.IsDouble()) {
+    return static_cast<T>(value.GetDouble());
+  } else {
+    return {};
+  }
+}
+
+template <typename T>
+std::vector<T> GetNumericArrayAs(const Value& value) {
+  if (value.IsIntegerArray()) {
+    auto arr = value.GetIntegerArray();
+    return {arr.begin(), arr.end()};
+  } else if (value.IsFloatArray()) {
+    auto arr = value.GetFloatArray();
+    return {arr.begin(), arr.end()};
+  } else if (value.IsDoubleArray()) {
+    auto arr = value.GetDoubleArray();
+    return {arr.begin(), arr.end()};
+  } else {
+    return {};
+  }
+}
+
+Value ConvertNumericValue(const Value& value, NT_Type type);
+
 }  // namespace nt