Fix move handling of C++ Sendable in SmartDashboard and LiveWindow

wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp

@@ -7,15 +7,22 @@
 
 #include "frc/smartdashboard/SendableRegistry.h"
 
+#include <memory>
+
 #include <wpi/DenseMap.h>
 #include <wpi/SmallVector.h>
+#include <wpi/UidVector.h>
 #include <wpi/mutex.h>
 
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
+
 using namespace frc;
 
 struct SendableRegistry::Impl {
   struct Component {
     Sendable* sendable = nullptr;
+    SendableBuilderImpl builder;
     std::string name;
     std::string subsystem = "Ungrouped";
     Sendable* parent = nullptr;
@@ -37,17 +44,21 @@ struct SendableRegistry::Impl {
 
   wpi::mutex mutex;
 
-  wpi::DenseMap<void*, Component> components;
+  wpi::UidVector<std::unique_ptr<Component>, 32> components;
+  wpi::DenseMap<void*, UID> componentMap;
   int nextDataHandle = 0;
 
-  Component& GetOrAdd(Sendable* sendable);
+  Component& GetOrAdd(void* sendable, UID* uid = nullptr);
 };
 
 SendableRegistry::Impl::Component& SendableRegistry::Impl::GetOrAdd(
-    Sendable* sendable) {
-  auto& comp = components[sendable];
-  comp.sendable = sendable;
-  return comp;
+    void* sendable, UID* uid) {
+  UID& compUid = componentMap[sendable];
+  if (compUid == 0)
+    compUid = components.emplace_back(std::make_unique<Component>()) + 1;
+  if (uid) *uid = compUid;
+
+  return *components[compUid - 1];
 }
 
 SendableRegistry& SendableRegistry::GetInstance() {
@@ -58,6 +69,7 @@ SendableRegistry& SendableRegistry::GetInstance() {
 void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.name = name.str();
 }
 
@@ -65,6 +77,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
                            int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.SetName(moduleType, channel);
 }
 
@@ -72,6 +85,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
                            int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.SetName(moduleType, moduleNumber, channel);
 }
 
@@ -79,6 +93,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
                            const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.name = name.str();
   comp.subsystem = subsystem.str();
 }
@@ -86,6 +101,7 @@ void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
 void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.name = name.str();
 }
@@ -94,6 +110,7 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
                              int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.SetName(moduleType, channel);
 }
@@ -102,6 +119,7 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
                              int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.SetName(moduleType, moduleNumber, channel);
 }
@@ -110,6 +128,7 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
                              const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto& comp = m_impl->GetOrAdd(sendable);
+  comp.sendable = sendable;
   comp.liveWindow = true;
   comp.name = name.str();
   comp.subsystem = subsystem.str();
@@ -117,82 +136,95 @@ void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
 
 void SendableRegistry::AddChild(Sendable* parent, void* child) {
   std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->components[child];
+  auto& comp = m_impl->GetOrAdd(child);
   comp.parent = parent;
 }
 
 bool SendableRegistry::Remove(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
-  return m_impl->components.erase(sendable);
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return false;
+  UID compUid = it->getSecond();
+  m_impl->components.erase(compUid - 1);
+  m_impl->componentMap.erase(it);
+  return true;
 }
 
 void SendableRegistry::Move(Sendable* to, Sendable* from) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(from);
-  if (it == m_impl->components.end()) return;
-  Impl::Component old = std::move(it->getSecond());
-  m_impl->components.erase(it);
-  m_impl->components[to] = std::move(old);
-  m_impl->components[to].sendable = to;
+  auto it = m_impl->componentMap.find(from);
+  if (it == m_impl->componentMap.end()) return;
+  UID compUid = it->getSecond();
+  m_impl->componentMap.erase(it);
+  m_impl->componentMap[to] = compUid;
+  auto& comp = *m_impl->components[compUid - 1];
+  comp.sendable = to;
+  if (comp.builder.HasTable()) {
+    // rebuild builder, as lambda captures can point to "from"
+    comp.builder.ClearProperties();
+    to->InitSendable(comp.builder);
+  }
 }
 
 bool SendableRegistry::Contains(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
-  return m_impl->components.count(sendable) != 0;
+  return m_impl->componentMap.count(sendable) != 0;
 }
 
 std::string SendableRegistry::GetName(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return std::string{};
-  return it->getSecond().name;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return std::string{};
+  return m_impl->components[it->getSecond() - 1]->name;
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().name = name.str();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->name = name.str();
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int channel) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().SetName(moduleType, channel);
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().SetName(moduleType, moduleNumber, channel);
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
+                                                   channel);
 }
 
 void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
                                const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().name = name.str();
