Add Python bindings for drake_ros_core package.

Signed-off-by: Shane Loretz <[email protected]>
Signed-off-by: Michel Hidalgo <[email protected]>

drake_ros_core/CMakeLists.txt

@@ -12,6 +12,8 @@ endif()
 
 find_package(ament_cmake_ros REQUIRED)
 find_package(drake REQUIRED)
+# Must use Drake's fork of Pybind11
+find_package(pybind11 REQUIRED HINTS "${drake_DIR}/../pybind11" NO_DEFAULT_PATH)
 find_package(rclcpp REQUIRED)
 find_package(rosidl_runtime_c REQUIRED)
 find_package(rosidl_typesupport_cpp REQUIRED)
@@ -59,6 +61,30 @@ install(
   DESTINATION include
 )
 
+###
+# Python bindings
+###
+pybind11_add_module(py_drake_ros_core SHARED
+  src/python_bindings/module_drake_ros_core.cpp
+)
+set_target_properties(py_drake_ros_core PROPERTIES OUTPUT_NAME "drake_ros_core")
+target_link_libraries(py_drake_ros_core PRIVATE
+  drake_ros_core
+)
+target_include_directories(py_drake_ros_core
+  PRIVATE
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/python_bindings>"
+)
+
+# Sets PYTHON_INSTALL_DIR
+_ament_cmake_python_get_python_install_dir()
+
+install(
+  TARGETS py_drake_ros_core
+  DESTINATION "${PYTHON_INSTALL_DIR}"
+)
+### End Python bindings
+
 if(BUILD_TESTING)
   find_package(ament_cmake_clang_format REQUIRED)
   find_package(ament_cmake_cpplint REQUIRED)
@@ -77,6 +103,8 @@ if(BUILD_TESTING)
     ${test_msgs_TARGETS}
   )
 
