Merge pull request RobotLocomotion#17260 from jwnimmer-tri/shared-poi…

…nter-system

[primitives] Add SharedPointerSystem

systems/primitives/BUILD.bazel

@@ -34,6 +34,7 @@ drake_cc_package_library(
         ":port_switch",
         ":random_source",
         ":saturation",
+        ":shared_pointer_system",
         ":sine",
         ":symbolic_vector_system",
         ":trajectory_affine_system",
@@ -253,6 +254,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "shared_pointer_system",
+    srcs = ["shared_pointer_system.cc"],
+    hdrs = ["shared_pointer_system.h"],
+    deps = [
+        "//systems/framework:diagram_builder",
+        "//systems/framework:leaf_system",
+    ],
+)
+
 drake_cc_library(
     name = "symbolic_vector_system",
     srcs = ["symbolic_vector_system.cc"],
@@ -631,6 +642,13 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "shared_pointer_system_test",
+    deps = [
+        ":shared_pointer_system",
+    ],
+)
+
 drake_cc_googletest(
     name = "symbolic_vector_system_test",
     deps = [

systems/primitives/shared_pointer_system.cc

@@ -0,0 +1,25 @@
+#include "drake/systems/primitives/shared_pointer_system.h"
+
+namespace drake {
+namespace systems {
+
+template <typename T>
+SharedPointerSystem<T>::SharedPointerSystem(
+    std::shared_ptr<void> held, std::type_index held_type)
+    : LeafSystem<T>(SystemTypeTag<SharedPointerSystem>{}),
+      held_(std::move(held)),
+      held_type_(held_type) {}
+
+template <typename T>
+template <typename U>
+SharedPointerSystem<T>::SharedPointerSystem(const SharedPointerSystem<U>& other)
+    : SharedPointerSystem<T>(other.held_, other.held_type_) {}
+
+template <typename T>
+SharedPointerSystem<T>::~SharedPointerSystem() = default;
+
+}  // namespace systems
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::systems::SharedPointerSystem)

systems/primitives/shared_pointer_system.h

@@ -0,0 +1,118 @@
+#pragma once
+
+#include <memory>
+#include <typeindex>
+#include <utility>
+
+#include "drake/common/default_scalars.h"
+#include "drake/common/drake_throw.h"
+#include "drake/systems/framework/diagram_builder.h"
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace systems {
+
+/** %SharedPointerSystem holds a single `shared_ptr` that will be released at
+System deletion time (i.e., the end of a Diagram lifespan). It has no input,
+output, state, nor parameters. This is useful for storing objects that will be
+pointed-to by other systems outside of the usual input/output port connections.
+
+Scalar conversion is supported and will simply increment the reference count
+for the contained object. The contained object will not be scalar-converted,
+so should not depend on `T`.
+
+@tparam_default_scalar */
+template <typename T>
+class SharedPointerSystem final : public LeafSystem<T> {
+ public:
+  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SharedPointerSystem)
+
+  /** Creates a system holding the given value.
+  The value is allowed to be `nullptr`.
+  @note To immediately give up ownership at the call site,
+    remember to use `std::move` on the `value_to_hold`.
+  @tparam Held The type used to store the given value.
+    Calls to get<>() must provide the same type for retrieval. */
+  template <typename Held>
+  explicit SharedPointerSystem(std::shared_ptr<Held> value_to_hold)
+      : SharedPointerSystem(std::move(value_to_hold),
+                            std::type_index(typeid(Held))) {}
+
+  /** Creates a system holding the given value.
+  This overload accepts a unique_ptr (but still stores it at a shared_ptr).
+  The value is allowed to be `nullptr`.
+  @tparam Held The type used to store the given value.
+    Calls to get<>() must provide the same type for retrieval.
+  @exclude_from_pydrake_mkdoc{A unique_ptr input is unworkable in Python} */
+  template <typename Held>
+  explicit SharedPointerSystem(std::unique_ptr<Held> value_to_hold)
+      : SharedPointerSystem(std::shared_ptr<Held>(std::move(value_to_hold))) {}
+
+  /** Scalar-converting copy constructor. See @ref system_scalar_conversion. */
+  template <typename U>
+  explicit SharedPointerSystem(const SharedPointerSystem<U>&);
+
+  ~SharedPointerSystem() final;
+
+  /** Creates a system holding the given value and adds it to the builder.
+  The value is allowed to be `nullptr`. Returns an alias to the value (or
+  `nullptr` in case `nullptr` was passed in as the `value_to_hold`).
+  @note To immediately give up ownership at the call site,
+    remember to use `std::move` on the `value_to_hold`.
+  @tparam Held The type used to store the given value.
+    Calls to get<>() must provide the same type for retrieval.
+  @pre builder is non-null */
+  template <typename Held>
+  static Held* AddToBuilder(
+      DiagramBuilder<T>* builder,
+      std::shared_ptr<Held> value_to_hold) {
+    DRAKE_THROW_UNLESS(builder != nullptr);
+    auto holder = std::make_unique<SharedPointerSystem<T>>(
+        std::move(value_to_hold));
+    auto* result = holder->template get<Held>();
+    builder->AddSystem(std::move(holder));
+    return result;
+  }
+
+  /** Creates a system holding the given value and adds it to the builder.
+  This overload accepts a unique_ptr (but still stores it at a shared_ptr).
+  The value is allowed to be `nullptr`. Returns an alias to the value (or
+  `nullptr` in case `nullptr` was passed in as the `value_to_hold`)
+  @tparam Held The type used to store the given value.
+    Calls to get<>() must provide the same type for retrieval.
+  @pre builder is non-null
+  @exclude_from_pydrake_mkdoc{A unique_ptr input is unworkable in Python} */
+  template <typename Held>
+  static Held* AddToBuilder(
+      DiagramBuilder<T>* builder,
+      std::unique_ptr<Held> value_to_hold) {
+    return SharedPointerSystem::AddToBuilder(
+        builder, std::shared_ptr<Held>(std::move(value_to_hold)));
+  }
+
+  /** (Advanced) Retrieves an alias to the stored value.
+  Returns `nullptr` in case `nullptr` was passed in as the `value_to_hold`.
+  @tparam Held The type used to store the given value, per our constructor.
+  @throws std::bad_cast if Held doesn't match the type used at construction. */
+  template <typename Held>
+  Held* get() const {
+    if (std::type_index(typeid(Held)) != held_type_) {
+      throw std::bad_cast();
+    }
+    return static_cast<Held*>(held_.get());
+  }
+
+ private:
+  template <typename> friend class SharedPointerSystem;
+
+  SharedPointerSystem(std::shared_ptr<void> held, std::type_index held_type);
+
+  const std::shared_ptr<void> held_;
+  const std::type_index held_type_;
+};
+
+}  // namespace systems
+}  // namespace drake
+
+DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::systems::SharedPointerSystem)

