From f13bf96d555a7ec69a54d2568845e96ac4498794 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Mon, 12 Jul 2021 19:40:45 -0700 Subject: [PATCH] [pydrake] Add bindings for LcmScopeSystem (#15309) --- bindings/pydrake/systems/lcm_py.cc | 28 +++++++++++++++++++++++ bindings/pydrake/systems/test/lcm_test.py | 12 ++++++++++ 2 files changed, 40 insertions(+) diff --git a/bindings/pydrake/systems/lcm_py.cc b/bindings/pydrake/systems/lcm_py.cc index 4da4cb0a3556..199277c85514 100644 --- a/bindings/pydrake/systems/lcm_py.cc +++ b/bindings/pydrake/systems/lcm_py.cc @@ -11,6 +11,7 @@ #include "drake/systems/lcm/connect_lcm_scope.h" #include "drake/systems/lcm/lcm_interface_system.h" #include "drake/systems/lcm/lcm_publisher_system.h" +#include "drake/systems/lcm/lcm_scope_system.h" #include "drake/systems/lcm/lcm_subscriber_system.h" #include "drake/systems/lcm/serializer.h" @@ -169,6 +170,33 @@ PYBIND11_MODULE(lcm, m) { py::arg("timeout") = -1, cls_doc.WaitForMessage.doc); } + { + using Class = LcmScopeSystem; + constexpr auto& cls_doc = doc.LcmScopeSystem; + py::class_>(m, "LcmScopeSystem") + .def(py::init(), py::arg("size"), cls_doc.ctor.doc) + .def_static( + "AddToBuilder", + [](systems::DiagramBuilder* builder, + drake::lcm::DrakeLcmInterface* lcm, + const OutputPort& signal, const std::string& channel, + double publish_period) { + auto [scope, publisher] = LcmScopeSystem::AddToBuilder( + builder, lcm, signal, channel, publish_period); + // Annotate the proper rvp on the tuple of pointers. + py::object py_builder = py::cast(builder, py_rvp::reference); + py::list result; + result.append( + py::cast(scope, py_rvp::reference_internal, py_builder)); + result.append( + py::cast(publisher, py_rvp::reference_internal, py_builder)); + return result; + }, + py::arg("builder"), py::arg("lcm"), py::arg("signal"), + py::arg("channel"), py::arg("publish_period"), + cls_doc.AddToBuilder.doc); + } + m.def("ConnectLcmScope", &ConnectLcmScope, py::arg("src"), py::arg("channel"), py::arg("builder"), py::arg("lcm") = nullptr, py::arg("publish_period") = 0.0, py::keep_alive<0, 2>(), diff --git a/bindings/pydrake/systems/test/lcm_test.py b/bindings/pydrake/systems/test/lcm_test.py index 76b73e8dcd38..0ada060fe3d2 100644 --- a/bindings/pydrake/systems/test/lcm_test.py +++ b/bindings/pydrake/systems/test/lcm_test.py @@ -192,6 +192,18 @@ def test_diagram_publisher(self): diagram = builder.Build() diagram.Publish(diagram.CreateDefaultContext()) + def test_lcm_scope(self): + builder = DiagramBuilder() + source = builder.AddSystem(ConstantVectorSource(np.zeros(4))) + scope, publisher = mut.LcmScopeSystem.AddToBuilder( + builder=builder, + lcm=DrakeLcm(), + signal=source.get_output_port(0), + channel="TEST_CHANNEL", + publish_period=0.001) + self.assertIsInstance(scope, mut.LcmScopeSystem) + self.assertIsInstance(publisher, mut.LcmPublisherSystem) + def test_connect_lcm_scope(self): builder = DiagramBuilder() source = builder.AddSystem(ConstantVectorSource(np.zeros(4)))