[py] Make it easier to pass C++ node pointers in Python

bazel_ros2_rules/README.md

@@ -40,7 +40,7 @@ add_bazel_ros2_rules_dependencies()
 load("@bazel_ros2_rules//ros2:defs.bzl", "ros2_local_repository")
 ros2_local_repository(
     name = "ros2",
-    workspace = ["/opt/ros/<distro>"],
+    workspaces = ["/opt/ros/<distro>"],
 )
 ```
 

drake_ros/core/drake_ros.cc

@@ -11,7 +11,7 @@ namespace drake_ros {
 namespace core {
 struct DrakeRos::Impl {
   rclcpp::Context::SharedPtr context;
-  rclcpp::Node::UniquePtr node;
+  rclcpp::Node::SharedPtr node;
   rclcpp::executors::SingleThreadedExecutor::UniquePtr executor;
 };
 

drake_ros/drake_ros/BUILD.bazel

@@ -19,6 +19,7 @@ pybind_py_library(
     cc_deps = [
         ":python_bindings_internal_hdrs",
         "//:drake_ros_shared_library",
+        "@drake//bindings/pydrake:pydrake_pybind",
     ],
     cc_so_name = "_cc",
     cc_srcs = [
@@ -48,14 +49,34 @@ py_library(
     ],
 )
 
+pybind_py_library(
+    name = "test_pub_and_sub_cc_py",
+    testonly = 1,
+    cc_deps = [
+        ":python_bindings_internal_hdrs",
+        "@ros2//:rclcpp_cc",
+        "@ros2//:std_msgs_cc",
+    ],
+    # See comment in neighboring CMakeLists.txt as to why we name organize this
+    # Python module in this fashion.
+    cc_so_name = "test/drake_ros_test_pub_and_sub_cc",
+    cc_srcs = ["test/test_pub_and_sub_cc_py.cc"],
+    py_deps = [
+        ":drake_ros_py",
+    ],
+    py_imports = ["test"],
+)
+
 ros_py_test(
     name = "core_test",
     srcs = ["test/core_test.py"],
     main = "test/core_test.py",
     deps = [
         ":drake_ros_py",
+        ":test_pub_and_sub_cc_py",
         "@drake//bindings/pydrake",
         "@ros2//:rclpy_py",
+        "@ros2//:std_msgs_py",
         "@ros2//:test_msgs_py",
         "@ros2//resources/rmw_isolation:rmw_isolation_py",
     ],

drake_ros/drake_ros/CMakeLists.txt

@@ -28,6 +28,23 @@ if(BUILD_TESTING)
   find_package(Python3 COMPONENTS Interpreter)
   find_package(ament_cmake_test REQUIRED)
 
+  # TODO(eric.cousineau): I am not sure how to place a test-only Python library
+  # within a non-test ament package. To that end, we make this an independent
+  # Python package.
+  pybind11_add_module(py_test_pub_and_sub_cc SHARED
+    "test/test_pub_and_sub_cc_py.cc"
+  )
+  target_link_libraries(py_test_pub_and_sub_cc PRIVATE
+      drake_ros_core)
+  set_target_properties(py_test_pub_and_sub_cc
+    PROPERTIES
+      OUTPUT_NAME "drake_ros_test_pub_and_sub_cc"
+      LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/py/")
+  target_include_directories(py_test_pub_and_sub_cc
+    PRIVATE
+      "$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}>"
+  )
+
   macro(add_python_test test_name test_file)
     # Invoke Pytest in a way that prevents source space from
     # Getting added to `sys.path`, so that CPython extensions
@@ -41,7 +58,9 @@ if(BUILD_TESTING)
         "--junit-prefix=drake_ros"
       # Let Python import mock-install structure in build folder
       WORKING_DIRECTORY
-        "${PROJECT_BINARY_DIR}/py/")
+        "${PROJECT_BINARY_DIR}/py/"
+      APPEND_ENV
+        "PYTHONPATH=${PROJECT_BINARY_DIR}/py/")
   endmacro()
 
   add_python_test(core_test_py test/core_test.py)

drake_ros/drake_ros/core/__init__.py

@@ -2,6 +2,9 @@
 
 from drake_ros._cc.core import ClockSystem
 from drake_ros._cc.core import init
+from drake_ros._cc.core import CppNode
+from drake_ros._cc.core import CppNodeOptions
+from drake_ros._cc.core import DrakeRos
 from drake_ros._cc.core import Isometry3ToRosPose
 from drake_ros._cc.core import Isometry3ToRosTransform
 from drake_ros._cc.core import QuaternionToRosQuaternion
@@ -44,6 +47,12 @@
 from rclpy.type_support import check_for_type_support
 
 
+def _setattr_kwargs(obj, kwargs):
+    # For `ParamInit` in `drake_ros_pybind.h`.
+    for name, value in kwargs.items():
+        setattr(obj, name, value)
+
+
 class PySerializer(SerializerInterface):
     """
     A (de)serialization interface for Python ROS messages.

drake_ros/drake_ros/core/cc_pybind.cc

@@ -91,10 +91,78 @@ void DefCore(py::module m) {
   py::module::import("pydrake.math");
   py::module::import("pydrake.common.eigen_geometry");
 
+  {
+    using Class = rclcpp::NodeOptions;
+    py::class_<Class>(m, "CppNodeOptions", py::module_local())
+        .def(ParamInit<Class>())
+        .def_property(
+            "arguments", [](const Class& self) { return self.arguments(); },
+            [](Class* self, const std::vector<std::string>& value) {
+              self->arguments(value);
+            })
+        .def_property(
+            "use_global_arguments",
+            [](const Class& self) { return self.use_global_arguments(); },
+            [](Class* self, bool value) { self->use_global_arguments(value); })
+        .def_property(
+            "enable_rosout",
+            [](const Class& self) { return self.enable_rosout(); },
+            [](Class* self, bool value) { self->enable_rosout(value); })
+        .def_property(
+            "use_intra_process_comms",
+            [](const Class& self) { return self.use_intra_process_comms(); },
+            [](Class* self, bool value) {
+              self->use_intra_process_comms(value);
+            })
+        .def_property(
+            "start_parameter_event_publisher",
+            [](const Class& self) {
+              return self.start_parameter_event_publisher();
+            },
+            [](Class* self, bool value) {
+              self->start_parameter_event_publisher(value);
+            })
+        .def_property(
+            "use_clock_thread",
+            [](const Class& self) { return self.use_clock_thread(); },
+            [](Class* self, bool value) { self->use_clock_thread(value); })
+        .def_property(
+            "allow_undeclared_parameters",
+            [](const Class& self) {
+              return self.allow_undeclared_parameters();
+            },
+            [](Class* self, bool value) {
+              self->allow_undeclared_parameters(value);
+            })
+        .def_property(
+            "automatically_declare_parameters_from_overrides",
+            [](const Class& self) {
+              return self.automatically_declare_parameters_from_overrides();
+            },
+            [](Class* self, bool value) {
+              self->automatically_declare_parameters_from_overrides(value);
+            });
+  }
+
+  // N.B. We purposefully do not make this `py::module_local()`, as it seems to
+  // cause a segfault for downstream bindings, at least for the
+  // RobotLocomotion/pybind11 fork.
+  py::class_<rclcpp::Node, std::shared_ptr<rclcpp::Node>>(m, "CppNode")
+      .def(py::init<const std::string&, rclcpp::NodeOptions>(),
+           py::arg("node_name"),
+           py::arg("node_options") = rclcpp::NodeOptions{})
+      .def("get_name", &rclcpp::Node::get_name);
+
   // TODD(hidmic): populate Python docstrings with
   // C++ docstrings. Consider using mkdoc to keep
   // them in sync, like pydrake does.
-  py::class_<DrakeRos>(m, "DrakeRos");
+  py::class_<DrakeRos>(m, "DrakeRos")
+      .def(py::init<const std::string&, rclcpp::NodeOptions>(),
+           py::arg("node_name"),
+           py::arg("node_options") = rclcpp::NodeOptions{})
+      .def("get_node", [](const DrakeRos& self) {
+        return self.get_node().shared_from_this();
+      });
 
   py::class_<ClockSystem, LeafSystem<double>>(m, "ClockSystem")
       .def_static(

drake_ros/drake_ros/drake_ros_pybind.h

@@ -17,6 +17,20 @@ namespace drake_ros_py DRAKE_ROS_NO_EXPORT {
 
 namespace py = pybind11;
 
+/**
+Imported from pydrake_pybind.h
+*/
+template <typename Class>
+auto ParamInit() {
+  return py::init([](py::kwargs kwargs) {
+    Class obj{};
+    py::object py_obj = py::cast(&obj, py::return_value_policy::reference);
+    py::module m = py::module::import("drake_ros.core");
+    m.attr("_setattr_kwargs")(py_obj, kwargs);
+    return obj;
+  });
+}
+
 // clang-format off
 }  // namespace drake_ros_py
 // clang-format on

drake_ros/drake_ros/test/core_test.py

@@ -1,5 +1,6 @@
 import os
 import sys
+import warnings
 
 from pydrake.systems.analysis import Simulator
 from pydrake.systems.framework import DiagramBuilder
@@ -13,12 +14,17 @@
 from rclpy.qos import QoSProfile
 from rclpy.qos import ReliabilityPolicy
 
+from std_msgs.msg import Int32
 from test_msgs.msg import BasicTypes
 
 import drake_ros.core
+from drake_ros.core import CppNode
+from drake_ros.core import CppNodeOptions
+from drake_ros.core import DrakeRos
 from drake_ros.core import RosInterfaceSystem
 from drake_ros.core import RosPublisherSystem
 from drake_ros.core import RosSubscriberSystem
+from drake_ros_test_pub_and_sub_cc import CppPubAndSub
 
 
 def isolate_if_using_bazel():
@@ -28,10 +34,18 @@ def isolate_if_using_bazel():
         isolate_rmw_by_path(os.environ['TEST_TMPDIR'])
 
 
-def test_nominal_case():
-    isolate_if_using_bazel()
+@pytest.fixture
+def drake_ros_fixture():
     drake_ros.core.init()
+    rclpy.init()
+    try:
+        yield
+    finally:
+        rclpy.shutdown()
+        drake_ros.core.shutdown()
+
 
+def test_nominal_case(drake_ros_fixture):
     builder = DiagramBuilder()
 
     system_ros = builder.AddSystem(
@@ -61,7 +75,6 @@ def test_nominal_case():
 
     simulator_context = simulator.get_mutable_context()
 
-    rclpy.init()
     direct_ros_node = rclpy.create_node('sub_to_pub_py')
 
     # Create publisher talking to subscriber system.
@@ -105,8 +118,66 @@ def rx_callback_direct_sub_out(msg):
         assert len(rx_msgs_direct_sub_out) == rx_msgs_count_after_pubsub
         assert rx_msgs_direct_sub_out[-1].uint64_value == i
 
-    drake_ros.core.shutdown()
+
+def assert_equal_for_cyclone_dds(actual, expected, name):
+    rmw_implementation = rclpy.utilities.get_rmw_implementation_identifier()
+    is_cyclone_dds = rmw_implementation == "rmw_cyclonedds_cpp"
+    if is_cyclone_dds:
+        assert actual == expected
+    else:
+        # TODO(#286): At least for rmw_fastrtps_cpp, the behavior of publishing
+        # and receiving a message seems much less reliable than cyclone. Demote
+        # errors to warnings in this case.
+        if actual != expected:
+            warnings.warn(
+                f"WARNING! For RMW_IMPLEMENTATION={rmw_implementation}, "
+                f"incorrect value for {name}: {actual} != {expected}"
+            )
+
+
+def test_cpp_node(drake_ros_fixture):
+    # Create a Python node.
+    node_py = rclpy.create_node("node_py")
+    sub_value_py = -1
+
+    def on_sub(message):
+        nonlocal sub_value_py
+        sub_value_py = message.data
+
+    node_py.create_subscription(Int32, "/cpp_pub", on_sub, 1)
+    pub_py = node_py.create_publisher(Int32, "/cpp_sub", 1)
+
+    # Create a C++ node.
+    cpp_node_options = CppNodeOptions(use_global_arguments=False)
+    assert not cpp_node_options.use_global_arguments
+    node_cpp = CppNode("direct_node_cpp", node_options=cpp_node_options)
+    assert node_cpp.get_name() == "direct_node_cpp"
+    # Create a "fixture" that will publish and subscribe to above topics.
+    fixture = CppPubAndSub(node=node_cpp)
+    timeout_sec = 0.0
+
+    # Show that we can publish from C++ to Python.
+    fixture.Publish(value=10)
+    rclpy.spin_once(node_py, timeout_sec=timeout_sec)
+    assert_equal_for_cyclone_dds(sub_value_py, 10, "sub_value_py")
+
+    # Show that we can publish from Python to C++.
+    message = Int32()
+    message.data = 100
+    pub_py.publish(message)
+    sub_value_cpp = fixture.SpinAndReturnLatest(timeout_sec=timeout_sec)
+    assert_equal_for_cyclone_dds(sub_value_cpp, 100, "sub_value_cpp")
+
+
+def test_drake_ros(drake_ros_fixture):
+    drake_ros = DrakeRos("sample_node")
+    assert isinstance(drake_ros.get_node(), CppNode)
+
+
+def main():
+    isolate_if_using_bazel()
+    sys.exit(pytest.main(sys.argv))
 
 
 if __name__ == '__main__':
-    sys.exit(pytest.main(sys.argv))
+    main()

drake_ros/drake_ros/test/test_pub_and_sub_cc_py.cc

@@ -0,0 +1,56 @@
+#include <pybind11/pybind11.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/int32.hpp>
+
+#include "drake_ros/drake_ros_pybind.h"
+
+namespace drake_ros {
+namespace drake_ros_py {
+namespace {
+
+using std_msgs::msg::Int32;
+
+class CppPubAndSub {
+ public:
+  explicit CppPubAndSub(rclcpp::Node::SharedPtr node) : node_(node) {
+    sub_ = node_->create_subscription<Int32>(
+        "/cpp_sub", 1, [this](const Int32& message) { value_ = message.data; });
+    pub_ = node_->create_publisher<Int32>("/cpp_pub", 1);
+  }
+
+  void Publish(int value) {
+    Int32 message{};
+    message.data = value;
+    pub_->publish(std::move(message));
+  }
+
+  int SpinAndReturnLatest(double timeout_sec) {
+    rclcpp::executors::SingleThreadedExecutor exec;
+    exec.add_node(node_);
+    const std::chrono::nanoseconds timeout(static_cast<int>(timeout_sec * 1e9));
+    exec.spin_some(timeout);
+    return value_;
+  }
+
+ private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Subscription<Int32>::SharedPtr sub_;
+  rclcpp::Publisher<Int32>::SharedPtr pub_;
+  int value_{-1};
+};
+
+PYBIND11_MODULE(drake_ros_test_pub_and_sub_cc, m) {
+  py::module_::import("drake_ros.core");
+
+  py::class_<CppPubAndSub>(m, "CppPubAndSub")
+      .def(py::init<rclcpp::Node::SharedPtr>(), py::arg("node"))
+      .def("Publish", &CppPubAndSub::Publish, py::arg("value"))
+      .def("SpinAndReturnLatest", &CppPubAndSub::SpinAndReturnLatest,
+           py::arg("timeout_sec"));
+}
+
+}  // namespace
+// clang-format off
+}  // namespace drake_ros_py
+// clang-format on
+}  // namespace drake_ros