diff --git a/bazel_ros2_rules/README.md b/bazel_ros2_rules/README.md index f7ffa790..73414f8e 100644 --- a/bazel_ros2_rules/README.md +++ b/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/"], + workspaces = ["/opt/ros/"], ) ``` diff --git a/drake_ros/core/drake_ros.cc b/drake_ros/core/drake_ros.cc index fb36f524..13a2ab45 100644 --- a/drake_ros/core/drake_ros.cc +++ b/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; }; diff --git a/drake_ros/drake_ros/BUILD.bazel b/drake_ros/drake_ros/BUILD.bazel index 7c6d104c..3bd00039 100644 --- a/drake_ros/drake_ros/BUILD.bazel +++ b/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", ], diff --git a/drake_ros/drake_ros/CMakeLists.txt b/drake_ros/drake_ros/CMakeLists.txt index 45763bf3..fba53e36 100644 --- a/drake_ros/drake_ros/CMakeLists.txt +++ b/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 + "$" + ) + 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) diff --git a/drake_ros/drake_ros/core/__init__.py b/drake_ros/drake_ros/core/__init__.py index 69225fc2..2de48bf9 100644 --- a/drake_ros/drake_ros/core/__init__.py +++ b/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. diff --git a/drake_ros/drake_ros/core/cc_pybind.cc b/drake_ros/drake_ros/core/cc_pybind.cc index 38f7ae83..5af540f0 100644 --- a/drake_ros/drake_ros/core/cc_pybind.cc +++ b/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_(m, "CppNodeOptions", py::module_local()) + .def(ParamInit()) + .def_property( + "arguments", [](const Class& self) { return self.arguments(); }, + [](Class* self, const std::vector& 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_>(m, "CppNode") + .def(py::init(), + 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_(m, "DrakeRos"); + py::class_(m, "DrakeRos") + .def(py::init(), + 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_>(m, "ClockSystem") .def_static( diff --git a/drake_ros/drake_ros/drake_ros_pybind.h b/drake_ros/drake_ros/drake_ros_pybind.h index 74a82f5c..96007792 100644 --- a/drake_ros/drake_ros/drake_ros_pybind.h +++ b/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 +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 diff --git a/drake_ros/drake_ros/test/core_test.py b/drake_ros/drake_ros/test/core_test.py index 1e946fa6..8aa8fcaf 100644 --- a/drake_ros/drake_ros/test/core_test.py +++ b/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() diff --git a/drake_ros/drake_ros/test/test_pub_and_sub_cc_py.cc b/drake_ros/drake_ros/test/test_pub_and_sub_cc_py.cc new file mode 100644 index 00000000..76735635 --- /dev/null +++ b/drake_ros/drake_ros/test/test_pub_and_sub_cc_py.cc @@ -0,0 +1,56 @@ +#include +#include +#include + +#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( + "/cpp_sub", 1, [this](const Int32& message) { value_ = message.data; }); + pub_ = node_->create_publisher("/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(timeout_sec * 1e9)); + exec.spin_some(timeout); + return value_; + } + + private: + rclcpp::Node::SharedPtr node_; + rclcpp::Subscription::SharedPtr sub_; + rclcpp::Publisher::SharedPtr pub_; + int value_{-1}; +}; + +PYBIND11_MODULE(drake_ros_test_pub_and_sub_cc, m) { + py::module_::import("drake_ros.core"); + + py::class_(m, "CppPubAndSub") + .def(py::init(), 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 diff --git a/ros2_example_bazel_installed/setup/prereq-rosdep-keys.txt b/ros2_example_bazel_installed/setup/prereq-rosdep-keys.txt index 48ad3dff..50b888d9 100644 --- a/ros2_example_bazel_installed/setup/prereq-rosdep-keys.txt +++ b/ros2_example_bazel_installed/setup/prereq-rosdep-keys.txt @@ -21,6 +21,7 @@ liborocos-kdl-dev libqt5-core libqt5-gui libqt5-opengl +libqt5-svg libqt5-widgets libsqlite3-dev libx11-dev @@ -63,6 +64,7 @@ python3-pytest python3-qt5-bindings python3-rosdistro-modules python3-setuptools +python3-vcstool python3-yaml qtbase5-dev spdlog