From 70102a60b7de8d5d643ad9456a8cde015b5dfdad Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Mon, 25 Nov 2019 21:47:06 -0800
Subject: [PATCH] SendableRegistry.foreachLiveWindow: Prevent concurrent
 modification (#2129)

Copy the internal map values into an array before iterating.

In C++, change to recursive mutex as well.
---
 .../native/cpp/smartdashboard/SendableRegistry.cpp    |  6 ++++--
 .../wpilibj/smartdashboard/SendableRegistry.java      | 11 ++++++++++-
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
index 56a27dc6d0..bce619642f 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
@@ -42,7 +42,7 @@ struct SendableRegistry::Impl {
     }
   };
 
-  wpi::mutex mutex;
+  wpi::recursive_mutex mutex;
 
   wpi::UidVector<std::unique_ptr<Component>, 32> components;
   wpi::DenseMap<void*, UID> componentMap;
@@ -310,7 +310,9 @@ void SendableRegistry::ForeachLiveWindow(
     wpi::function_ref<void(CallbackData& data)> callback) const {
   assert(dataHandle >= 0);
   std::scoped_lock lock(m_impl->mutex);
-  for (auto&& comp : m_impl->components) {
+  wpi::SmallVector<Impl::Component*, 128> components;
+  for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
+  for (auto comp : components) {
     if (comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
index 79b9b2a9f6..a8cd5572f0 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
@@ -8,7 +8,9 @@
 package edu.wpi.first.wpilibj.smartdashboard;
 
 import java.lang.ref.WeakReference;
+import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.List;
 import java.util.Map;
 import java.util.WeakHashMap;
 import java.util.function.Consumer;
@@ -454,6 +456,10 @@ public class SendableRegistry {
     public SendableBuilderImpl builder;
   }
 
+  // As foreachLiveWindow is single threaded, cache the components it
+  // iterates over to avoid risk of ConcurrentModificationException
+  private static List<Component> foreachComponents = new ArrayList<>();
+
   /**
    * Iterates over LiveWindow-enabled objects in the registry.
    * It is *not* safe to call other SendableRegistry functions from the
@@ -467,7 +473,9 @@ public class SendableRegistry {
   public static synchronized void foreachLiveWindow(int dataHandle,
       Consumer<CallbackData> callback) {
     CallbackData cbdata = new CallbackData();
-    for (Component comp : components.values()) {
+    foreachComponents.clear();
+    foreachComponents.addAll(components.values());
+    for (Component comp : foreachComponents) {
       if (comp.m_sendable == null) {
         continue;
       }
@@ -508,5 +516,6 @@ public class SendableRegistry {
         }
       }
     }
+    foreachComponents.clear();
   }
 }