From 92be0bd4a6c50cdd0edb1963323ccbd178c0f608 Mon Sep 17 00:00:00 2001
From: Michael Sherman <sherm1@gmail.com>
Date: Tue, 10 Aug 2021 18:24:09 -0700
Subject: [PATCH] Makes ValidateCreatedForThisSystem() inline-able and moves it
 to SystemBase (#15595)

---
 common/identifier.h                           | 10 ++++
 common/test/identifier_test.cc                | 21 +++++++
 systems/framework/diagram.cc                  |  8 +--
 systems/framework/system.cc                   |  2 +-
 systems/framework/system.h                    | 19 -------
 systems/framework/system_base.cc              | 14 +++++
 systems/framework/system_base.h               | 55 ++++++++++++++++++-
 .../framework/system_compatibility_doxygen.h  | 10 ++--
 systems/framework/system_output.h             |  8 +--
 systems/framework/test/system_base_test.cc    | 37 ++++++++++++-
 10 files changed, 147 insertions(+), 37 deletions(-)

diff --git a/common/identifier.h b/common/identifier.h
index 4eca8e9d1c47..bec41960e9d6 100644
--- a/common/identifier.h
+++ b/common/identifier.h
@@ -200,6 +200,16 @@ class Identifier {
     hash_append(hasher, i.value_);
   }
 
+  /** (Internal use only) Compares this possibly-invalid Identifier with one
+  that is known to be valid and returns `false` if they don't match. It is an
+  error if `valid_id` is not actually valid, and that is strictly enforced in
+  Debug builds. However, it is not an error if `this` id is invalid; that
+  results in a `false` return. This method can be faster than testing
+  separately for validity and equality. */
+  bool is_same_as_valid_id(Identifier valid_id) const {
+    return value_ == valid_id.get_value();
+  }
+
  protected:
   // Instantiates an identifier from the underlying representation type.
   explicit Identifier(int64_t val) : value_(val) {}
diff --git a/common/test/identifier_test.cc b/common/test/identifier_test.cc
index 1bf0fd851fae..006a06fc13d7 100644
--- a/common/test/identifier_test.cc
+++ b/common/test/identifier_test.cc
@@ -72,6 +72,17 @@ TEST_F(IdentifierTests, AssignmentAndComparison) {
   EXPECT_TRUE(temp == a3_);
 }
 
+// Check the specialized internal-use compare that can be used on an invalid
+// Identifier.
+TEST_F(IdentifierTests, InvalidOrSameComparison) {
+  AId same_as_a1 = a1_;
+  EXPECT_TRUE(same_as_a1.is_same_as_valid_id(a1_));
+  EXPECT_FALSE(a1_.is_same_as_valid_id(a2_));
+
+  AId invalid;
+  EXPECT_FALSE(invalid.is_same_as_valid_id(a1_));
+}
+
 // Confirms that ids are configured to serve as unique keys in
 // STL containers.
 TEST_F(IdentifierTests, ServeAsMapKey) {
@@ -234,6 +245,16 @@ TEST_F(IdentifierTests, InvalidInequalityCompare) {
   DRAKE_EXPECT_THROWS_MESSAGE(unused(invalid != a1_), ".*is_valid.*failed.*");
 }
 
+// Comparison of invalid ids is an error.
+TEST_F(IdentifierTests, BadInvalidOrSameComparison) {
+  if (kDrakeAssertIsDisarmed) {
+    return;
+  }
+  AId invalid;
+  DRAKE_EXPECT_THROWS_MESSAGE(unused(a1_.is_same_as_valid_id(invalid)),
+                              ".*is_valid.*failed.*");
+}
+
 // Hashing an invalid id is *not* an error.
 TEST_F(IdentifierTests, InvalidHash) {
   if (kDrakeAssertIsDisarmed) {
diff --git a/systems/framework/diagram.cc b/systems/framework/diagram.cc
index 774040d50464..75f5c4529f72 100644
--- a/systems/framework/diagram.cc
+++ b/systems/framework/diagram.cc
@@ -366,7 +366,7 @@ template <typename T>
 const ContinuousState<T>& Diagram<T>::GetSubsystemDerivatives(
     const System<T>& subsystem,
     const ContinuousState<T>& derivatives) const {
-  this->ValidateCreatedForThisSystem(&derivatives);
+  this->ValidateCreatedForThisSystem(derivatives);
   auto diagram_derivatives =
       dynamic_cast<const DiagramContinuousState<T>*>(&derivatives);
   DRAKE_DEMAND(diagram_derivatives != nullptr);
@@ -378,7 +378,7 @@ template <typename T>
 const DiscreteValues<T>& Diagram<T>::GetSubsystemDiscreteValues(
     const System<T>& subsystem,
     const DiscreteValues<T>& discrete_values) const {
-  this->ValidateCreatedForThisSystem(&discrete_values);
+  this->ValidateCreatedForThisSystem(discrete_values);
   auto diagram_discrete_state =
       dynamic_cast<const DiagramDiscreteValues<T>*>(&discrete_values);
   DRAKE_DEMAND(diagram_discrete_state != nullptr);
@@ -390,7 +390,7 @@ template <typename T>
 const CompositeEventCollection<T>&
 Diagram<T>::GetSubsystemCompositeEventCollection(const System<T>& subsystem,
     const CompositeEventCollection<T>& events) const {
-  this->ValidateCreatedForThisSystem(&events);
+  this->ValidateCreatedForThisSystem(events);
   auto ret = DoGetTargetSystemCompositeEventCollection(subsystem, &events);
   DRAKE_DEMAND(ret != nullptr);
   return *ret;
@@ -427,7 +427,7 @@ State<T>& Diagram<T>::GetMutableSubsystemState(const System<T>& subsystem,
 template <typename T>
 const State<T>& Diagram<T>::GetSubsystemState(const System<T>& subsystem,
                                               const State<T>& state) const {
-  this->ValidateCreatedForThisSystem(&state);
+  this->ValidateCreatedForThisSystem(state);
   auto ret = DoGetTargetSystemState(subsystem, &state);
   DRAKE_DEMAND(ret != nullptr);
   return *ret;
diff --git a/systems/framework/system.cc b/systems/framework/system.cc
index c2b23a4baf9f..6acd51aa8a66 100644
--- a/systems/framework/system.cc
+++ b/systems/framework/system.cc
@@ -274,7 +274,7 @@ void System<T>::CalcImplicitTimeDerivativesResidual(
         this->implicit_time_derivatives_residual_size(), residual->size()));
   }
   ValidateContext(context);
-  ValidateCreatedForThisSystem(&proposed_derivatives);
+  ValidateCreatedForThisSystem(proposed_derivatives);
   DoCalcImplicitTimeDerivativesResidual(context, proposed_derivatives,
                                         residual);
 }
diff --git a/systems/framework/system.h b/systems/framework/system.h
index bd8e47426dda..1738ae8f49b8 100644
--- a/systems/framework/system.h
+++ b/systems/framework/system.h
@@ -1693,25 +1693,6 @@ class System : public SystemBase {
     return system_scalar_converter_;
   }
 
-  /** Checks whether the given object was created for this system.
-  @note This method is sufficiently fast for performance sensitive code. */
-  template <template <typename> class Clazz>
-  void ValidateCreatedForThisSystem(const Clazz<T>* object) const {
-    DRAKE_THROW_UNLESS(object != nullptr);
-    if (!object->get_system_id().is_valid()) {
-      throw std::logic_error(fmt::format(
-          "{} lacks a system_id so was not created for {} system {}",
-          NiceTypeName::Get<Clazz<T>>(), this->GetSystemType(),
-          this->GetSystemPathname()));
-    }
-    if (object->get_system_id() != this->get_system_id()) {
-      throw std::logic_error(fmt::format(
-          "{} was not created for {} system {}",
-          NiceTypeName::Get<Clazz<T>>(), this->GetSystemType(),
-          this->GetSystemPathname()));
-    }
-  }
-
   template <template <typename> class Clazz>
   DRAKE_DEPRECATED("2021-09-01",
                    "Please use ValidateCreatedForThisSystem instead.")
diff --git a/systems/framework/system_base.cc b/systems/framework/system_base.cc
index 991351b99bda..3d720e2b4c84 100644
--- a/systems/framework/system_base.cc
+++ b/systems/framework/system_base.cc
@@ -281,6 +281,20 @@ void SystemBase::ThrowValidateContextMismatch(
       context.GetSystemPathname()));
 }
 
+void SystemBase::ThrowNotCreatedForThisSystemImpl(
+    const std::string& nice_type_name, internal::SystemId id) const {
+  if (!id.is_valid()) {
+    throw std::logic_error(fmt::format(
+        "{} was not associated with any System but should have been "
+        "created for {} System {}",
+        nice_type_name, GetSystemType(), GetSystemPathname()));
+  } else {
+    throw std::logic_error(fmt::format("{} was not created for {} System {}",
+                                       nice_type_name, GetSystemType(),
+                                       GetSystemPathname()));
+  }
+}
+
 [[noreturn]] void SystemBase::ThrowUnsupportedScalarConversion(
     const SystemBase& from, const std::string& destination_type_name) {
   std::stringstream ss;
diff --git a/systems/framework/system_base.h b/systems/framework/system_base.h
index ef7099099a6b..f9590218e102 100644
--- a/systems/framework/system_base.h
+++ b/systems/framework/system_base.h
@@ -817,21 +817,59 @@ class SystemBase : public internal::SystemMessageInterface {
                : num_continuous_states();
   }
 
+  // Note that it is extremely unlikely that a Context will have an invalid
+  // system id because it is near impossible for a user to create one. We'll
+  // just assume it's valid as a precondition on the ValidateContext() methods.
+  // In Debug builds this will be reported as an error but otherwise a
+  // readable but imperfect "Not created for this System" message will issue
+  // if there is no id.
+
+  // @pre both `context` and `this` have valid System Ids.
   /** Checks whether the given context was created for this system.
-  @note This method is sufficiently fast for performance sensitive code. */
+  @note This method is sufficiently fast for performance sensitive code.
+  @throws std::exception if the System Ids don't match. */
   void ValidateContext(const ContextBase& context) const final {
     if (context.get_system_id() != system_id_) {
       ThrowValidateContextMismatch(context);
     }
   }
 
+  // @pre if `context` is non-null, then both `context` and `this` have valid
+  //      System Ids.
   /** Checks whether the given context was created for this system.
-  @note This method is sufficiently fast for performance sensitive code. */
-  void ValidateContext(ContextBase* context) const {
+  @note This method is sufficiently fast for performance sensitive code.
+  @throws std::exception if the System Ids don't match.
+  @throws std::exception if `context` is null. */
+  void ValidateContext(const ContextBase* context) const {
     DRAKE_THROW_UNLESS(context != nullptr);
     ValidateContext(*context);
   }
 
+  // In contrast to Contexts it is easier to create some System-related objects
+  // without assigning them a valid system id. So for checking arbitrary object
+  // types we'll permit the object to have an invalid system id. (We still
+  // require that this SystemBase has a valid system id.)
+
+  // @pre `this` System has a valid system id.
+  /** Checks whether the given object was created for this system.
+  @note This method is sufficiently fast for performance sensitive code.
+  @throws std::exception if the System Ids don't match or if `object` doesn't
+                         have an associated System Id.
+  @throws std::exception if the argument type is a pointer and it is null. */
+  template <class Clazz>
+  void ValidateCreatedForThisSystem(const Clazz& object) const {
+    const internal::SystemId id = [&]() {
+      if constexpr (std::is_pointer_v<Clazz>) {
+        DRAKE_THROW_UNLESS(object != nullptr);
+        return object->get_system_id();
+      } else {
+        return object.get_system_id();
+      }
+    }();
+    if (!id.is_same_as_valid_id(system_id_))
+      ThrowNotCreatedForThisSystem(object, id);
+  }
+
  protected:
   /** (Internal use only). */
   SystemBase() = default;
@@ -1210,6 +1248,17 @@ class SystemBase : public internal::SystemMessageInterface {
     return abstract_parameter_tickets_[index];
   }
 
+  template <class Clazz>
+  [[noreturn]] void ThrowNotCreatedForThisSystem(const Clazz& object,
+                                                 internal::SystemId id) const {
+    unused(object);
+    ThrowNotCreatedForThisSystemImpl(
+        NiceTypeName::Get<std::remove_pointer_t<Clazz>>(), id);
+  }
+
+  [[noreturn]] void ThrowNotCreatedForThisSystemImpl(
+      const std::string& nice_type_name, internal::SystemId id) const;
+
   // Ports and cache entries hold their own DependencyTickets. Note that the
   // addresses of the elements are stable even if the std::vectors are resized.
 
diff --git a/systems/framework/system_compatibility_doxygen.h b/systems/framework/system_compatibility_doxygen.h
index 5e65a91a5ade..9c924e0ff449 100644
--- a/systems/framework/system_compatibility_doxygen.h
+++ b/systems/framework/system_compatibility_doxygen.h
@@ -34,7 +34,7 @@ in the system ID hinting family by implementing the following concept:
 get_system_id of an object whose set_system_id has not been called will return
 an invalid (default-constructed, zero-valued) ID.
 
-A System method participates by calling ValidateCreatedForThisSystem(&object)
+A System method participates by calling ValidateCreatedForThisSystem(object)
 on an object that implements the concept.
 
 An invalid system ID represents the absence of information about the associated
@@ -53,10 +53,10 @@ Where an ID-bearing object implements Clone(), it MUST clone the system ID and,
 recursively, those of its members, whether valid or invalid.
 
 Where an ID-bearing object implements the potentially-scalar-converting
-SetFrom(), if the scalar types differ, it MUST NOT set the system IDs, nor may it
-set the system IDs of its copied members (because being of the wrong type it is
-no longer valid data for the original System). This prohibition does not apply
-when the scalar types are the same.
+SetFrom(), if the scalar types differ, it MUST NOT set the system IDs, nor may
+it set the system IDs of its copied members (because being of the wrong type it
+is no longer valid data for the original System). This prohibition does not
+apply when the scalar types are the same.
 
 Data types that currently implement the above-described scheme include Context,
 ContinuousState, DiscreteValues, CompositeEventCollection, Parameters, State,
diff --git a/systems/framework/system_output.h b/systems/framework/system_output.h
index be9795695682..e3b134e15a93 100644
--- a/systems/framework/system_output.h
+++ b/systems/framework/system_output.h
@@ -79,6 +79,10 @@ class SystemOutput {
     return &port_values_[index]->template get_mutable_value<BasicVector<T>>();
   }
 
+  /** (Internal) Gets the id of the System that created this output.
+  See @ref system_compatibility. */
+  internal::SystemId get_system_id() const { return system_id_; }
+
  private:
   friend class System<T>;
   friend class SystemOutputTest;
@@ -90,10 +94,6 @@ class SystemOutput {
     port_values_.emplace_back(std::move(model_value));
   }
 
-  // Gets the id of the subsystem that created this output.
-  // See @ref system_compatibility.
-  internal::SystemId get_system_id() const { return system_id_; }
-
   // Records the id of the subsystem that created this output.
   // See @ref system_compatibility.
   void set_system_id(internal::SystemId id) { system_id_ = id; }
diff --git a/systems/framework/test/system_base_test.cc b/systems/framework/test/system_base_test.cc
index e2a5fdf37351..d5ebd35d9415 100644
--- a/systems/framework/test/system_base_test.cc
+++ b/systems/framework/test/system_base_test.cc
@@ -21,6 +21,7 @@ namespace systems {
 namespace system_base_test_internal {
 
 // A minimal concrete ContextBase object suitable for some simple tests.
+// Objects of this class are created with no system id.
 class MyContextBase final : public ContextBase {
  public:
   MyContextBase() = default;
@@ -71,13 +72,47 @@ GTEST_TEST(SystemBaseTest, NameAndMessageSupport) {
   EXPECT_EQ(system.GetSystemType(),
             "drake::systems::system_base_test_internal::MySystemBase");
 
-  auto context = system.AllocateContext();
+  // Test specialized ValidateContext() and more-general
+  // ValidateCreatedForThisSystem() which can check anything that supports
+  // a get_system_id() method (we'll just use Context here for that also
+  // since it qualifies). Both methods have pointer and non-pointer variants
+  // but should ignore that distinction in the error message.
+
+  std::unique_ptr<ContextBase> context = system.AllocateContext();
   DRAKE_EXPECT_NO_THROW(system.ValidateContext(*context));
+  DRAKE_EXPECT_NO_THROW(system.ValidateContext(context.get()));
+  DRAKE_EXPECT_NO_THROW(system.ValidateCreatedForThisSystem(*context));
+  DRAKE_EXPECT_NO_THROW(system.ValidateCreatedForThisSystem(context.get()));
 
   MySystemBase other_system;
   auto other_context = other_system.AllocateContext();
   DRAKE_EXPECT_THROWS_MESSAGE(system.ValidateContext(*other_context),
                               ".*Context.*was not created for.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(system.ValidateContext(other_context.get()),
+                              ".*Context.*was not created for.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      system.ValidateCreatedForThisSystem(*other_context),
+      ".*ContextBase.*was not created for.*MySystemBase.*any_name_will_do.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      system.ValidateCreatedForThisSystem(other_context.get()),
+      ".*ContextBase.*was not created for.*MySystemBase.*any_name_will_do.*");
+
+  // These methods check for null pointers even in Release builds but don't
+  // generate fancy messages for them. We're happy as long as "nullptr" gets
+  // mentioned.
+  ContextBase* null_context = nullptr;
+  DRAKE_EXPECT_THROWS_MESSAGE(system.ValidateContext(null_context),
+                              ".*nullptr.*");
+  DRAKE_EXPECT_THROWS_MESSAGE(system.ValidateCreatedForThisSystem(null_context),
+                              ".*nullptr.*");
+
+  MyContextBase disconnected_context;  // No system id.
+  // ValidateContext() can't be used on a Context that has no system id.
+
+  DRAKE_EXPECT_THROWS_MESSAGE(
+      system.ValidateCreatedForThisSystem(disconnected_context),
+      ".*MyContextBase.*was not associated.*should have been "
+      "created for.*MySystemBase.*any_name_will_do.*");
 }
 
 }  // namespace system_base_test_internal