From 4bb171072891d05cf17bd4e02c74e9b6a46743f4 Mon Sep 17 00:00:00 2001 From: Rebecca Butler Date: Wed, 16 Jun 2021 13:36:47 -0400 Subject: [PATCH 1/2] Refactor to use generic pub/sub from rclcpp Signed-off-by: Rebecca Butler --- CMakeLists.txt | 2 - doc/design.md | 3 +- src/domain_bridge/domain_bridge.cpp | 41 +++----- src/domain_bridge/generic_publisher.cpp | 55 ---------- src/domain_bridge/generic_publisher.hpp | 46 --------- src/domain_bridge/generic_subscription.cpp | 115 --------------------- src/domain_bridge/generic_subscription.hpp | 101 ------------------ 7 files changed, 16 insertions(+), 347 deletions(-) delete mode 100644 src/domain_bridge/generic_publisher.cpp delete mode 100644 src/domain_bridge/generic_publisher.hpp delete mode 100644 src/domain_bridge/generic_subscription.cpp delete mode 100644 src/domain_bridge/generic_subscription.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ef389..04ee82f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,8 +25,6 @@ find_package(yaml_cpp_vendor REQUIRED) add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/domain_bridge.cpp src/${PROJECT_NAME}/domain_bridge_options.cpp - src/${PROJECT_NAME}/generic_publisher.cpp - src/${PROJECT_NAME}/generic_subscription.cpp src/${PROJECT_NAME}/parse_domain_bridge_yaml_config.cpp src/${PROJECT_NAME}/qos_options.cpp src/${PROJECT_NAME}/topic_bridge_options.cpp diff --git a/doc/design.md b/doc/design.md index e7867eb..77b697f 100644 --- a/doc/design.md +++ b/doc/design.md @@ -108,8 +108,7 @@ For convenience, a standalone binary executable is also provided for easy integr When bridging data, serialized data will be read from one domain and forwarded to another domain. As long as the bridge does not need access to the typed data, there is no need for deserialization and, therefore, C++ type support. In this way, we can handle arbitrary data coming from the middleware (as long as we load type support for the middleware). -This is exactly how [rosbag2](https://github.com/ros2/rosbag2/tree/e4ce24cdfa7e24c6d2c025ecc38ab1157a0eecc8/rosbag2_transport) works; defining generic publishers and subscriptions. -In fact, the generic publisher and subscription implementation is being moved to a common location that the domain bridge can leverage once available (see https://github.com/ros2/rclcpp/pull/1452). +The domain bridge currently leverages the generic publisher and subscription implementations available in rclcpp. ### QoS mapping diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index 483e1f1..d3bee19 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -32,12 +32,12 @@ #include "rclcpp/executor.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/generic_publisher.hpp" +#include "rclcpp/generic_subscription.hpp" #include "rclcpp/rclcpp.hpp" #include "rosbag2_cpp/typesupport_helpers.hpp" #include "rmw/types.h" -#include "generic_publisher.hpp" -#include "generic_subscription.hpp" #include "wait_for_qos_handler.hpp" namespace domain_bridge @@ -50,7 +50,8 @@ class DomainBridgeImpl using NodeMap = std::unordered_map>; using TopicBridgeMap = std::map< TopicBridge, - std::pair, std::shared_ptr>>; + std::pair, + std::shared_ptr>>; using TypesupportMap = std::unordered_map< std::string, std::shared_ptr>; @@ -107,41 +108,33 @@ class DomainBridgeImpl type, "rosidl_typesupport_cpp"); } - std::shared_ptr create_publisher( + std::shared_ptr create_publisher( rclcpp::Node::SharedPtr node, const std::string & topic_name, + const std::string & type, const rclcpp::QoS & qos, - const rosidl_message_type_support_t & typesupport_handle, rclcpp::CallbackGroup::SharedPtr group) { - auto publisher = std::make_shared( - node->get_node_base_interface().get(), - typesupport_handle, - topic_name, - qos); + auto publisher = node->create_generic_publisher(topic_name, type, qos); node->get_node_topics_interface()->add_publisher(publisher, std::move(group)); return publisher; } - std::shared_ptr create_subscription( + std::shared_ptr create_subscription( rclcpp::Node::SharedPtr node, - std::shared_ptr publisher, + std::shared_ptr publisher, const std::string & topic_name, + const std::string & type, const rclcpp::QoS & qos, - const rosidl_message_type_support_t & typesupport_handle, rclcpp::CallbackGroup::SharedPtr group) { - // Create subscription - auto subscription = std::make_shared( - node->get_node_base_interface().get(), - typesupport_handle, + auto subscription = node->create_generic_subscription( topic_name, + type, qos, [publisher](std::shared_ptr msg) { // Publish message into the other domain - auto serialized_data_ptr = std::make_shared( - msg->get_rcl_serialized_message()); - publisher->publish(serialized_data_ptr); + publisher->publish(*msg); }); node->get_node_topics_interface()->add_subscription(subscription, std::move(group)); return subscription; @@ -239,17 +232,13 @@ class DomainBridgeImpl std::cerr << warning << std::endl; } - // Get typesupport handle - auto typesupport_handle = rosbag2_cpp::get_typesupport_handle( - type, "rosidl_typesupport_cpp", loaded_typesupports_.at(type)); - // Create publisher for the 'to_domain' // The publisher should be created first so it is available to the subscription callback auto publisher = this->create_publisher( to_domain_node, topic_remapped, + type, qos, - *typesupport_handle, topic_options.callback_group()); // Create subscription for the 'from_domain' @@ -257,8 +246,8 @@ class DomainBridgeImpl from_domain_node, publisher, topic, + type, qos, - *typesupport_handle, topic_options.callback_group()); this->bridged_topics_[topic_bridge] = {publisher, subscription}; diff --git a/src/domain_bridge/generic_publisher.cpp b/src/domain_bridge/generic_publisher.cpp deleted file mode 100644 index d6830cf..0000000 --- a/src/domain_bridge/generic_publisher.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2018, Bosch Software Innovations GmbH. -// -// 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. - -// NOTE: This file was directly copied from rosbag2_transport -// TODO(jacobperron): Replace with upstream implementation when available -// https://github.com/ros2/rclcpp/pull/1452 - -#include "generic_publisher.hpp" - -#include -#include - -namespace -{ -rcl_publisher_options_t rosbag2_get_publisher_options(const rclcpp::QoS & qos) -{ - auto options = rcl_publisher_get_default_options(); - options.qos = qos.get_rmw_qos_profile(); - return options; -} -} // unnamed namespace - -namespace domain_bridge -{ - -GenericPublisher::GenericPublisher( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const rosidl_message_type_support_t & type_support, - const std::string & topic_name, - const rclcpp::QoS & qos) -: rclcpp::PublisherBase(node_base, topic_name, type_support, rosbag2_get_publisher_options(qos)) -{} - -void GenericPublisher::publish(std::shared_ptr message) -{ - auto return_code = rcl_publish_serialized_message( - get_publisher_handle().get(), message.get(), NULL); - - if (return_code != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(return_code, "failed to publish serialized message"); - } -} - -} // namespace domain_bridge diff --git a/src/domain_bridge/generic_publisher.hpp b/src/domain_bridge/generic_publisher.hpp deleted file mode 100644 index 6f3d6ec..0000000 --- a/src/domain_bridge/generic_publisher.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2018, Bosch Software Innovations GmbH. -// -// 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. - -// NOTE: This file was directly copied from rosbag2_transport -// TODO(jacobperron): Replace with upstream implementation when available -// https://github.com/ros2/rclcpp/pull/1452 - -#ifndef DOMAIN_BRIDGE__GENERIC_PUBLISHER_HPP_ -#define DOMAIN_BRIDGE__GENERIC_PUBLISHER_HPP_ - -#include -#include - -#include "rclcpp/rclcpp.hpp" - -namespace domain_bridge -{ - -class GenericPublisher : public rclcpp::PublisherBase -{ -public: - GenericPublisher( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const rosidl_message_type_support_t & type_support, - const std::string & topic_name, - const rclcpp::QoS & qos); - - virtual ~GenericPublisher() = default; - - void publish(std::shared_ptr message); -}; - -} // namespace domain_bridge - -#endif // DOMAIN_BRIDGE__GENERIC_PUBLISHER_HPP_ diff --git a/src/domain_bridge/generic_subscription.cpp b/src/domain_bridge/generic_subscription.cpp deleted file mode 100644 index 55caaaf..0000000 --- a/src/domain_bridge/generic_subscription.cpp +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright 2018, Bosch Software Innovations GmbH. -// -// 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. -// -// NOTE: This file was directly copied from rosbag2_transport -// TODO(jacobperron): Replace with upstream implementation when available -// https://github.com/ros2/rclcpp/pull/1452 - -#include "generic_subscription.hpp" - -#include -#include - -#include "rclcpp/any_subscription_callback.hpp" -#include "rclcpp/subscription.hpp" - -namespace -{ -rcl_subscription_options_t rosbag2_get_subscription_options(const rclcpp::QoS & qos) -{ - auto options = rcl_subscription_get_default_options(); - options.qos = qos.get_rmw_qos_profile(); - return options; -} -} // unnamed namespace - -namespace domain_bridge -{ - -GenericSubscription::GenericSubscription( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const rosidl_message_type_support_t & ts, - const std::string & topic_name, - const rclcpp::QoS & qos, - std::function)> callback) -: SubscriptionBase( - node_base, - ts, - topic_name, - rosbag2_get_subscription_options(qos), - true), - default_allocator_(rcutils_get_default_allocator()), - callback_(callback), - qos_(qos) -{} - -std::shared_ptr GenericSubscription::create_message() -{ - return create_serialized_message(); -} - -std::shared_ptr GenericSubscription::create_serialized_message() -{ - return borrow_serialized_message(0); -} - -void GenericSubscription::handle_message( - std::shared_ptr & message, const rclcpp::MessageInfo & message_info) -{ - (void) message; - (void) message_info; - throw std::runtime_error{"unexpected callback being called"}; -} - -void GenericSubscription::handle_loaned_message( - void * message, const rclcpp::MessageInfo & message_info) -{ - (void) message; - (void) message_info; - throw std::runtime_error{"unexpected callback being called"}; -} - -void GenericSubscription::handle_serialized_message( - const std::shared_ptr & serialized_message, - const rclcpp::MessageInfo & message_info) -{ - (void)message_info; - auto typed_message = std::static_pointer_cast(serialized_message); - callback_(typed_message); -} - -void GenericSubscription::return_message(std::shared_ptr & message) -{ - auto typed_message = std::static_pointer_cast(message); - return_serialized_message(typed_message); -} - -void GenericSubscription::return_serialized_message( - std::shared_ptr & message) -{ - message.reset(); -} - -const rclcpp::QoS & GenericSubscription::qos_profile() const -{ - return qos_; -} - -std::shared_ptr -GenericSubscription::borrow_serialized_message(size_t capacity) -{ - return std::make_shared(capacity); -} - -} // namespace domain_bridge diff --git a/src/domain_bridge/generic_subscription.hpp b/src/domain_bridge/generic_subscription.hpp deleted file mode 100644 index a293777..0000000 --- a/src/domain_bridge/generic_subscription.hpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2018, Bosch Software Innovations GmbH. -// -// 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. - -// NOTE: This file was directly copied from rosbag2_transport -// TODO(jacobperron): Replace with upstream implementation when available -// https://github.com/ros2/rclcpp/pull/1452 - -#ifndef DOMAIN_BRIDGE__GENERIC_SUBSCRIPTION_HPP_ -#define DOMAIN_BRIDGE__GENERIC_SUBSCRIPTION_HPP_ - -#include -#include - -#include "rclcpp/any_subscription_callback.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/subscription.hpp" - -namespace domain_bridge -{ - -/** - * This class is an implementation of an rclcpp::Subscription for serialized messages whose topic - * is not known at compile time (hence templating does not work). - * - * It does not support intra-process handling - */ -class GenericSubscription : public rclcpp::SubscriptionBase -{ -public: - // cppcheck-suppress unknownMacro - RCLCPP_SMART_PTR_DEFINITIONS(GenericSubscription) - - /** - * Constructor. In order to properly subscribe to a topic, this subscription needs to be added to - * the node_topic_interface of the node passed into this constructor. - * - * \param node_base NodeBaseInterface pointer used in parts of the setup. - * \param ts Type support handle - * \param topic_name Topic name - * \param callback Callback for new messages of serialized form - */ - GenericSubscription( - rclcpp::node_interfaces::NodeBaseInterface * node_base, - const rosidl_message_type_support_t & ts, - const std::string & topic_name, - const rclcpp::QoS & qos, - std::function)> callback); - - // Same as create_serialized_message() as the subscription is to serialized_messages only - std::shared_ptr create_message() override; - - std::shared_ptr create_serialized_message() override; - - void handle_message( - std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override; - - void handle_loaned_message( - void * loaned_message, const rclcpp::MessageInfo & message_info) override; - - // Post-Galactic, handle_serialized_message is a pure virtual function in - // rclcpp::SubscriptionBase, so we must override it. However, in order to - // make this change compatible with both Galactic and later, we leave off - // the 'override' flag. - void - handle_serialized_message( - const std::shared_ptr & serialized_message, - const rclcpp::MessageInfo & message_info); - - // Same as return_serialized_message() as the subscription is to serialized_messages only - void return_message(std::shared_ptr & message) override; - - void return_serialized_message(std::shared_ptr & message) override; - - // Provide a const reference to the QoS Profile used to create this subscription. - const rclcpp::QoS & qos_profile() const; - -private: - // cppcheck-suppress unknownMacro - RCLCPP_DISABLE_COPY(GenericSubscription) - - std::shared_ptr borrow_serialized_message(size_t capacity); - rcutils_allocator_t default_allocator_; - std::function)> callback_; - const rclcpp::QoS qos_; -}; - -} // namespace domain_bridge - -#endif // DOMAIN_BRIDGE__GENERIC_SUBSCRIPTION_HPP_ From c55cd2a92c787bc2d32928a66e260ff6f9a8017c Mon Sep 17 00:00:00 2001 From: Rebecca Butler Date: Thu, 17 Jun 2021 09:57:21 -0400 Subject: [PATCH 2/2] Remove unnecessary add_publisher and add_subscription calls Signed-off-by: Rebecca Butler --- src/domain_bridge/domain_bridge.cpp | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/domain_bridge/domain_bridge.cpp b/src/domain_bridge/domain_bridge.cpp index d3bee19..24ecca1 100644 --- a/src/domain_bridge/domain_bridge.cpp +++ b/src/domain_bridge/domain_bridge.cpp @@ -50,8 +50,9 @@ class DomainBridgeImpl using NodeMap = std::unordered_map>; using TopicBridgeMap = std::map< TopicBridge, - std::pair, - std::shared_ptr>>; + std::pair< + std::shared_ptr, + std::shared_ptr>>; using TypesupportMap = std::unordered_map< std::string, std::shared_ptr>; @@ -113,10 +114,9 @@ class DomainBridgeImpl const std::string & topic_name, const std::string & type, const rclcpp::QoS & qos, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::PublisherOptionsWithAllocator> & options) { - auto publisher = node->create_generic_publisher(topic_name, type, qos); - node->get_node_topics_interface()->add_publisher(publisher, std::move(group)); + auto publisher = node->create_generic_publisher(topic_name, type, qos, options); return publisher; } @@ -126,7 +126,7 @@ class DomainBridgeImpl const std::string & topic_name, const std::string & type, const rclcpp::QoS & qos, - rclcpp::CallbackGroup::SharedPtr group) + rclcpp::SubscriptionOptionsWithAllocator> & options) { auto subscription = node->create_generic_subscription( topic_name, @@ -135,8 +135,8 @@ class DomainBridgeImpl [publisher](std::shared_ptr msg) { // Publish message into the other domain publisher->publish(*msg); - }); - node->get_node_topics_interface()->add_subscription(subscription, std::move(group)); + }, + options); return subscription; } @@ -232,6 +232,12 @@ class DomainBridgeImpl std::cerr << warning << std::endl; } + rclcpp::PublisherOptionsWithAllocator> publisher_options; + rclcpp::SubscriptionOptionsWithAllocator> subscription_options; + + publisher_options.callback_group = topic_options.callback_group(); + subscription_options.callback_group = topic_options.callback_group(); + // Create publisher for the 'to_domain' // The publisher should be created first so it is available to the subscription callback auto publisher = this->create_publisher( @@ -239,7 +245,7 @@ class DomainBridgeImpl topic_remapped, type, qos, - topic_options.callback_group()); + publisher_options); // Create subscription for the 'from_domain' auto subscription = this->create_subscription( @@ -248,7 +254,7 @@ class DomainBridgeImpl topic, type, qos, - topic_options.callback_group()); + subscription_options); this->bridged_topics_[topic_bridge] = {publisher, subscription}; };