From 1567abeaf54614613f30bb0e6a8eb7e2585c5dac Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Thu, 16 Jan 2025 10:42:07 -0800 Subject: [PATCH] Switch pub/sub serializers away from unique_ptr --- drake_ros/core/ros_publisher_system.cc | 4 ++-- drake_ros/core/ros_publisher_system.h | 2 +- drake_ros/core/ros_subscriber_system.cc | 4 ++-- drake_ros/core/ros_subscriber_system.h | 4 ++-- drake_ros/drake_ros/core/cc_pybind.cc | 21 +++++++++++++++------ 5 files changed, 22 insertions(+), 13 deletions(-) diff --git a/drake_ros/core/ros_publisher_system.cc b/drake_ros/core/ros_publisher_system.cc index 4905481e..d6562fbb 100644 --- a/drake_ros/core/ros_publisher_system.cc +++ b/drake_ros/core/ros_publisher_system.cc @@ -13,13 +13,13 @@ namespace drake_ros { namespace core { struct RosPublisherSystem::Impl { // Interface for message (de)serialization. - std::unique_ptr serializer; + std::shared_ptr serializer; // Publisher for serialized messages. std::unique_ptr pub; }; RosPublisherSystem::RosPublisherSystem( - std::unique_ptr serializer, + std::shared_ptr serializer, const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros, const std::unordered_set& publish_triggers, double publish_period) diff --git a/drake_ros/core/ros_publisher_system.h b/drake_ros/core/ros_publisher_system.h index 2209b932..cca25f9e 100644 --- a/drake_ros/core/ros_publisher_system.h +++ b/drake_ros/core/ros_publisher_system.h @@ -57,7 +57,7 @@ class RosPublisherSystem : public drake::systems::LeafSystem { @param[in] publish_period optional publishing period, in seconds. Only applicable when periodic publishing is enabled. */ - RosPublisherSystem(std::unique_ptr serializer, + RosPublisherSystem(std::shared_ptr serializer, const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros, const std::unordered_set& diff --git a/drake_ros/core/ros_subscriber_system.cc b/drake_ros/core/ros_subscriber_system.cc index c2a0573a..db3066c0 100644 --- a/drake_ros/core/ros_subscriber_system.cc +++ b/drake_ros/core/ros_subscriber_system.cc @@ -35,7 +35,7 @@ class MessageQueue { struct RosSubscriberSystem::Impl { // Interface for message (de)serialization. - std::unique_ptr serializer; + std::shared_ptr serializer; // Subscription to serialized messages. std::shared_ptr sub; // Queue of serialized messages. @@ -45,7 +45,7 @@ struct RosSubscriberSystem::Impl { }; RosSubscriberSystem::RosSubscriberSystem( - std::unique_ptr serializer, + std::shared_ptr serializer, const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros) : impl_(new Impl()) { impl_->serializer = std::move(serializer); diff --git a/drake_ros/core/ros_subscriber_system.h b/drake_ros/core/ros_subscriber_system.h index 61485e9e..ffd39441 100644 --- a/drake_ros/core/ros_subscriber_system.h +++ b/drake_ros/core/ros_subscriber_system.h @@ -29,7 +29,7 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { static std::unique_ptr Make(ArgsT&&... args) { // Assume C++ typesupport since this is a C++ template function return std::make_unique( - std::make_unique>(), std::forward(args)...); + std::make_shared>(), std::forward(args)...); } /** A constructor for the ROS subscriber system. @@ -41,7 +41,7 @@ class RosSubscriberSystem : public drake::systems::LeafSystem { @param[in] qos QoS profile for the underlying ROS subscription. @param[in] ros interface to a live ROS node to publish from. */ - RosSubscriberSystem(std::unique_ptr serializer, + RosSubscriberSystem(std::shared_ptr serializer, const std::string& topic_name, const rclcpp::QoS& qos, DrakeRos* ros); diff --git a/drake_ros/drake_ros/core/cc_pybind.cc b/drake_ros/drake_ros/core/cc_pybind.cc index c56f9914..ba471ac9 100644 --- a/drake_ros/drake_ros/core/cc_pybind.cc +++ b/drake_ros/drake_ros/core/cc_pybind.cc @@ -36,9 +36,9 @@ using py_rvp = pybind11::return_value_policy; // A (de)serialization interface implementation for Python ROS messages // that can be overriden from Python itself. -class PySerializerInterface : public py::wrapper { +class PySerializerInterface : public SerializerInterface { public: - using Base = py::wrapper; + using Base = SerializerInterface; PySerializerInterface() : Base() {} @@ -50,8 +50,17 @@ class PySerializerInterface : public py::wrapper { } std::unique_ptr CreateDefaultValue() const override { - PYBIND11_OVERLOAD_PURE(std::unique_ptr, - SerializerInterface, CreateDefaultValue); + // Our required unique_ptr return type cannot be directly fulfilled by a + // Python override, so we only ask the Python override for a py::object and + // then just Clone it to obtain the necessary C++ signature. Because the + // PYBIND11_OVERLOAD_PURE macro embeds a `return ...;` statement, we must + // wrap it in lambda so that we can post-process the return value. + py::object default_value = [this]() -> py::object { + PYBIND11_OVERLOAD_PURE( + py::object, SerializerInterface, CreateDefaultValue); + }(); + DRAKE_THROW_UNLESS(!default_value.is_none()); + return default_value.template cast()->Clone(); } rclcpp::SerializedMessage Serialize( @@ -247,7 +256,7 @@ void DefCore(py::module m) { }); py::class_>(m, "RosPublisherSystem") - .def(py::init([](std::unique_ptr serializer, + .def(py::init([](std::shared_ptr serializer, const char* topic_name, QoS qos, DrakeRos* ros_interface, std::unordered_set publish_triggers, double publish_period) { @@ -257,7 +266,7 @@ void DefCore(py::module m) { })); py::class_>(m, "RosSubscriberSystem") - .def(py::init([](std::unique_ptr serializer, + .def(py::init([](std::shared_ptr serializer, const char* topic_name, QoS qos, DrakeRos* ros_interface) { return std::make_unique(