[wpilibc] SendableRegistry: Add range and null checks (#2830)

If a Sendable like SendableChooser is destroyed and recreated, it leaves
a stale object in the Sendable registry. Using this object results in a
crash. This patch avoids using the stale object.

We should remove stale objects from the global registry upon object
destruction, but this fixes the crashing issue for now.

Closes #2818.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

.github/workflows/ci.yml

@@ -8,7 +8,7 @@ jobs:
       fail-fast: false
       matrix:
         include:
-          - container: wpilib/roborio-cross-ubuntu:2020-18.04
+          - container: wpilib/roborio-cross-ubuntu:2021-18.04
             artifact-name: Athena
             build-options: "-Ponlylinuxathena"
           - container: wpilib/raspbian-cross-ubuntu:10-18.04
@@ -101,8 +101,11 @@ jobs:
       - uses: actions/setup-java@v1
         with:
           java-version: 13
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
       - name: Build with Gradle
-        run: ./gradlew docs:zipDocs
+        run: ./gradlew docs:zipDocs -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
       - uses: actions/upload-artifact@v2
         with:
           name: Documentation

CONTRIBUTING.md

@@ -1,6 +1,6 @@
 # Contributing to WPILib
 
-So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules written here, and behave with Gracious Professionalism.
+So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/master/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
 
 - [General Contribution Rules](#general-contribution-rules)
 - [What to Contribute](#what-to-contribute)
@@ -37,7 +37,7 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 
 ## Coding Guidelines
 
-WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 6.0 with wpiformat.
+WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 10.0 with wpiformat.
 
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 
@@ -45,11 +45,11 @@ While the library should be fully formatted according to the styles, additional
 
 ### Pull Request Format
 
-Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, we ask that you squash your changes down to a single commit. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. No change will be merged unless it is up to date with the current master. We will also not merge any changes with merge commits in them; please rebase off of master before submitting a pull request. We do this to make sure that the git history isn't too cluttered.
+Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current master. We do this to make sure that the git history isn't too cluttered.
 
 ### Merge Process
 
-When you first submit changes, Azure DevOps will attempt to run `./gradlew check` on your change. If this fails, you will need to fix any issues that it sees. Once Azure passes, we will begin the review process in more earnest. One or more WPILib team members will review your change. This will be a back-and-forth process with the WPILib team and the greater community. Once we are satisfied that your change is ready, we will allow our hosted instance to test it. This will run the full gamut of checks, including integration tests on actual hardware. Once all tests have passed and the team is satisfied, we will merge your change into the WPILib repository.
+When you first submit changes, GitHub Actions will attempt to run `./gradlew check` on your change. If this fails, you will need to fix any issues that it sees. Once Actions passes, we will begin the review process in more earnest. One or more WPILib team members will review your change. This will be a back-and-forth process with the WPILib team and the greater community. Once we are satisfied that your change is ready, we will allow our hosted instance to test it. This will run the full gamut of checks, including integration tests on actual hardware. Once all tests have passed and the team is satisfied, we will merge your change into the WPILib repository.
 
 ## Licensing
 

OtherVersions.md

@@ -58,7 +58,8 @@ wpi.wpilibVersion = 'YEAR.424242.+'
 C++
 ```groovy
 plugins {
-  id "java"
+  id "cpp"
+  id "google-test-test-suite"
   id "edu.wpi.first.GradleRIO" version "2020.3.2"
 }
 

azure-pipelines-testbench.yaml

@@ -15,7 +15,7 @@ stages:
       vmImage: 'Ubuntu 16.04'
 
     container:
-      image:  wpilib/roborio-cross-ubuntu:2020-18.04
+      image:  wpilib/roborio-cross-ubuntu:2021-18.04
 
     timeoutInMinutes: 0
 

buildSrc/build.gradle

@@ -5,5 +5,5 @@ repositories {
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2021.0.1"
+    implementation "edu.wpi.first:native-utils:2021.0.4"
 }

cmake/toolchains/arm-frc-gnueabi.toolchain.cmake

@@ -1,4 +1,4 @@
 set(GCC_COMPILER_VERSION "" CACHE STRING "GCC Compiler version")
-set(GNU_MACHINE "arm-frc2020-linux-gnueabi" CACHE STRING "GNU compiler triple")
+set(GNU_MACHINE "arm-frc2021-linux-gnueabi" CACHE STRING "GNU compiler triple")
 set(SOFTFP yes)
 include("${CMAKE_CURRENT_LIST_DIR}/arm.toolchain.cmake")

wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp

@@ -44,5 +44,7 @@ double AnalogPotentiometer::Get() const {
 double AnalogPotentiometer::PIDGet() { return Get(); }
 
 void AnalogPotentiometer::InitSendable(SendableBuilder& builder) {
-  m_analog_input->InitSendable(builder);
+  builder.SetSmartDashboardType("Analog Input");
+  builder.AddDoubleProperty(
+      "Value", [=]() { return Get(); }, nullptr);
 }

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

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 FIRST. All Rights Reserved.                        */
 /* Open Source Software - may be modified and shared by FRC teams. The code   */
 /* must be accompanied by the FIRST BSD license file in the root directory of */
 /* the project.                                                               */
@@ -163,7 +163,9 @@ bool SendableRegistry::Remove(Sendable* sendable) {
 void SendableRegistry::Move(Sendable* to, Sendable* from) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(from);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   UID compUid = it->getSecond();
   m_impl->componentMap.erase(it);
   m_impl->componentMap[to] = compUid;
@@ -188,14 +190,18 @@ bool SendableRegistry::Contains(const Sendable* sendable) const {
 std::string SendableRegistry::GetName(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    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->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->name = name.str();
 }
 
@@ -203,7 +209,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
 }
 
@@ -211,7 +219,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
                                int moduleNumber, int channel) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
                                                    channel);
 }
@@ -220,7 +230,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
                                const wpi::Twine& name) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   comp.name = name.str();
   comp.subsystem = subsystem.str();
@@ -229,7 +241,9 @@ void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
 std::string SendableRegistry::GetSubsystem(const Sendable* sendable) const {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return std::string{};
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return std::string{};
   return m_impl->components[it->getSecond() - 1]->subsystem;
 }
 
@@ -237,7 +251,9 @@ void SendableRegistry::SetSubsystem(Sendable* sendable,
                                     const wpi::Twine& subsystem) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
 }
 
@@ -251,7 +267,9 @@ std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return nullptr;
   auto& comp = *m_impl->components[it->getSecond() - 1];
   std::shared_ptr<void> rv;
   if (static_cast<size_t>(handle) < comp.data.size())
@@ -267,7 +285,9 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
   assert(handle >= 0);
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return nullptr;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    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];
@@ -276,14 +296,18 @@ std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
 void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
   std::scoped_lock lock(m_impl->mutex);
   auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    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->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return;
+  if (it == m_impl->componentMap.end() ||
+      !m_impl->components[it->getSecond() - 1])
+    return;
   m_impl->components[it->getSecond() - 1]->liveWindow = false;
 }
 
@@ -298,13 +322,17 @@ SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
 Sendable* SendableRegistry::GetSendable(UID uid) {
   if (uid == 0) return nullptr;
   std::scoped_lock lock(m_impl->mutex);
+  if ((uid - 1) >= m_impl->components.size() || !m_impl->components[uid - 1])
+    return nullptr;
   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;
+  if (sendableUid == 0 || (sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   auto& comp = *m_impl->components[sendableUid - 1];
   comp.builder = SendableBuilderImpl{};  // clear any current builder
   comp.builder.SetTable(table);
@@ -316,6 +344,9 @@ void SendableRegistry::Publish(UID sendableUid,
 void SendableRegistry::Update(UID sendableUid) {
   if (sendableUid == 0) return;
   std::scoped_lock lock(m_impl->mutex);
+  if ((sendableUid - 1) >= m_impl->components.size() ||
+      !m_impl->components[sendableUid - 1])
+    return;
   m_impl->components[sendableUid - 1]->builder.UpdateTable();
 }
 
@@ -327,7 +358,7 @@ void SendableRegistry::ForeachLiveWindow(
   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 (comp && comp->sendable && comp->liveWindow) {
       if (static_cast<size_t>(dataHandle) >= comp->data.size())
         comp->data.resize(dataHandle + 1);
       CallbackData cbdata{comp->sendable,         comp->name,

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java

@@ -159,7 +159,8 @@ public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseab
   @Override
   public void initSendable(SendableBuilder builder) {
     if (m_analogInput != null) {
-      m_analogInput.initSendable(builder);
+      builder.setSmartDashboardType("Analog Input");
+      builder.addDoubleProperty("Value", this::get, null);
     }
   }
 

wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java

@@ -272,29 +272,30 @@ public class LinearSystemLoop<States extends Num, Inputs extends Num,
   }
 
   /**
-   * Zeroes reference r, controller output u and plant output y.
-   * The previous reference for PlantInversionFeedforward is set to the
-   * initial reference.
-   * @param initialReference The initial reference.
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
    */
-  public void reset(Matrix<States, N1> initialReference) {
-    m_controller.reset();
-    m_feedforward.reset(initialReference);
-    m_observer.reset();
+  public void reset(Matrix<States, N1> initialState) {
     m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
   }
 
   /**
-   * Returns difference between reoid predict(double dtSference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    *
-   * @return the
+   * @return The state error matrix.
    */
   public Matrix<States, N1> getError() {
     return getController().getR().minus(m_observer.getXhat());
   }
 
   /**
-   * Returns difference between reference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    *
    * @param index The index of the error matrix to return.
    * @return The error at that index.

wpimath/src/main/native/include/frc/system/LinearSystemLoop.h

@@ -227,20 +227,21 @@ class LinearSystemLoop {
   }
 
   /**
-   * Zeroes reference r, controller output u and plant output y.
-   * The previous reference for PlantInversionFeedforward is set to the
-   * initial reference.
-   * @param initialReference The initial reference.
+   * Zeroes reference r and controller output u. The previous reference
+   * of the PlantInversionFeedforward and the initial state estimate of
+   * the KalmanFilter are set to the initial state provided.
+   *
+   * @param initialState The initial state.
    */
   void Reset(Eigen::Matrix<double, States, 1> initialState) {
+    m_nextR.setZero();
     m_controller.Reset();
     m_feedforward.Reset(initialState);
-    m_observer.Reset();
-    m_nextR.setZero();
+    m_observer.SetXhat(initialState);
   }
 
   /**
-   * Returns difference between reference r and x-hat.
+   * Returns difference between reference r and current state x-hat.
    */
   const Eigen::Matrix<double, States, 1> Error() const {
     return m_controller.R() - m_observer.Xhat();