-  it->getSecond().subsystem = subsystem.str();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  auto& comp = *m_impl->components[it->getSecond() - 1];
+  comp.name = name.str();
+  comp.subsystem = subsystem.str();
 }
 
 std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return std::string{};
-  return it->getSecond().subsystem;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return std::string{};
+  return m_impl->components[it->getSecond() - 1]->subsystem;
 }
 
 void SendableRegistry::SetSubsystem(Sendable* sendable,
                                     const wpi::Twine& subsystem) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().subsystem = subsystem.str();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
 }
 
 int SendableRegistry::GetDataHandle() {
@@ -204,9 +236,9 @@ std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
                                                 std::shared_ptr<void> data) {
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return nullptr;
-  auto& comp = it->getSecond();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return nullptr;
+  auto& comp = *m_impl->components[it->getSecond() - 1];
   std::shared_ptr<void> rv;
   if (static_cast<size_t>(handle) < comp.data.size())
     rv = std::move(comp.data[handle]);
@@ -220,42 +252,72 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
                                                 int handle) {
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return nullptr;
-  auto& comp = it->getSecond();
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return nullptr;
+  auto& comp = *m_impl->components[it->getSecond() - 1];
   if (static_cast<size_t>(handle) >= comp.data.size()) return nullptr;
   return comp.data[handle];
 }
 
 void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().liveWindow = true;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->liveWindow = true;
 }
 
 void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->components.find(sendable);
-  if (it == m_impl->components.end()) return;
-  it->getSecond().liveWindow = false;
+  auto it = m_impl->componentMap.find(sendable);
+  if (it == m_impl->componentMap.end()) return;
+  m_impl->components[it->getSecond() - 1]->liveWindow = false;
+}
+
+SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
+  std::scoped_lock lock(m_impl->mutex);
+  UID uid;
+  auto& comp = m_impl->GetOrAdd(sendable, &uid);
+  comp.sendable = sendable;
+  return uid;
+}
+
+Sendable* SendableRegistry::GetSendable(UID uid) {
+  if (uid == 0) return nullptr;
+  std::scoped_lock lock(m_impl->mutex);
+  return m_impl->components[uid - 1]->sendable;
+}
+
+void SendableRegistry::Publish(UID sendableUid,
+                               std::shared_ptr<NetworkTable> table) {
+  std::scoped_lock lock(m_impl->mutex);
+  if (sendableUid == 0) return;
+  auto& comp = *m_impl->components[sendableUid - 1];
+  comp.builder = SendableBuilderImpl{};  // clear any current builder
+  comp.builder.SetTable(table);
+  comp.sendable->InitSendable(comp.builder);
+  comp.builder.UpdateTable();
+  comp.builder.StartListeners();
+}
+
+void SendableRegistry::Update(UID sendableUid) {
+  if (sendableUid == 0) return;
+  std::scoped_lock lock(m_impl->mutex);
+  m_impl->components[sendableUid - 1]->builder.UpdateTable();
 }
 
 void SendableRegistry::ForeachLiveWindow(
     int dataHandle,
-    wpi::function_ref<void(Sendable* sendable, wpi::StringRef name,
-                           wpi::StringRef subsystem, Sendable* parent,
-                           std::shared_ptr<void>& data)>
-        callback) const {
+    wpi::function_ref<void(CallbackData& data)> callback) const {
   assert(dataHandle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  for (auto& i : m_impl->components) {
-    auto& comp = i.getSecond();
-    if (comp.sendable && comp.liveWindow) {
-      if (static_cast<size_t>(dataHandle) >= comp.data.size())
-        comp.data.resize(dataHandle + 1);
-      callback(comp.sendable, comp.name, comp.subsystem, comp.parent,
-               comp.data[dataHandle]);
+  for (auto&& comp : m_impl->components) {
+    if (comp->sendable && comp->liveWindow) {
+      if (static_cast<size_t>(dataHandle) >= comp->data.size())
+        comp->data.resize(dataHandle + 1);
+      CallbackData cbdata{comp->sendable,         comp->name,
+                          comp->subsystem,        comp->parent,
+                          comp->data[dataHandle], comp->builder};
+      callback(cbdata);
     }
   }
 }