+  ament_add_pytest_test(test_pub_sub_py test/test_pub_sub.py)
+
   ament_add_gtest(test_drake_ros test/test_drake_ros.cpp)
   target_link_libraries(test_drake_ros
     drake_ros_core

drake_ros_core/src/python_bindings/module_drake_ros_core.cpp

@@ -0,0 +1,115 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include <drake/systems/framework/leaf_system.h>
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <memory>
+#include <unordered_set>
+
+#include "drake_ros_core/drake_ros.hpp"
+#include "drake_ros_core/ros_interface_system.hpp"
+#include "drake_ros_core/ros_publisher_system.hpp"
+#include "drake_ros_core/ros_subscriber_system.hpp"
+
+#include "py_serializer.hpp"
+#include "qos_type_caster.hpp"
+
+namespace py = pybind11;
+
+using drake::systems::Diagram;
+using drake::systems::LeafSystem;
+using drake::systems::TriggerType;
+
+using drake_ros_core::DrakeRos;
+using drake_ros_core::DrakeRosInterface;
+using drake_ros_core::PySerializer;
+using drake_ros_core::RosInterfaceSystem;
+using drake_ros_core::RosPublisherSystem;
+using drake_ros_core::RosSubscriberSystem;
+using drake_ros_core::SerializerInterface;
+
+
+PYBIND11_MODULE(drake_ros_core, m) {
+  m.doc() = "Python wrapper for drake_ros_core";
+
+  py::module::import("pydrake.systems.framework");
+  py::module::import("pydrake.multibody.plant");
+
+  // Use std::shared_ptr holder so pybind11 doesn't try to delete interfaces returned from
+  // get_ros_interface
+  py::class_<DrakeRosInterface, std::shared_ptr<DrakeRosInterface>>(m, "DrakeRosInterface");
+
+  py::class_<RosInterfaceSystem, LeafSystem<double>>(m, "RosInterfaceSystem")
+  .def(
+    py::init(
+      []() {return std::make_unique<RosInterfaceSystem>(std::make_unique<DrakeRos>());}))
+  .def("get_ros_interface", &RosInterfaceSystem::get_ros_interface);
+
+  py::class_<RosPublisherSystem, LeafSystem<double>>(m, "RosPublisherSystem")
+  .def(
+    py::init(
+      [](
+        pybind11::object type,
+        const char * topic_name,
+        drake_ros_core::QoS qos,
+        std::shared_ptr<DrakeRosInterface> ros_interface)
+      {
+        std::unique_ptr<SerializerInterface> serializer = std::make_unique<PySerializer>(type);
+        return std::make_unique<RosPublisherSystem>(
+          serializer,
+          topic_name,
+          qos,
+          ros_interface,
+          std::unordered_set<TriggerType>{TriggerType::kPerStep, TriggerType::kForced},
+          0.0);
+      }))
+  .def(
+    py::init(
+      [](
+        pybind11::object type,
+        const char * topic_name,
+        drake_ros_core::QoS qos,
+        std::shared_ptr<DrakeRosInterface> ros_interface,
+        std::unordered_set<TriggerType> publish_triggers,
+        double publish_period)
+      {
+        std::unique_ptr<SerializerInterface> serializer = std::make_unique<PySerializer>(type);
+        return std::make_unique<RosPublisherSystem>(
+          serializer,
+          topic_name,
+          qos,
+          ros_interface,
+          publish_triggers,
+          publish_period);
+      }));
+
+  py::class_<RosSubscriberSystem, LeafSystem<double>>(m, "RosSubscriberSystem")
+  .def(
+    py::init(
+      [](
+        pybind11::object type,
+        const char * topic_name,
+        drake_ros_core::QoS qos,
+        std::shared_ptr<DrakeRosInterface> ros_interface)
+      {
+        std::unique_ptr<SerializerInterface> serializer = std::make_unique<PySerializer>(type);
+        return std::make_unique<RosSubscriberSystem>(
+          serializer,
+          topic_name,
+          qos,
+          ros_interface);
+      }));
+}

drake_ros_core/src/python_bindings/py_serializer.hpp

@@ -0,0 +1,181 @@
+// Copyright 2020 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef PYTHON_BINDINGS__PY_SERIALIZER_HPP_
+#define PYTHON_BINDINGS__PY_SERIALIZER_HPP_
+
+#include <pybind11/eval.h>
+#include <pybind11/pybind11.h>
+#include <rmw/rmw.h>
+
+#include <memory>
+
+#include "drake_ros_core/serializer_interface.hpp"
+
+namespace py = pybind11;
+
+namespace drake_ros_core
+{
+// Serialize/Deserialize Python ROS types to rclcpp::SerializedMessage
+class PySerializer : public SerializerInterface
+{
+public:
+  explicit PySerializer(py::object message_type)
+  : message_type_(message_type)
+  {
+    py::dict scope;
+    py::exec(
+      R"delim(
+def get_typesupport(msg_type, attribute_name):
+    metaclass = msg_type.__class__
+    if metaclass._TYPE_SUPPORT is None:
+        # Import typesupport if not done already
+        metaclass.__import_type_support__()
+    return getattr(metaclass, attribute_name)
+
+
+def make_abstract_value(some_type):
+    from pydrake.common.value import AbstractValue
+    return AbstractValue.Make(some_type())
+      )delim",
+      py::globals(), scope);
+
+    py_make_abstract_value_ = scope["make_abstract_value"];
+    py::object py_get_typesupport = scope["get_typesupport"];
+
+    // Get type support capsule and pointer
+    auto typesupport = py::cast<py::capsule>(py_get_typesupport(message_type, "_TYPE_SUPPORT"));
+    // TODO(sloretz) use get_pointer() in py 2.6+
+    type_support_ = static_cast<decltype(type_support_)>(typesupport);
+
+    auto convert_from_py =
+      py::cast<py::capsule>(py_get_typesupport(message_type, "_CONVERT_FROM_PY"));
+    // TODO(sloretz) use get_pointer() in py 2.6+
+    convert_from_py_ = reinterpret_cast<decltype(convert_from_py_)>(
+      static_cast<void *>(convert_from_py));
+
+    auto convert_to_py =
+      py::cast<py::capsule>(py_get_typesupport(message_type, "_CONVERT_TO_PY"));
+    // TODO(sloretz) use get_pointer() in py 2.6+
+    convert_to_py_ = reinterpret_cast<decltype(convert_to_py_)>(
+      static_cast<void *>(convert_to_py));
+
+    auto create_ros_message =
+      py::cast<py::capsule>(py_get_typesupport(message_type, "_CREATE_ROS_MESSAGE"));
+    // TODO(sloretz) use get_pointer() in py 2.6+
+    create_ros_message_ = reinterpret_cast<decltype(create_ros_message_)>(
+      static_cast<void *>(create_ros_message));
+
+    auto destroy_ros_message =
+      py::cast<py::capsule>(py_get_typesupport(message_type, "_DESTROY_ROS_MESSAGE"));
+    // TODO(sloretz) use get_pointer() in py 2.6+
+    destroy_ros_message_ = reinterpret_cast<decltype(destroy_ros_message_)>(
+      static_cast<void *>(destroy_ros_message));
+  }
+
+  rclcpp::SerializedMessage
+  serialize(const drake::AbstractValue & abstract_value) const override
+  {
+    // convert from inaccessible drake::pydrake::Object type
+    py::dict scope;
+    scope["av"] = &abstract_value;
+    py::object message = py::eval("av.Clone().get_mutable_value()", scope);
+
+    // Create  C ROS message
+    auto c_ros_message = std::unique_ptr<void, decltype(destroy_ros_message_)>(
+      create_ros_message_(), destroy_ros_message_);
+
+    // Convert the Python message to a C ROS message
+    if (!convert_from_py_(message.ptr(), c_ros_message.get())) {
+      throw std::runtime_error("Failed to convert Python message to C ROS message");
+    }
+
+    // Serialize the C message
+    rclcpp::SerializedMessage serialized_msg;
+    const auto ret = rmw_serialize(
+      c_ros_message.get(),
+      type_support_,
+      &serialized_msg.get_rcl_serialized_message());
+    if (ret != RMW_RET_OK) {
+      throw std::runtime_error("Failed to serialize C ROS message");
+    }
+    return serialized_msg;
+  }
+
+  bool
+  deserialize(
+    const rclcpp::SerializedMessage & serialized_message,
+    drake::AbstractValue & abstract_value) const override
+  {
+    // TODO(sloretz) it would be so much more convenient if I didn't have to
+    // care that the Python typesupport used the C type support internally.
+    // Why isn't this encapsulated in the python type support itself?
+
+    // Create a C type support version of the message
+    auto c_ros_message = std::unique_ptr<void, decltype(destroy_ros_message_)>(
+      create_ros_message_(), destroy_ros_message_);
+    if (nullptr == c_ros_message.get()) {
+      return false;
+    }
+
+    // Deserialize to C message type
+    const auto ret = rmw_deserialize(
+      &serialized_message.get_rcl_serialized_message(),
+      type_support_,
+      c_ros_message.get());
+
+    if (RMW_RET_OK != ret) {
+      return false;
+    }
+
+    // Convert C type to Python type
+    PyObject * pymessage = convert_to_py_(c_ros_message.get());
+    if (!pymessage) {
+      return false;
+    }
+
+    // Store the Python message in the AbstractValue
+    // convert to inaccessible drake::pydrake::Object type
+    py::dict scope;
+    scope["av"] = &abstract_value;
+    scope["message"] = pymessage;
+    py::exec("av.set_value(message)", scope);
+
+    return true;
+  }
+
+  std::unique_ptr<drake::AbstractValue>
+  create_default_value() const override
+  {
+    return py::cast<std::unique_ptr<drake::AbstractValue>>(py_make_abstract_value_(message_type_));
+  }
+
+  const rosidl_message_type_support_t *
+  get_type_support() const override
+  {
+    return type_support_;
+  }
+
+private:
+  py::object message_type_;
+  rosidl_message_type_support_t * type_support_;
+
+  py::object py_make_abstract_value_;
+
+  bool (* convert_from_py_)(PyObject *, void *);
+  PyObject * (* convert_to_py_)(void *);
+  void * (* create_ros_message_)(void);
+  void (* destroy_ros_message_)(void *);
+};
+}  // namespace drake_ros_core
+#endif  // PYTHON_BINDINGS__PY_SERIALIZER_HPP_