systems/primitives/test/shared_pointer_system_test.cc

@@ -0,0 +1,77 @@
+#include "drake/systems/primitives/shared_pointer_system.h"
+
+#include <string>
+#include <string_view>
+
+#include <gtest/gtest.h>
+
+namespace drake {
+namespace systems {
+namespace {
+
+GTEST_TEST(SharedPointerSystemTest, CtorFromUnique) {
+  auto held = std::make_unique<std::string>("held");
+  auto dut = std::make_unique<SharedPointerSystem<double>>(std::move(held));
+  std::string* gotten = dut->get<std::string>();
+  ASSERT_NE(gotten, nullptr);
+  EXPECT_EQ(*gotten, "held");
+}
+
+GTEST_TEST(SharedPointerSystemTest, CtorFromShared) {
+  auto held = std::make_shared<std::string>("held");
+  auto dut = std::make_unique<SharedPointerSystem<double>>(std::move(held));
+  std::string* gotten = dut->get<std::string>();
+  ASSERT_NE(gotten, nullptr);
+  EXPECT_EQ(*gotten, "held");
+}
+
+GTEST_TEST(SharedPointerSystemTest, BuilderFromUnique) {
+  auto held = std::make_unique<std::string>("held");
+  auto builder = std::make_unique<DiagramBuilder<double>>();
+  std::string* gotten = SharedPointerSystem<double>::AddToBuilder(
+      builder.get(), std::move(held));
+  auto diagram = builder->Build();
+  ASSERT_NE(gotten, nullptr);
+  EXPECT_EQ(*gotten, "held");
+}
+
+GTEST_TEST(SharedPointerSystemTest, BuilderFromShared) {
+  auto held = std::make_shared<std::string>("held");
+  auto builder = std::make_unique<DiagramBuilder<double>>();
+  std::string* gotten = SharedPointerSystem<double>::AddToBuilder(
+      builder.get(), std::move(held));
+  auto diagram = builder->Build();
+  ASSERT_NE(gotten, nullptr);
+  EXPECT_EQ(*gotten, "held");
+}
+
+GTEST_TEST(SharedPointerSystemTest, ScalarConversion) {
+  auto dut1 = std::make_unique<SharedPointerSystem<double>>(
+      std::make_unique<std::string>("held"));
+  auto dut2 = System<double>::ToSymbolic(*dut1);
+  ASSERT_NE(dut2, nullptr);
+  dut1.release();
+  std::string* gotten = dut2->get<std::string>();
+  ASSERT_NE(gotten, nullptr);
+  EXPECT_EQ(*gotten, "held");
+}
+
+// Ensure that type-checking guards operate correctly.
+GTEST_TEST(SharedPointerSystemTest, BadCast) {
+  auto held = std::make_shared<std::string>("held");
+  auto dut = std::make_unique<SharedPointerSystem<double>>(std::move(held));
+  EXPECT_THROW(dut->get<std::string_view>(), std::bad_cast);
+}
+
+// Ensure that nulls don't crash anything.
+GTEST_TEST(SharedPointerSystemTest, Null) {
+  auto builder = std::make_unique<DiagramBuilder<double>>();
+  std::string* nothing = SharedPointerSystem<double>::AddToBuilder(
+      builder.get(), std::shared_ptr<std::string>{});
+  EXPECT_EQ(nothing, nullptr);
+  auto diagram = builder->Build();
+}
+
+}  // namespace
+}  // namespace systems
+}  // namespace drake